Merged from branch 'branches/unordered'
Richard Roberts
commit
175965a6bf
|
|
@ -100,10 +100,15 @@ endif()
|
|||
###############################################################################
|
||||
# Find boost
|
||||
|
||||
# To change the path for boost, you will need to set:
|
||||
# BOOST_ROOT: path to install prefix for boost
|
||||
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
|
||||
|
||||
# If using Boost shared libs, set up auto linking for shared libs
|
||||
if(MSVC AND NOT Boost_USE_STATIC_LIBS)
|
||||
add_definitions(-DBOOST_ALL_DYN_LINK)
|
||||
endif()
|
||||
add_definitions(-DBOOST_ALL_NO_LIB)
|
||||
|
||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
|
||||
|
||||
|
|
@ -115,7 +120,9 @@ endif()
|
|||
|
||||
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
|
||||
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
|
||||
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
|
||||
set(GTSAM_BOOST_LIBRARIES
|
||||
${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
|
||||
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
|
||||
if (GTSAM_DISABLE_NEW_TIMERS)
|
||||
message("WARNING: GTSAM timing instrumentation manually disabled")
|
||||
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
|
||||
|
|
@ -123,7 +130,7 @@ else()
|
|||
if(Boost_TIMER_LIBRARY)
|
||||
list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
|
||||
else()
|
||||
message("WARNING: Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.")
|
||||
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
|
|
@ -156,12 +163,32 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig
|
|||
install(FILES ${CMAKE_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
|
||||
|
||||
|
||||
###############################################################################
|
||||
# Find TBB
|
||||
set(ENV{TBB_ARCH_PLATFORM} "intel64/vc11")
|
||||
set(ENV{TBB_BIN_DIR} "C:/Program Files/Intel/TBB/bin")
|
||||
set(ENV{TBB_TARGET_ARCH} intel64)
|
||||
set(ENV{TBB_TARGET_VS} vc11)
|
||||
find_package(TBB REQUIRED)
|
||||
set(TBB_LIBS "")
|
||||
if(TBB_DEBUG_LIBRARIES)
|
||||
foreach(lib ${TBB_LIBRARIES})
|
||||
list(APPEND TBB_LIBS optimized "${lib}")
|
||||
endforeach()
|
||||
foreach(lib ${TBB_DEBUG_LIBRARIES})
|
||||
list(APPEND TBB_LIBS debug "${lib}")
|
||||
endforeach()
|
||||
else()
|
||||
set(TBB_LIBS ${TBB_LIBRARIES})
|
||||
endif()
|
||||
|
||||
|
||||
###############################################################################
|
||||
# Global compile options
|
||||
|
||||
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
|
||||
# takes precedence over a system-installed one.
|
||||
include_directories(BEFORE ${Boost_INCLUDE_DIR})
|
||||
include_directories(BEFORE ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
|
||||
|
||||
# Add includes for source directories 'BEFORE' boost and any system include
|
||||
# paths so that the compiler uses GTSAM headers in our source directory instead
|
||||
|
|
@ -175,7 +202,9 @@ include_directories(BEFORE
|
|||
|
||||
if(MSVC)
|
||||
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
|
||||
add_definitions(/wd4251 /wd4275 /wd4251) # Disable non-DLL-exported base class warnings
|
||||
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661) # Disable non-DLL-exported base class and other warnings
|
||||
else()
|
||||
list(APPEND CMAKE_CXX_FLAGS "-std=c++11") # FIXME: we should probably avoid these extensions
|
||||
endif()
|
||||
|
||||
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -0,0 +1,481 @@
|
|||
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||
\lyxformat 413
|
||||
\begin_document
|
||||
\begin_header
|
||||
\textclass article
|
||||
\begin_preamble
|
||||
\usepackage{color}
|
||||
|
||||
\definecolor{mygreen}{rgb}{0,0.6,0}
|
||||
\definecolor{mygray}{rgb}{0.5,0.5,0.5}
|
||||
\definecolor{mymauve}{rgb}{0.58,0,0.82}
|
||||
|
||||
\lstset{ %
|
||||
backgroundcolor=\color{white}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor}
|
||||
basicstyle=\footnotesize, % the size of the fonts that are used for the code
|
||||
breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace
|
||||
breaklines=true, % sets automatic line breaking
|
||||
captionpos=b, % sets the caption-position to bottom
|
||||
commentstyle=\color{mygreen}, % comment style
|
||||
% deletekeywords={...}, % if you want to delete keywords from the given language
|
||||
escapeinside={\%*}{*)}, % if you want to add LaTeX within your code
|
||||
extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8
|
||||
frame=single, % adds a frame around the code
|
||||
keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible)
|
||||
keywordstyle=\color{blue}, % keyword style
|
||||
language=C++, % the language of the code
|
||||
morekeywords={*,...}, % if you want to add more keywords to the set
|
||||
numbers=left, % where to put the line-numbers; possible values are (none, left, right)
|
||||
numbersep=5pt, % how far the line-numbers are from the code
|
||||
numberstyle=\tiny\color{mygray}, % the style that is used for the line-numbers
|
||||
rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here))
|
||||
showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces'
|
||||
showstringspaces=false, % underline spaces within strings only
|
||||
showtabs=false, % show tabs within strings adding particular underscores
|
||||
stepnumber=2, % the step between two line-numbers. If it's 1, each line will be numbered
|
||||
stringstyle=\color{mymauve}, % string literal style
|
||||
tabsize=2, % sets default tabsize to 2 spaces
|
||||
title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title
|
||||
}
|
||||
\end_preamble
|
||||
\use_default_options true
|
||||
\maintain_unincluded_children false
|
||||
\language english
|
||||
\language_package default
|
||||
\inputencoding auto
|
||||
\fontencoding global
|
||||
\font_roman lmodern
|
||||
\font_sans lmss
|
||||
\font_typewriter lmtt
|
||||
\font_default_family default
|
||||
\use_non_tex_fonts false
|
||||
\font_sc false
|
||||
\font_osf false
|
||||
\font_sf_scale 100
|
||||
\font_tt_scale 100
|
||||
|
||||
\graphics default
|
||||
\default_output_format default
|
||||
\output_sync 0
|
||||
\bibtex_command default
|
||||
\index_command default
|
||||
\paperfontsize default
|
||||
\spacing single
|
||||
\use_hyperref false
|
||||
\papersize default
|
||||
\use_geometry false
|
||||
\use_amsmath 1
|
||||
\use_esint 1
|
||||
\use_mhchem 1
|
||||
\use_mathdots 1
|
||||
\cite_engine basic
|
||||
\use_bibtopic false
|
||||
\use_indices false
|
||||
\paperorientation portrait
|
||||
\suppress_date false
|
||||
\use_refstyle 1
|
||||
\index Index
|
||||
\shortcut idx
|
||||
\color #008000
|
||||
\end_index
|
||||
\secnumdepth 3
|
||||
\tocdepth 3
|
||||
\paragraph_separation indent
|
||||
\paragraph_indentation default
|
||||
\quotes_language english
|
||||
\papercolumns 1
|
||||
\papersides 1
|
||||
\paperpagestyle default
|
||||
\tracking_changes false
|
||||
\output_changes false
|
||||
\html_math_output 0
|
||||
\html_css_as_file 0
|
||||
\html_be_strict false
|
||||
\end_header
|
||||
|
||||
\begin_body
|
||||
|
||||
\begin_layout Section
|
||||
Template Classes
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
Templated classes are great for writing generic code for multiple types
|
||||
(e.g.
|
||||
the same elimination algorithm code for symbolic, discrete, and Gaussian
|
||||
elimination) without the drawbacks of virtual inheritance (which include
|
||||
rigid class interfaces, downcasting from returned base class pointers,
|
||||
and additional runtime overhead).
|
||||
Depending on how they're used, though, templates can result in very slow
|
||||
compile times, large binary files, and hard-to-use code.
|
||||
This section describes the
|
||||
\begin_inset Quotes eld
|
||||
\end_inset
|
||||
|
||||
best practices
|
||||
\begin_inset Quotes erd
|
||||
\end_inset
|
||||
|
||||
we have developed for gaining the benefits of templates without the drawbacks.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
If you need to write generic code or classes, here are several programming
|
||||
patterns we have found to work very well:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsection
|
||||
The
|
||||
\begin_inset Quotes eld
|
||||
\end_inset
|
||||
|
||||
Templated Base, Specialized Derived
|
||||
\begin_inset Quotes erd
|
||||
\end_inset
|
||||
|
||||
Pattern
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
This pattern is for when you have a generic class containing algorithm or
|
||||
data structure code that will be specialized to several types.
|
||||
The templated base class should never be used directly, instead only the
|
||||
specializations should be used.
|
||||
Some specialized types can be pre-compiled into the library, but the option
|
||||
remains to specialize new types in external libraries or projects.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsubsection
|
||||
Basic Class Structure
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
We'll use
|
||||
\family typewriter
|
||||
FactorGraph
|
||||
\family default
|
||||
as an example.
|
||||
It is templated on the factor type stored in it and has several specializations.
|
||||
The templated base class
|
||||
\family typewriter
|
||||
FactorGraph<class FACTOR>
|
||||
\family default
|
||||
is divided into a header file (
|
||||
\family typewriter
|
||||
.h
|
||||
\family default
|
||||
) and an
|
||||
\begin_inset Quotes eld
|
||||
\end_inset
|
||||
|
||||
instantiation
|
||||
\begin_inset Quotes erd
|
||||
\end_inset
|
||||
|
||||
file (
|
||||
\family typewriter
|
||||
-inst.h
|
||||
\family default
|
||||
).
|
||||
The basic class structure is as follows.
|
||||
\begin_inset listings
|
||||
lstparams "basicstyle={\scriptsize\ttfamily},language={C++}"
|
||||
inline false
|
||||
status open
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
// File FactorGraph.h
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Include a minimal set of headers.
|
||||
Do not include any '-inst.h' files (this is the key to fast compiles).}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
...
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
namespace gtsam {
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/** Class description */
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
template<class FACTOR>
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
class FactorGraph
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
{
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Make 'private' any typedefs that must be redefined in derived
|
||||
classes.
|
||||
E.g.
|
||||
'This' in the context of the derived class should refer to the derived
|
||||
class.
|
||||
These typedefs will be used only by the generic code in this base class.}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
private:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to
|
||||
this
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Make 'public' the typedefs that will be valid in the derived
|
||||
class.}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
public:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
typedef FACTOR FactorType; ///< Factor type stored in this graph
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer
|
||||
to a factor
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
...
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Normally, data is 'protected' so the derived class can access
|
||||
it.}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
protected:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/** Collection of factors */
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
std::vector<sharedFactor> factors_;
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Make 'protected' all constructors, named constructors, or
|
||||
methods returning the base class type.
|
||||
These are not public - the derived class will call them and properly convert
|
||||
returned base classes to the derived class.}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @name Standard Constructors
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @{
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/** Default constructor */
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
FactorGraphUnordered() {}
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/** Named constructor from iterator over factors */
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
template<typename ITERATOR>
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
static This FromIterator(ITERATOR firstFactor, ITERATOR lastFactor);
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @}
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
%*
|
||||
\backslash
|
||||
bfseries{
|
||||
\backslash
|
||||
emph{
|
||||
\backslash
|
||||
color{red}{// Make 'public' standard methods that will be available in the
|
||||
derived class's API.}}}*)
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
public:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @name Adding Factors
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @{
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/** ...
|
||||
*/
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
void reserve(size_t size);
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
...
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
/// @}
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
};
|
||||
\end_layout
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
}
|
||||
\end_layout
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\end_body
|
||||
\end_document
|
||||
|
|
@ -27,7 +27,7 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
// We will use simple integer Keys to refer to the robot poses.
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
// As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements.
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
// We will use simple integer Keys to refer to the robot poses.
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
// Each variable in the system (poses) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
// Here we will use simple integer keys
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <boost/archive/binary_oarchive.hpp>
|
||||
#include <boost/archive/binary_iarchive.hpp>
|
||||
#include <boost/serialization/export.hpp>
|
||||
|
|
@ -489,15 +490,14 @@ void runPerturb()
|
|||
|
||||
// Perturb values
|
||||
VectorValues noise;
|
||||
Ordering ordering = *initial.orderingArbitrary();
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
|
||||
{
|
||||
Vector noisev(key_val.value.dim());
|
||||
for(Vector::Index i = 0; i < noisev.size(); ++i)
|
||||
noisev(i) = normal(rng);
|
||||
noise.insert(ordering[key_val.key], noisev);
|
||||
noise.insert(key_val.key, noisev);
|
||||
}
|
||||
Values perturbed = initial.retract(noise, ordering);
|
||||
Values perturbed = initial.retract(noise);
|
||||
|
||||
// Write results
|
||||
try {
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
|
|
|||
|
|
@ -5,7 +5,8 @@ set (gtsam_subdirs
|
|||
base
|
||||
geometry
|
||||
inference
|
||||
discrete
|
||||
symbolic
|
||||
#discrete
|
||||
linear
|
||||
nonlinear
|
||||
slam
|
||||
|
|
@ -77,13 +78,21 @@ set(gtsam_srcs
|
|||
${base_srcs}
|
||||
${geometry_srcs}
|
||||
${inference_srcs}
|
||||
${discrete_srcs}
|
||||
${symbolic_srcs}
|
||||
#${discrete_srcs}
|
||||
${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
${slam_srcs}
|
||||
${navigation_srcs}
|
||||
${gtsam_core_headers}
|
||||
)
|
||||
|
||||
# Generate and install config and dllexport files
|
||||
configure_file(config.h.in config.h)
|
||||
set(library_name GTSAM) # For substitution in dllexport.h.in
|
||||
configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
|
||||
list(APPEND gtsam_srcs "${CMAKE_BINARY_DIR}/gtsam/config.h" "${CMAKE_BINARY_DIR}/gtsam/dllexport.h")
|
||||
install(FILES "${CMAKE_BINARY_DIR}/gtsam/config.h" "${CMAKE_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam)
|
||||
|
||||
# Versions
|
||||
set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})
|
||||
|
|
@ -95,7 +104,7 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
|
|||
if (GTSAM_BUILD_STATIC_LIBRARY)
|
||||
message(STATUS "Building GTSAM - static")
|
||||
add_library(gtsam-static STATIC ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES})
|
||||
target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES} ${TBB_LIBS})
|
||||
set_target_properties(gtsam-static PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
|
|
@ -114,7 +123,7 @@ endif ()
|
|||
if (GTSAM_BUILD_SHARED_LIBRARY)
|
||||
message(STATUS "Building GTSAM - shared")
|
||||
add_library(gtsam-shared SHARED ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES})
|
||||
target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES} ${TBB_LIBS})
|
||||
set_target_properties(gtsam-shared PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
|
|
@ -142,10 +151,6 @@ if(MSVC)
|
|||
set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
|
||||
APPEND PROPERTY COMPILE_FLAGS "/bigobj")
|
||||
endif()
|
||||
|
||||
# Generate and install config file
|
||||
configure_file(config.h.in config.h)
|
||||
install(FILES ${CMAKE_BINARY_DIR}/gtsam/config.h DESTINATION include/gtsam)
|
||||
|
||||
# Create the matlab toolbox for the gtsam library
|
||||
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||
|
|
|
|||
|
|
@ -58,6 +58,9 @@ public:
|
|||
return std::map<KEY,VALUE>(this->begin(), this->end());
|
||||
}
|
||||
|
||||
/** Handy 'exists' function */
|
||||
bool exists(const KEY& e) const { return this->find(e) != this->end(); }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -91,6 +91,9 @@ public:
|
|||
return std::set<VALUE>(this->begin(), this->end());
|
||||
}
|
||||
|
||||
/** Handy 'exists' function */
|
||||
bool exists(const VALUE& e) const { return this->find(e) != this->end(); }
|
||||
|
||||
/** Print to implement Testable */
|
||||
void print(const std::string& str = "") const { FastSetTestableHelper<VALUE>::print(*this, str); }
|
||||
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/tokenizer.hpp>
|
||||
|
||||
#include <cstdarg>
|
||||
#include <cstring>
|
||||
|
|
@ -587,7 +588,7 @@ void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
|
|||
if (inf_mask) {
|
||||
for (size_t i=0; i<m; ++i) {
|
||||
const double& vi = v(i);
|
||||
if (!isnan(vi) && !isinf(vi))
|
||||
if (std::isfinite(vi))
|
||||
A.row(i) *= vi;
|
||||
}
|
||||
} else {
|
||||
|
|
@ -612,7 +613,7 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
|
|||
if (inf_mask) {
|
||||
for (size_t j=0; j<n; ++j) {
|
||||
const double& vj = v(j);
|
||||
if (!isnan(vj) && !isinf(vj))
|
||||
if (std::isfinite(vj))
|
||||
M.col(j) *= vj;
|
||||
}
|
||||
} else {
|
||||
|
|
@ -768,6 +769,44 @@ Matrix Cayley(const Matrix& A) {
|
|||
// inlined to let Eigen do more optimization
|
||||
return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
|
||||
{
|
||||
stringstream ss;
|
||||
const string firstline = label;
|
||||
ss << firstline;
|
||||
const string padding(firstline.size(), ' ');
|
||||
const bool transposeMatrix = makeVectorHorizontal && matrix.cols() == 1 && matrix.rows() > 1;
|
||||
const DenseIndex effectiveRows = transposeMatrix ? matrix.cols() : matrix.rows();
|
||||
|
||||
if(matrix.rows() > 0 && matrix.cols() > 0)
|
||||
{
|
||||
stringstream matrixPrinted;
|
||||
if(transposeMatrix)
|
||||
matrixPrinted << matrix.transpose();
|
||||
else
|
||||
matrixPrinted << matrix;
|
||||
const std::string matrixStr = matrixPrinted.str();
|
||||
boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));
|
||||
|
||||
DenseIndex row = 0;
|
||||
BOOST_FOREACH(const std::string& line, tok)
|
||||
{
|
||||
assert(row < effectiveRows);
|
||||
if(row > 0)
|
||||
ss << padding;
|
||||
ss << "[ " << line << " ]";
|
||||
if(row < effectiveRows - 1)
|
||||
ss << "\n";
|
||||
++ row;
|
||||
}
|
||||
} else {
|
||||
ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
|
||||
}
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -497,6 +497,8 @@ Eigen::Matrix<double, N, N> Cayley(const Eigen::Matrix<double, N, N>& A) {
|
|||
return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
|
||||
}
|
||||
|
||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,50 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymmetricBlockMatrix.cpp
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/base/SymmetricBlockMatrix.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other)
|
||||
{
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other)
|
||||
{
|
||||
SymmetricBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.cols(), other.cols());
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,229 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymmetricBlockMatrix.h
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VerticalBlockMatrix;
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a collection of blocks. When
|
||||
* constructed, the caller must provide the dimensions of the blocks.
|
||||
*
|
||||
* The block structure is symmetric, but the underlying matrix does not necessarily need to be.
|
||||
*
|
||||
* This class also has a parameter that can be changed after construction to change the apparent
|
||||
* matrix view. firstBlock() determines the block that appears to have index 0 for all operations
|
||||
* (except re-setting firstBlock()).
|
||||
*
|
||||
* @addtogroup base */
|
||||
class SymmetricBlockMatrix
|
||||
{
|
||||
public:
|
||||
typedef SymmetricBlockMatrix This;
|
||||
typedef Eigen::Block<Matrix> Block;
|
||||
typedef Eigen::Block<const Matrix> constBlock;
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
std::vector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
|
||||
|
||||
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
|
||||
public:
|
||||
/** Construct from an empty matrix (asserts that the matrix is empty) */
|
||||
SymmetricBlockMatrix() :
|
||||
blockStart_(0)
|
||||
{
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each block. */
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions) :
|
||||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from iterator over the sizes of each vertical block. */
|
||||
template<typename ITERATOR>
|
||||
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
|
||||
blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
|
||||
template<typename CONTAINER>
|
||||
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
matrix_(matrix), blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if(matrix_.rows() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
|
||||
if(variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
* modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
* SymmetricBlockMatrix, blockStart() will be 0. */
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
|
||||
|
||||
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
* modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
* SymmetricBlockMatrix, blockStart() will be 0. */
|
||||
static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
|
||||
|
||||
/** Row size */
|
||||
DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
/** Column size */
|
||||
DenseIndex cols() const { return rows(); }
|
||||
|
||||
|
||||
/** Block count */
|
||||
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
Block operator()(DenseIndex i_block, DenseIndex j_block) {
|
||||
return range(i_block, i_block+1, j_block, j_block+1);
|
||||
}
|
||||
|
||||
constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
|
||||
return range(i_block, i_block+1, j_block, j_block+1);
|
||||
}
|
||||
|
||||
Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) {
|
||||
assertInvariants();
|
||||
DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
checkBlock(i_actualStartBlock);
|
||||
checkBlock(j_actualStartBlock);
|
||||
if(i_startBlock != 0 || i_endBlock != 0) {
|
||||
checkBlock(i_actualStartBlock);
|
||||
assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
if(j_startBlock != 0 || j_endBlock != 0) {
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
return matrix_.block(
|
||||
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
|
||||
}
|
||||
|
||||
constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const {
|
||||
assertInvariants();
|
||||
DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
|
||||
DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
|
||||
DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
|
||||
DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
|
||||
if(i_startBlock != 0 || i_endBlock != 0) {
|
||||
checkBlock(i_actualStartBlock);
|
||||
assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
if(j_startBlock != 0 || j_endBlock != 0) {
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
return matrix_.block(
|
||||
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
|
||||
Block full() {
|
||||
return range(0,nBlocks(), 0,nBlocks());
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by firstBlock(). */
|
||||
constBlock full() const {
|
||||
return range(0,nBlocks(), 0,nBlocks());
|
||||
}
|
||||
|
||||
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
|
||||
const Matrix& matrix() const { return matrix_; }
|
||||
|
||||
/** Access to full matrix, including any portions excluded by firstBlock() to other operations. */
|
||||
Matrix& matrix() { return matrix_; }
|
||||
|
||||
DenseIndex offset(DenseIndex block) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
DenseIndex& blockStart() { return blockStart_; }
|
||||
DenseIndex blockStart() const { return blockStart_; }
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < (DenseIndex)variableColOffsets_.size()-1);
|
||||
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
friend class VerticalBlockMatrix;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
|
||||
ar & BOOST_SERIALIZATION_NVP(blockStart_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -231,8 +231,6 @@ bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
|
|||
template<class V>
|
||||
bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-9) {
|
||||
bool match = true;
|
||||
if (expected.size() != actual.size())
|
||||
match = false;
|
||||
typename V::const_iterator
|
||||
itExp = expected.begin(),
|
||||
itAct = actual.begin();
|
||||
|
|
@ -243,6 +241,8 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-
|
|||
break;
|
||||
}
|
||||
}
|
||||
if(itExp != expected.end() || itAct != actual.end())
|
||||
match = false;
|
||||
}
|
||||
if(!match) {
|
||||
std::cout << "expected: " << std::endl;
|
||||
|
|
|
|||
|
|
@ -165,7 +165,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
|
|||
if (vec1.size()!=vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[i]) > tol)
|
||||
return false;
|
||||
|
|
@ -178,7 +178,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol
|
|||
if (vec1.size()!=vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[i]) > tol)
|
||||
return false;
|
||||
|
|
@ -387,7 +387,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
|
|||
// Slow version with error checking
|
||||
pair<Vector, double>
|
||||
weightedPseudoinverse(const Vector& a, const Vector& weights) {
|
||||
int m = weights.size();
|
||||
DenseIndex m = weights.size();
|
||||
if (a.size() != m)
|
||||
throw invalid_argument("a and weights have different sizes!");
|
||||
Vector pseudo(m);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,52 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VerticalBlockMatrix.cpp
|
||||
* @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and
|
||||
* GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010 */
|
||||
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
#include <gtsam/base/SymmetricBlockMatrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other)
|
||||
{
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(other.rows(), result.variableColOffsets_.back());
|
||||
result.rowEnd_ = other.rows();
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other, DenseIndex height)
|
||||
{
|
||||
VerticalBlockMatrix result;
|
||||
result.variableColOffsets_.resize(other.nBlocks() + 1);
|
||||
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
|
||||
result.variableColOffsets_[i] =
|
||||
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
|
||||
result.matrix_.resize(height, result.variableColOffsets_.back());
|
||||
result.rowEnd_ = height;
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,229 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VerticalBlockMatrix.h
|
||||
* @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and
|
||||
* GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010 */
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class SymmetricBlockMatrix;
|
||||
|
||||
/**
|
||||
* This class stores a dense matrix and allows it to be accessed as a collection of vertical
|
||||
* blocks. The dimensions of the blocks are provided when constructing this class.
|
||||
*
|
||||
* This class also has three parameters that can be changed after construction that change the
|
||||
* apparent view of the matrix without any reallocation or data copying. firstBlock() determines
|
||||
* the block that has index 0 for all operations (except for re-setting firstBlock()). rowStart()
|
||||
* determines the apparent first row of the matrix for all operations (except for setting
|
||||
* rowStart() and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last
|
||||
* 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 */
|
||||
class GTSAM_EXPORT VerticalBlockMatrix
|
||||
{
|
||||
public:
|
||||
typedef VerticalBlockMatrix This;
|
||||
typedef Eigen::Block<Matrix> Block;
|
||||
typedef Eigen::Block<const Matrix> constBlock;
|
||||
|
||||
protected:
|
||||
Matrix matrix_; ///< The full matrix
|
||||
std::vector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
|
||||
|
||||
DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment.
|
||||
DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
|
||||
|
||||
public:
|
||||
|
||||
/** Construct an empty VerticalBlockMatrix */
|
||||
VerticalBlockMatrix() :
|
||||
rowStart_(0), rowEnd_(0), blockStart_(0)
|
||||
{
|
||||
variableColOffsets_.push_back(0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
|
||||
{
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
if(variableColOffsets_.back() != matrix_.cols())
|
||||
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from iterator over the sizes of each vertical block. */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(height, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
|
||||
* If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
|
||||
* the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
|
||||
* rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
|
||||
* underlying matrix will be the size of the view of the source matrix. */
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
|
||||
|
||||
/** Copy the block structure, but do not copy the matrix data. If blockStart() has been
|
||||
* modified, this copies the structure of the corresponding matrix view. In the destination
|
||||
* VerticalBlockMatrix, blockStart() will be 0. */
|
||||
static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
|
||||
|
||||
/// Row size
|
||||
DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
|
||||
|
||||
/// Column size
|
||||
DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
/// Block count
|
||||
DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
/** Access a single block in the underlying matrix with read/write access */
|
||||
Block operator()(DenseIndex block) { return range(block, block+1); }
|
||||
|
||||
/** Access a const block view */
|
||||
const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
|
||||
|
||||
/** access ranges of blocks at a time */
|
||||
Block range(DenseIndex startBlock, DenseIndex endBlock) {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
|
||||
}
|
||||
|
||||
const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualStartBlock = startBlock + blockStart_;
|
||||
DenseIndex actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 || endBlock != 0) {
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
|
||||
}
|
||||
const DenseIndex startCol = variableColOffsets_[actualStartBlock];
|
||||
const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
|
||||
return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
Block full() { return range(0, nBlocks()); }
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
const constBlock full() const { return range(0, nBlocks()); }
|
||||
|
||||
DenseIndex offset(DenseIndex block) const {
|
||||
assertInvariants();
|
||||
DenseIndex actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/** Get or set the apparent first row of the underlying matrix for all operations */
|
||||
DenseIndex& rowStart() { return rowStart_; }
|
||||
|
||||
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
DenseIndex& rowEnd() { return rowEnd_; }
|
||||
|
||||
/** Get or set the apparent first block for all operations */
|
||||
DenseIndex& firstBlock() { return blockStart_; }
|
||||
|
||||
/** Get the apparent first row of the underlying matrix for all operations */
|
||||
DenseIndex rowStart() const { return rowStart_; }
|
||||
|
||||
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
DenseIndex rowEnd() const { return rowEnd_; }
|
||||
|
||||
/** Get the apparent first block for all operations */
|
||||
DenseIndex firstBlock() const { return blockStart_; }
|
||||
|
||||
/** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
|
||||
const Matrix& matrix() const { return matrix_; }
|
||||
|
||||
/** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
|
||||
Matrix& matrix() { return matrix_; }
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
assert(rowStart_ <= matrix_.rows());
|
||||
assert(rowEnd_ <= matrix_.rows());
|
||||
assert(rowStart_ <= rowEnd_);
|
||||
}
|
||||
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < (DenseIndex)variableColOffsets_.size() - 1);
|
||||
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
DenseIndex j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
friend class SymmetricBlockMatrix;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
|
||||
ar & BOOST_SERIALIZATION_NVP(rowStart_);
|
||||
ar & BOOST_SERIALIZATION_NVP(rowEnd_);
|
||||
ar & BOOST_SERIALIZATION_NVP(blockStart_);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,627 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 blockMatrices.h
|
||||
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
|
||||
* @author Richard Roberts
|
||||
* @date Sep 18, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class MATRIX> class SymmetricBlockView;
|
||||
|
||||
/**
|
||||
* This class stores a *reference* to a matrix and allows it to be accessed as
|
||||
* a collection of vertical blocks. It also provides for accessing individual
|
||||
* columns from those blocks. When constructed or resized, the caller must
|
||||
* provide the dimensions of the blocks, as well as an underlying matrix
|
||||
* storage object. This class will resize the underlying matrix such that it
|
||||
* is consistent with the given block dimensions.
|
||||
*
|
||||
* This class also has three parameters that can be changed after construction
|
||||
* that change the apparent view of the matrix. firstBlock() determines the
|
||||
* block that has index 0 for all operations (except for re-setting
|
||||
* firstBlock()). rowStart() determines the apparent first row of the matrix
|
||||
* for all operations (except for setting rowStart() and rowEnd()). rowEnd()
|
||||
* determines the apparent *exclusive* last 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
|
||||
*/
|
||||
template<class MATRIX>
|
||||
class VerticalBlockView {
|
||||
public:
|
||||
typedef MATRIX FullMatrix;
|
||||
typedef Eigen::Block<MATRIX> Block;
|
||||
typedef Eigen::Block<const MATRIX> constBlock;
|
||||
|
||||
// columns of blocks
|
||||
typedef Eigen::VectorBlock<typename MATRIX::ColXpr> Column;
|
||||
typedef Eigen::VectorBlock<const typename MATRIX::ConstColXpr> constColumn;
|
||||
|
||||
protected:
|
||||
FullMatrix& matrix_; // the reference to the full matrix
|
||||
std::vector<size_t> variableColOffsets_; // the starting columns of each block (0-based)
|
||||
|
||||
// Changes apparent matrix view, see main class comment.
|
||||
size_t rowStart_; // 0 initially
|
||||
size_t rowEnd_; // the number of row - 1, initially
|
||||
size_t blockStart_; // 0 initially
|
||||
|
||||
public:
|
||||
/** Construct from an empty matrix (asserts that the matrix is empty) */
|
||||
VerticalBlockView(FullMatrix& matrix) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) {
|
||||
fillOffsets((size_t*)0, (size_t*)0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from a non-empty matrix and copy the block structure from
|
||||
* another block view.
|
||||
*/
|
||||
template<class RHS>
|
||||
VerticalBlockView(FullMatrix& matrix, const RHS& rhs) :
|
||||
matrix_(matrix) {
|
||||
if((size_t) matrix_.rows() != rhs.rows() || (size_t) matrix_.cols() != rhs.cols())
|
||||
throw std::invalid_argument(
|
||||
"In VerticalBlockView<>(FullMatrix& matrix, const RHS& rhs), matrix and rhs must\n"
|
||||
"already be of the same size. If not, construct the VerticalBlockView from an\n"
|
||||
"empty matrix and then use copyStructureFrom(const RHS& rhs) to resize the matrix\n"
|
||||
"and set up the block structure.");
|
||||
copyStructureFrom(rhs);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from iterators over the sizes of each vertical block */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.rows()), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from a vector of the sizes of each vertical block, resize the
|
||||
* matrix so that its height is matrixNewHeight and its width fits the given
|
||||
* block dimensions.
|
||||
*/
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(matrixNewHeight, variableColOffsets_.back());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Row size
|
||||
*/
|
||||
size_t rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
|
||||
|
||||
/** Column size
|
||||
*/
|
||||
size_t cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
|
||||
/** Block count
|
||||
*/
|
||||
size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
|
||||
/** Access a single block in the underlying matrix with read/write access */
|
||||
inline Block operator()(size_t block) {
|
||||
return range(block, block+1);
|
||||
}
|
||||
|
||||
/** Access a const block view */
|
||||
inline const constBlock operator()(size_t block) const {
|
||||
return range(block, block+1);
|
||||
}
|
||||
|
||||
/** access ranges of blocks at a time */
|
||||
inline Block range(size_t startBlock, size_t endBlock) {
|
||||
assertInvariants();
|
||||
size_t actualStartBlock = startBlock + blockStart_;
|
||||
size_t actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 && endBlock != 0)
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < variableColOffsets_.size());
|
||||
const size_t& startCol = variableColOffsets_[actualStartBlock];
|
||||
const size_t& endCol = variableColOffsets_[actualEndBlock];
|
||||
return matrix_.block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol);
|
||||
}
|
||||
|
||||
inline const constBlock range(size_t startBlock, size_t endBlock) const {
|
||||
assertInvariants();
|
||||
size_t actualStartBlock = startBlock + blockStart_;
|
||||
size_t actualEndBlock = endBlock + blockStart_;
|
||||
if(startBlock != 0 && endBlock != 0)
|
||||
checkBlock(actualStartBlock);
|
||||
assert(actualEndBlock < variableColOffsets_.size());
|
||||
const size_t& startCol = variableColOffsets_[actualStartBlock];
|
||||
const size_t& endCol = variableColOffsets_[actualEndBlock];
|
||||
return ((const FullMatrix&)matrix_).block(rowStart_, startCol, rowEnd_-rowStart_, endCol-startCol);
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
inline Block full() {
|
||||
return range(0,nBlocks());
|
||||
}
|
||||
|
||||
/** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
|
||||
inline const constBlock full() const {
|
||||
return range(0,nBlocks());
|
||||
}
|
||||
|
||||
/** get a single column out of a block */
|
||||
Column column(size_t block, size_t columnOffset) {
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
assert(variableColOffsets_[actualBlock] + columnOffset < variableColOffsets_[actualBlock+1]);
|
||||
return matrix_.col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_);
|
||||
}
|
||||
|
||||
/** get a single column out of a block */
|
||||
const constColumn column(size_t block, size_t columnOffset) const {
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
assert(variableColOffsets_[actualBlock] + columnOffset < (size_t) matrix_.cols());
|
||||
return ((const FullMatrix&)matrix_).col(variableColOffsets_[actualBlock] + columnOffset).segment(rowStart_, rowEnd_-rowStart_);
|
||||
}
|
||||
|
||||
size_t offset(size_t block) const {
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
/** Get or set the apparent first row of the underlying matrix for all operations */
|
||||
size_t& rowStart() { return rowStart_; }
|
||||
|
||||
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
size_t& rowEnd() { return rowEnd_; }
|
||||
|
||||
/** Get or set the apparent first block for all operations */
|
||||
size_t& firstBlock() { return blockStart_; }
|
||||
|
||||
/** Get the apparent first row of the underlying matrix for all operations */
|
||||
size_t rowStart() const { return rowStart_; }
|
||||
|
||||
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
|
||||
size_t rowEnd() const { return rowEnd_; }
|
||||
|
||||
/** Get the apparent first block for all operations */
|
||||
size_t firstBlock() const { return blockStart_; }
|
||||
|
||||
/** access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
|
||||
const FullMatrix& fullMatrix() const { return matrix_; }
|
||||
|
||||
/**
|
||||
* Copy the block structure and resize the underlying matrix, but do not
|
||||
* copy the matrix data. If blockStart(), rowStart(), and/or rowEnd() have
|
||||
* been modified, this copies the structure of the corresponding matrix view.
|
||||
* In the destination VerticalBlockView, blockStart() and rowStart() will
|
||||
* thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and
|
||||
* the underlying matrix will be the size of the view of the source matrix.
|
||||
*/
|
||||
template<class RHS>
|
||||
void copyStructureFrom(const RHS& rhs) {
|
||||
if((size_t) matrix_.rows() != (size_t) rhs.rows() || (size_t) matrix_.cols() != (size_t) rhs.cols())
|
||||
matrix_.resize(rhs.rows(), rhs.cols());
|
||||
if(rhs.blockStart_ == 0)
|
||||
variableColOffsets_ = rhs.variableColOffsets_;
|
||||
else {
|
||||
variableColOffsets_.resize(rhs.nBlocks() + 1);
|
||||
variableColOffsets_[0] = 0;
|
||||
size_t j=0;
|
||||
assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1);
|
||||
for(std::vector<size_t>::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off);
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
rowStart_ = 0;
|
||||
rowEnd_ = matrix_.rows();
|
||||
blockStart_ = 0;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Copy the block struture and matrix data, resizing the underlying matrix
|
||||
* in the process. This can deal with assigning between different types of
|
||||
* underlying matrices, as long as the matrices themselves are assignable.
|
||||
* To avoid creating a temporary matrix this assumes no aliasing, i.e. that
|
||||
* no part of the underlying matrices refer to the same memory!
|
||||
*
|
||||
* If blockStart(), rowStart(), and/or rowEnd() have been modified, this
|
||||
* copies the structure of the corresponding matrix view. In the destination
|
||||
* VerticalBlockView, blockStart() and rowStart() will thus be 0, rowEnd()
|
||||
* will be cols() of the source VerticalBlockView, and the underlying matrix
|
||||
* will be the size of the view of the source matrix.
|
||||
*/
|
||||
template<class RHS>
|
||||
VerticalBlockView<MATRIX>& assignNoalias(const RHS& rhs) {
|
||||
copyStructureFrom(rhs);
|
||||
matrix_.noalias() = rhs.full();
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Swap the contents of the underlying matrix and the block information with
|
||||
* another VerticalBlockView.
|
||||
*/
|
||||
void swap(VerticalBlockView<MATRIX>& other) {
|
||||
matrix_.swap(other.matrix_);
|
||||
variableColOffsets_.swap(other.variableColOffsets_);
|
||||
std::swap(rowStart_, other.rowStart_);
|
||||
std::swap(rowEnd_, other.rowEnd_);
|
||||
std::swap(blockStart_, other.blockStart_);
|
||||
assertInvariants();
|
||||
other.assertInvariants();
|
||||
}
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert((size_t) matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < variableColOffsets_.size());
|
||||
assert(rowStart_ <= (size_t) matrix_.rows());
|
||||
assert(rowEnd_ <= (size_t) matrix_.rows());
|
||||
assert(rowStart_ <= rowEnd_);
|
||||
}
|
||||
|
||||
void checkBlock(size_t block) const {
|
||||
assert((size_t) matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < variableColOffsets_.size()-1);
|
||||
assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
size_t j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
template<class OTHER> friend class SymmetricBlockView;
|
||||
template<class RELATED> friend class VerticalBlockView;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
|
||||
ar & BOOST_SERIALIZATION_NVP(rowStart_);
|
||||
ar & BOOST_SERIALIZATION_NVP(rowEnd_);
|
||||
ar & BOOST_SERIALIZATION_NVP(blockStart_);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* This class stores a *reference* to a matrix and allows it to be accessed as
|
||||
* a collection of blocks. It also provides for accessing individual
|
||||
* columns from those blocks. When constructed or resized, the caller must
|
||||
* provide the dimensions of the blocks, as well as an underlying matrix
|
||||
* storage object. This class will resize the underlying matrix such that it
|
||||
* is consistent with the given block dimensions.
|
||||
*
|
||||
* This class uses a symmetric block structure. The underlying matrix does not
|
||||
* necessarily need to be symmetric.
|
||||
*
|
||||
* This class also has a parameter that can be changed after construction to
|
||||
* change the apparent matrix view. firstBlock() determines the block that
|
||||
* appears to have index 0 for all operations (except re-setting firstBlock()).
|
||||
*
|
||||
* @addtogroup base
|
||||
*/
|
||||
template<class MATRIX>
|
||||
class SymmetricBlockView {
|
||||
public:
|
||||
typedef MATRIX FullMatrix;
|
||||
typedef Eigen::Block<MATRIX> Block;
|
||||
typedef Eigen::Block<const MATRIX> constBlock;
|
||||
typedef typename FullMatrix::ColXpr::SegmentReturnType Column;
|
||||
typedef typename FullMatrix::ConstColXpr::ConstSegmentReturnType constColumn;
|
||||
|
||||
private:
|
||||
static FullMatrix matrixTemp_; // just for finding types
|
||||
|
||||
protected:
|
||||
FullMatrix& matrix_; // the reference to the full matrix
|
||||
std::vector<size_t> variableColOffsets_; // the starting columns of each block (0-based)
|
||||
|
||||
// Changes apparent matrix view, see main class comment.
|
||||
size_t blockStart_; // 0 initially
|
||||
|
||||
public:
|
||||
/** Construct from an empty matrix (asserts that the matrix is empty) */
|
||||
SymmetricBlockView(FullMatrix& matrix) :
|
||||
matrix_(matrix), blockStart_(0) {
|
||||
fillOffsets((size_t*)0, (size_t*)0);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from iterators over the sizes of each block */
|
||||
template<typename ITERATOR>
|
||||
SymmetricBlockView(FullMatrix& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
|
||||
matrix_(matrix), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/**
|
||||
* Modify the size and structure of the underlying matrix and this block
|
||||
* view. If 'preserve' is true, the underlying matrix data will be copied if
|
||||
* the matrix size changes, otherwise the new data will be uninitialized.
|
||||
*/
|
||||
template<typename ITERATOR>
|
||||
void resize(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool preserve) {
|
||||
blockStart_ = 0;
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
if (preserve)
|
||||
matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
else
|
||||
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
|
||||
}
|
||||
|
||||
/** Row size
|
||||
*/
|
||||
size_t rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
|
||||
/** Column size
|
||||
*/
|
||||
size_t cols() const { return rows(); }
|
||||
|
||||
|
||||
/** Block count
|
||||
*/
|
||||
size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
|
||||
Block operator()(size_t i_block, size_t j_block) {
|
||||
return range(i_block, i_block+1, j_block, j_block+1);
|
||||
}
|
||||
|
||||
constBlock operator()(size_t i_block, size_t j_block) const {
|
||||
return range(i_block, i_block+1, j_block, j_block+1);
|
||||
}
|
||||
|
||||
Block range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) {
|
||||
assertInvariants();
|
||||
size_t i_actualStartBlock = i_startBlock + blockStart_;
|
||||
size_t i_actualEndBlock = i_endBlock + blockStart_;
|
||||
size_t j_actualStartBlock = j_startBlock + blockStart_;
|
||||
size_t j_actualEndBlock = j_endBlock + blockStart_;
|
||||
checkBlock(i_actualStartBlock);
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(i_actualEndBlock < variableColOffsets_.size());
|
||||
assert(j_actualEndBlock < variableColOffsets_.size());
|
||||
return matrix_.block(
|
||||
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]);
|
||||
}
|
||||
|
||||
constBlock range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) const {
|
||||
assertInvariants();
|
||||
size_t i_actualStartBlock = i_startBlock + blockStart_;
|
||||
size_t i_actualEndBlock = i_endBlock + blockStart_;
|
||||
size_t j_actualStartBlock = j_startBlock + blockStart_;
|
||||
size_t j_actualEndBlock = j_endBlock + blockStart_;
|
||||
checkBlock(i_actualStartBlock);
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(i_actualEndBlock < variableColOffsets_.size());
|
||||
assert(j_actualEndBlock < variableColOffsets_.size());
|
||||
return ((const FullMatrix&)matrix_).block(
|
||||
variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]);
|
||||
}
|
||||
|
||||
Block full() {
|
||||
return range(0,nBlocks(), 0,nBlocks());
|
||||
}
|
||||
|
||||
constBlock full() const {
|
||||
return range(0,nBlocks(), 0,nBlocks());
|
||||
}
|
||||
|
||||
/** access to full matrix */
|
||||
const FullMatrix& fullMatrix() const { return matrix_; }
|
||||
|
||||
Column column(size_t i_block, size_t j_block, size_t columnOffset) {
|
||||
assertInvariants();
|
||||
size_t i_actualBlock = i_block + blockStart_;
|
||||
size_t j_actualBlock = j_block + blockStart_;
|
||||
checkBlock(i_actualBlock);
|
||||
checkBlock(j_actualBlock);
|
||||
assert(i_actualBlock < variableColOffsets_.size());
|
||||
assert(j_actualBlock < variableColOffsets_.size());
|
||||
assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
|
||||
|
||||
return matrix_.col(
|
||||
variableColOffsets_[j_actualBlock] + columnOffset).segment(
|
||||
variableColOffsets_[i_actualBlock],
|
||||
variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]);
|
||||
}
|
||||
|
||||
constColumn column(size_t i_block, size_t j_block, size_t columnOffset) const {
|
||||
assertInvariants();
|
||||
size_t i_actualBlock = i_block + blockStart_;
|
||||
size_t j_actualBlock = j_block + blockStart_;
|
||||
checkBlock(i_actualBlock);
|
||||
checkBlock(j_actualBlock);
|
||||
assert(i_actualBlock < variableColOffsets_.size());
|
||||
assert(j_actualBlock < variableColOffsets_.size());
|
||||
assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
|
||||
|
||||
return ((const FullMatrix&)matrix_).col(
|
||||
variableColOffsets_[j_actualBlock] + columnOffset).segment(
|
||||
variableColOffsets_[i_actualBlock],
|
||||
variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]);
|
||||
// assertInvariants();
|
||||
// size_t j_actualBlock = j_block + blockStart_;
|
||||
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
|
||||
// constBlock blockMat(operator()(i_block, j_block));
|
||||
// return constColumn(blockMat, columnOffset);
|
||||
}
|
||||
|
||||
Column rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) {
|
||||
assertInvariants();
|
||||
|
||||
size_t i_actualStartBlock = i_startBlock + blockStart_;
|
||||
size_t i_actualEndBlock = i_endBlock + blockStart_;
|
||||
size_t j_actualStartBlock = j_block + blockStart_;
|
||||
checkBlock(i_actualStartBlock);
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(i_actualEndBlock < variableColOffsets_.size());
|
||||
assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]);
|
||||
|
||||
return matrix_.col(
|
||||
variableColOffsets_[j_actualStartBlock] + columnOffset).segment(
|
||||
variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]);
|
||||
}
|
||||
|
||||
constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const {
|
||||
assertInvariants();
|
||||
|
||||
size_t i_actualStartBlock = i_startBlock + blockStart_;
|
||||
size_t i_actualEndBlock = i_endBlock + blockStart_;
|
||||
size_t j_actualStartBlock = j_block + blockStart_;
|
||||
checkBlock(i_actualStartBlock);
|
||||
checkBlock(j_actualStartBlock);
|
||||
assert(i_actualEndBlock < variableColOffsets_.size());
|
||||
assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]);
|
||||
|
||||
return ((const FullMatrix&)matrix_).col(
|
||||
variableColOffsets_[j_actualStartBlock] + columnOffset).segment(
|
||||
variableColOffsets_[i_actualStartBlock],
|
||||
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]);
|
||||
}
|
||||
|
||||
size_t offset(size_t block) const {
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock];
|
||||
}
|
||||
|
||||
size_t& blockStart() { return blockStart_; }
|
||||
size_t blockStart() const { return blockStart_; }
|
||||
|
||||
/** Copy the block structure and resize the underlying matrix, but do not
|
||||
* copy the matrix data. If blockStart() has been modified, this copies the
|
||||
* structure of the corresponding matrix view. In the destination
|
||||
* SymmetricBlockView, startBlock() will thus be 0 and the underlying matrix
|
||||
* will be the size of the view of the source matrix.
|
||||
*/
|
||||
template<class RHS>
|
||||
void copyStructureFrom(const RHS& rhs) {
|
||||
matrix_.resize(rhs.cols(), rhs.cols());
|
||||
if(rhs.blockStart_ == 0)
|
||||
variableColOffsets_ = rhs.variableColOffsets_;
|
||||
else {
|
||||
variableColOffsets_.resize(rhs.nBlocks() + 1);
|
||||
variableColOffsets_[0] = 0;
|
||||
size_t j=0;
|
||||
assert(rhs.variableColOffsets_.begin()+rhs.blockStart_ < rhs.variableColOffsets_.end()-1);
|
||||
for(std::vector<size_t>::const_iterator off=rhs.variableColOffsets_.begin()+rhs.blockStart_; off!=rhs.variableColOffsets_.end()-1; ++off) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + (*(off+1) - *off);
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
blockStart_ = 0;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Copy the block struture and matrix data, resizing the underlying matrix
|
||||
* in the process. This can deal with assigning between different types of
|
||||
* underlying matrices, as long as the matrices themselves are assignable.
|
||||
* To avoid creating a temporary matrix this assumes no aliasing, i.e. that
|
||||
* no part of the underlying matrices refer to the same memory!
|
||||
*
|
||||
* If blockStart() has been modified, this copies the structure of the
|
||||
* corresponding matrix view. In the destination SymmetricBlockView,
|
||||
* startBlock() will thus be 0 and the underlying matrix will be the size
|
||||
* of the view of the source matrix.
|
||||
*/
|
||||
template<class RHSMATRIX>
|
||||
SymmetricBlockView<MATRIX>& assignNoalias(const SymmetricBlockView<RHSMATRIX>& rhs) {
|
||||
copyStructureFrom(rhs);
|
||||
matrix_.noalias() = rhs.full();
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Swap the contents of the underlying matrix and the block information with
|
||||
* another VerticalBlockView.
|
||||
*/
|
||||
void swap(SymmetricBlockView<MATRIX>& other) {
|
||||
matrix_.swap(other.matrix_);
|
||||
variableColOffsets_.swap(other.variableColOffsets_);
|
||||
std::swap(blockStart_, other.blockStart_);
|
||||
assertInvariants();
|
||||
other.assertInvariants();
|
||||
}
|
||||
|
||||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert((size_t) matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < variableColOffsets_.size());
|
||||
}
|
||||
|
||||
void checkBlock(size_t block) const {
|
||||
assert(matrix_.rows() == matrix_.cols());
|
||||
assert((size_t) matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < variableColOffsets_.size()-1);
|
||||
assert(variableColOffsets_[block] < (size_t) matrix_.cols() && variableColOffsets_[block+1] <= (size_t) matrix_.cols());
|
||||
}
|
||||
|
||||
template<typename ITERATOR>
|
||||
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
|
||||
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
|
||||
variableColOffsets_[0] = 0;
|
||||
size_t j=0;
|
||||
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
|
||||
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
|
||||
++ j;
|
||||
}
|
||||
}
|
||||
|
||||
template<class RELATED> friend class SymmetricBlockView;
|
||||
template<class OTHER> friend class VerticalBlockView;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
|
||||
ar & BOOST_SERIALIZATION_NVP(blockStart_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -1,133 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testBlockMatrices
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/blockMatrices.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testBlockMatrices, jacobian_factor1) {
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
AbMatrix matrix; // actual matrix - empty to start with
|
||||
|
||||
// from JacobianFactor::Constructor - one variable
|
||||
Matrix A1 = Matrix_(2,3,
|
||||
1., 2., 3.,
|
||||
4., 5., 6.);
|
||||
Vector b = Vector_(2, 7., 8.);
|
||||
size_t dims[] = { A1.cols(), 1};
|
||||
|
||||
// build the structure
|
||||
BlockAb Ab(matrix, dims, dims+2, b.size());
|
||||
|
||||
// add a matrix and get back out
|
||||
Ab(0) = A1;
|
||||
EXPECT(assert_equal(A1, Ab(0)));
|
||||
|
||||
// add vector to the system
|
||||
Ab.column(1, 0) = b;
|
||||
EXPECT(assert_equal(A1, Ab(0)));
|
||||
EXPECT(assert_equal(b, Ab.column(1,0)));
|
||||
|
||||
// examine matrix contents
|
||||
EXPECT_LONGS_EQUAL(2, Ab.nBlocks());
|
||||
Matrix expFull = Matrix_(2, 4,
|
||||
1., 2., 3., 7.,
|
||||
4., 5., 6., 8.);
|
||||
Matrix actFull = Ab.full();
|
||||
EXPECT(assert_equal(expFull, actFull));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testBlockMatrices, jacobian_factor2) {
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
AbMatrix matrix; // actual matrix - empty to start with
|
||||
|
||||
// from JacobianFactor::Constructor - two variables
|
||||
Matrix A1 = Matrix_(2,3,
|
||||
1., 2., 3.,
|
||||
4., 5., 6.);
|
||||
Matrix A2 = Matrix_(2,1,
|
||||
10.,
|
||||
11.);
|
||||
Vector b = Vector_(2, 7., 8.);
|
||||
size_t dims[] = { A1.cols(), A2.cols(), 1};
|
||||
|
||||
// build the structure
|
||||
BlockAb Ab(matrix, dims, dims+3, b.size());
|
||||
|
||||
// add blocks
|
||||
Ab(0) = A1;
|
||||
Ab(1) = A2;
|
||||
EXPECT(assert_equal(A1, Ab(0)));
|
||||
EXPECT(assert_equal(A2, Ab(1)));
|
||||
|
||||
// add vector to the system
|
||||
Ab.column(2, 0) = b;
|
||||
EXPECT(assert_equal(A1, Ab(0)));
|
||||
EXPECT(assert_equal(A2, Ab(1)));
|
||||
EXPECT(assert_equal(b, Ab.column(2,0)));
|
||||
|
||||
// examine matrix contents
|
||||
EXPECT_LONGS_EQUAL(3, Ab.nBlocks());
|
||||
Matrix expFull = Matrix_(2, 5,
|
||||
1., 2., 3., 10., 7.,
|
||||
4., 5., 6., 11., 8.);
|
||||
Matrix actFull = Ab.full();
|
||||
EXPECT(assert_equal(expFull, actFull));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testBlockMatrices, hessian_factor1) {
|
||||
typedef Matrix InfoMatrix;
|
||||
typedef SymmetricBlockView<InfoMatrix> BlockInfo;
|
||||
|
||||
Matrix expected_full = Matrix_(3, 3,
|
||||
3.0, 5.0, -8.0,
|
||||
0.0, 6.0, -9.0,
|
||||
0.0, 0.0, 10.0);
|
||||
|
||||
Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
|
||||
Vector g = Vector_(2, -8.0, -9.0);
|
||||
double f = 10.0;
|
||||
|
||||
size_t dims[] = { G.rows(), 1 };
|
||||
InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1);
|
||||
BlockInfo infoView(fullMatrix, dims, dims+2);
|
||||
infoView(0,0) = G;
|
||||
infoView.column(0,1,0) = g;
|
||||
infoView(1,1)(0,0) = f;
|
||||
|
||||
EXPECT_LONGS_EQUAL(0, infoView.blockStart());
|
||||
EXPECT_LONGS_EQUAL(2, infoView.nBlocks());
|
||||
EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix));
|
||||
EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1)))
|
||||
EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10);
|
||||
|
||||
EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0))));
|
||||
EXPECT(assert_equal(g, Vector(((const BlockInfo)infoView).rangeColumn(0, 1, 1, 0))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -28,7 +28,7 @@ double f(const LieVector& x) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(testNumericalDerivative, numericalHessian) {
|
||||
TEST(testNumericalDerivative, numericalHessian) {
|
||||
LieVector center = ones(2);
|
||||
|
||||
Matrix expected = Matrix_(2,2,
|
||||
|
|
@ -47,7 +47,7 @@ double f2(const LieVector& x) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(testNumericalDerivative, numericalHessian2) {
|
||||
TEST(testNumericalDerivative, numericalHessian2) {
|
||||
LieVector center(2, 0.5, 1.0);
|
||||
|
||||
Matrix expected = Matrix_(2,2,
|
||||
|
|
@ -66,7 +66,7 @@ double f3(const LieVector& x1, const LieVector& x2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(testNumericalDerivative, numericalHessian211) {
|
||||
TEST(testNumericalDerivative, numericalHessian211) {
|
||||
LieVector center1(1, 1.0), center2(1, 5.0);
|
||||
|
||||
Matrix expected11 = Matrix_(1,1,-sin(center1(0))*cos(center2(0)));
|
||||
|
|
@ -89,7 +89,7 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(testNumericalDerivative, numericalHessian311) {
|
||||
TEST(testNumericalDerivative, numericalHessian311) {
|
||||
LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
|
||||
double x = center1(0), y = center2(0), z = center3(0);
|
||||
Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,163 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testTreeTraversal
|
||||
* @brief Unit tests for tree traversal, cloning, and printing
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
|
||||
using boost::assign::operator+=;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
struct TestNode {
|
||||
typedef boost::shared_ptr<TestNode> shared_ptr;
|
||||
int data;
|
||||
vector<shared_ptr> children;
|
||||
TestNode() : data(-1) {}
|
||||
TestNode(int data) : data(data) {}
|
||||
};
|
||||
|
||||
struct TestForest {
|
||||
typedef TestNode Node;
|
||||
typedef Node::shared_ptr sharedNode;
|
||||
vector<sharedNode> roots_;
|
||||
const vector<sharedNode>& roots() const { return roots_; }
|
||||
};
|
||||
|
||||
TestForest makeTestForest() {
|
||||
// 0 1
|
||||
// / \
|
||||
// 2 3
|
||||
// |
|
||||
// 4
|
||||
TestForest forest;
|
||||
forest.roots_.push_back(boost::make_shared<TestNode>(0));
|
||||
forest.roots_.push_back(boost::make_shared<TestNode>(1));
|
||||
forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(2));
|
||||
forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(3));
|
||||
forest.roots_[0]->children[1]->children.push_back(boost::make_shared<TestNode>(4));
|
||||
return forest;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
struct PreOrderVisitor {
|
||||
// This visitor stores the nodes visited so the visit order can be checked later. It also returns
|
||||
// the current node index as the data and checks that the parent index properly matches the index
|
||||
// referenced in the node.
|
||||
std::list<int> visited;
|
||||
bool parentsMatched;
|
||||
PreOrderVisitor() : parentsMatched(true) {}
|
||||
int operator()(const TestNode::shared_ptr& node, int parentData) {
|
||||
visited.push_back(node->data);
|
||||
// Check parent index
|
||||
const int expectedParentIndex =
|
||||
node->data == 0 ? -1 :
|
||||
node->data == 1 ? -1 :
|
||||
node->data == 2 ? 0 :
|
||||
node->data == 3 ? 0 :
|
||||
node->data == 4 ? 3 :
|
||||
node->data == 10 ? 0 :
|
||||
(parentsMatched = false, -1);
|
||||
if(expectedParentIndex != parentData)
|
||||
parentsMatched = false;
|
||||
return node->data;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
struct PostOrderVisitor {
|
||||
// This visitor stores the nodes visited so the visit order can be checked later.
|
||||
std::list<int> visited;
|
||||
void operator()(const TestNode::shared_ptr& node, int myData) {
|
||||
visited.push_back(node->data);
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::list<int> getPreorder(const TestForest& forest) {
|
||||
std::list<int> result;
|
||||
PreOrderVisitor preVisitor;
|
||||
int rootData = -1;
|
||||
treeTraversal::DepthFirstForest(forest, rootData, preVisitor);
|
||||
result = preVisitor.visited;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(treeTraversal, DepthFirst)
|
||||
{
|
||||
// Get test forest
|
||||
TestForest testForest = makeTestForest();
|
||||
|
||||
// Expected visit order
|
||||
std::list<int> preOrderExpected;
|
||||
preOrderExpected += 0, 2, 3, 4, 1;
|
||||
std::list<int> postOrderExpected;
|
||||
postOrderExpected += 2, 4, 3, 0, 1;
|
||||
|
||||
// Actual visit order
|
||||
PreOrderVisitor preVisitor;
|
||||
PostOrderVisitor postVisitor;
|
||||
int rootData = -1;
|
||||
treeTraversal::DepthFirstForest(testForest, rootData, preVisitor, postVisitor);
|
||||
|
||||
EXPECT(preVisitor.parentsMatched);
|
||||
EXPECT(assert_container_equality(preOrderExpected, preVisitor.visited));
|
||||
EXPECT(assert_container_equality(postOrderExpected, postVisitor.visited));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(treeTraversal, CloneForest)
|
||||
{
|
||||
// Get test forest
|
||||
TestForest testForest1 = makeTestForest();
|
||||
TestForest testForest2;
|
||||
testForest2.roots_ = treeTraversal::CloneForest(testForest1);
|
||||
|
||||
// Check that the original and clone both are expected
|
||||
std::list<int> preOrder1Expected;
|
||||
preOrder1Expected += 0, 2, 3, 4, 1;
|
||||
std::list<int> preOrder1Actual = getPreorder(testForest1);
|
||||
std::list<int> preOrder2Actual = getPreorder(testForest2);
|
||||
EXPECT(assert_container_equality(preOrder1Expected, preOrder1Actual));
|
||||
EXPECT(assert_container_equality(preOrder1Expected, preOrder2Actual));
|
||||
|
||||
// Modify clone - should not modify original
|
||||
testForest2.roots_[0]->children[1]->data = 10;
|
||||
std::list<int> preOrderModifiedExpected;
|
||||
preOrderModifiedExpected += 0, 2, 10, 4, 1;
|
||||
|
||||
// Check that original is the same and only the clone is modified
|
||||
std::list<int> preOrder1ModActual = getPreorder(testForest1);
|
||||
std::list<int> preOrder2ModActual = getPreorder(testForest2);
|
||||
EXPECT(assert_container_equality(preOrder1Expected, preOrder1ModActual));
|
||||
EXPECT(assert_container_equality(preOrderModifiedExpected, preOrder2ModActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -199,7 +199,7 @@ TEST( TestVector, weightedPseudoinverse_constraint )
|
|||
|
||||
// verify
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
EXPECT(isinf(precision));
|
||||
EXPECT(std::isinf(precision));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,339 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 treeTraversal-inst.h
|
||||
* @author Richard Roberts
|
||||
* @date April 9, 2013
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
#include <stack>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#undef max // TBB seems to include windows.h and we don't want these macros
|
||||
#undef min
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Internal functions used for traversing trees */
|
||||
namespace treeTraversal {
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
// Internal node used in DFS preorder stack
|
||||
template<typename NODE, typename DATA>
|
||||
struct TraversalNode {
|
||||
bool expanded;
|
||||
const boost::shared_ptr<NODE>& treeNode;
|
||||
DATA& parentData;
|
||||
typename FastList<DATA>::iterator dataPointer;
|
||||
TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
|
||||
expanded(false), treeNode(_treeNode), parentData(_parentData) {}
|
||||
};
|
||||
|
||||
// Do nothing - default argument for post-visitor for tree traversal
|
||||
template<typename NODE, typename DATA>
|
||||
void no_op(const boost::shared_ptr<NODE>& node, const DATA& data) {}
|
||||
|
||||
// Internal node used in parallel traversal stack
|
||||
template<typename NODE, typename DATA>
|
||||
struct ParallelTraversalNode {
|
||||
const boost::shared_ptr<NODE>& treeNode;
|
||||
DATA myData;
|
||||
ParallelTraversalNode(const boost::shared_ptr<NODE>& treeNode, const DATA& myData) :
|
||||
treeNode(treeNode), myData(myData) {}
|
||||
};
|
||||
|
||||
template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
|
||||
class PreOrderTask : public tbb::task
|
||||
{
|
||||
public:
|
||||
const boost::shared_ptr<NODE>& treeNode;
|
||||
DATA myData;
|
||||
VISITOR_PRE& visitorPre;
|
||||
VISITOR_POST& visitorPost;
|
||||
int problemSizeThreshold;
|
||||
bool makeNewTasks;
|
||||
PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const DATA& myData,
|
||||
VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
|
||||
bool makeNewTasks = true) :
|
||||
treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
|
||||
problemSizeThreshold(problemSizeThreshold), makeNewTasks(makeNewTasks) {}
|
||||
|
||||
typedef ParallelTraversalNode<NODE, DATA> ParallelTraversalNodeType;
|
||||
|
||||
tbb::task* execute()
|
||||
{
|
||||
// Process this node and its children
|
||||
processNode(treeNode, myData);
|
||||
|
||||
// Return NULL
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void processNode(const boost::shared_ptr<NODE>& node, DATA& myData)
|
||||
{
|
||||
if(makeNewTasks)
|
||||
{
|
||||
bool overThreshold = (node->problemSize() >= problemSizeThreshold);
|
||||
|
||||
tbb::task_list childTasks;
|
||||
BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
|
||||
{
|
||||
// Process child in a subtask
|
||||
childTasks.push_back(*new(allocate_child())
|
||||
PreOrderTask(child, visitorPre(child, myData), visitorPre, visitorPost,
|
||||
problemSizeThreshold, overThreshold));
|
||||
}
|
||||
|
||||
// If we have child tasks, start subtasks and wait for them to complete
|
||||
set_ref_count(1 + (int)node->children.size());
|
||||
spawn(childTasks);
|
||||
wait_for_all();
|
||||
}
|
||||
else
|
||||
{
|
||||
BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
|
||||
{
|
||||
processNode(child, visitorPre(child, myData));
|
||||
}
|
||||
}
|
||||
|
||||
// Run the post-order visitor
|
||||
(void) visitorPost(node, myData);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
|
||||
class RootTask : public tbb::task
|
||||
{
|
||||
public:
|
||||
const ROOTS& roots;
|
||||
DATA& myData;
|
||||
VISITOR_PRE& visitorPre;
|
||||
VISITOR_POST& visitorPost;
|
||||
int problemSizeThreshold;
|
||||
RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
|
||||
int problemSizeThreshold) :
|
||||
roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
|
||||
problemSizeThreshold(problemSizeThreshold) {}
|
||||
|
||||
tbb::task* execute()
|
||||
{
|
||||
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
|
||||
// Set TBB ref count
|
||||
set_ref_count(1 + (int)roots.size());
|
||||
// Create data and tasks for our children
|
||||
std::vector<PreOrderTask*> tasks;
|
||||
tasks.reserve(roots.size());
|
||||
BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
|
||||
{
|
||||
tasks.push_back(new(allocate_child())
|
||||
PreOrderTask(root, visitorPre(root, myData), visitorPre, visitorPost, problemSizeThreshold));
|
||||
}
|
||||
// Spawn tasks
|
||||
BOOST_FOREACH(PreOrderTask* task, tasks)
|
||||
spawn(*task);
|
||||
// Wait for tasks to finish
|
||||
wait_for_all();
|
||||
// Return NULL
|
||||
return NULL;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
|
||||
RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
|
||||
CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
|
||||
int problemSizeThreshold)
|
||||
{
|
||||
typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
|
||||
return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//template<class NODE, typename DATA>
|
||||
//struct ParallelDFSData {
|
||||
// DATA myData;
|
||||
// FastList<ParallelTraversalNode<NODE,DATA> >&
|
||||
//};
|
||||
}
|
||||
|
||||
/** 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 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
|
||||
* to pass to the children. The returned \c DATA object will be copy-constructed only
|
||||
* upon returning to store internally, thus may be modified by visiting the children.
|
||||
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
|
||||
* @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. */
|
||||
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
|
||||
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
|
||||
{
|
||||
// Typedefs
|
||||
typedef typename FOREST::Node Node;
|
||||
typedef boost::shared_ptr<Node> sharedNode;
|
||||
|
||||
// Depth first traversal stack
|
||||
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
|
||||
typedef FastList<TraversalNode> Stack;
|
||||
Stack stack;
|
||||
FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
|
||||
|
||||
// Add roots to stack (insert such that they are visited and processed in order
|
||||
{
|
||||
typename Stack::iterator insertLocation = stack.begin();
|
||||
BOOST_FOREACH(const sharedNode& root, forest.roots())
|
||||
stack.insert(insertLocation, TraversalNode(root, rootData));
|
||||
}
|
||||
|
||||
// Traverse
|
||||
while(!stack.empty())
|
||||
{
|
||||
// Get next node
|
||||
TraversalNode& node = stack.front();
|
||||
|
||||
if(node.expanded) {
|
||||
// If already expanded, then the data stored in the node is no longer needed, so visit
|
||||
// then delete it.
|
||||
(void) visitorPost(node.treeNode, *node.dataPointer);
|
||||
dataList.erase(node.dataPointer);
|
||||
stack.pop_front();
|
||||
} else {
|
||||
// If not already visited, visit the node and add its children (use reverse iterators so
|
||||
// children are processed in the order they appear)
|
||||
node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData));
|
||||
typename Stack::iterator insertLocation = stack.begin();
|
||||
BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
|
||||
stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
|
||||
node.expanded = true;
|
||||
}
|
||||
}
|
||||
assert(dataList.empty());
|
||||
}
|
||||
|
||||
/** Traverse a forest depth-first, with a pre-order visit but no post-order visit.
|
||||
* @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 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
|
||||
* to pass to the children. The returned \c DATA object will be copy-constructed only
|
||||
* upon returning to store internally, thus may be modified by visiting the children.
|
||||
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
|
||||
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
|
||||
* root node. */
|
||||
template<class FOREST, typename DATA, typename VISITOR_PRE>
|
||||
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
|
||||
{
|
||||
DepthFirstForest(forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
|
||||
}
|
||||
|
||||
/** 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 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
|
||||
* to pass to the children. The returned \c DATA object will be copy-constructed only
|
||||
* upon returning to store internally, thus may be modified by visiting the children.
|
||||
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
|
||||
* @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. */
|
||||
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
|
||||
void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
|
||||
int problemSizeThreshold = 10)
|
||||
{
|
||||
// Typedefs
|
||||
typedef typename FOREST::Node Node;
|
||||
typedef boost::shared_ptr<Node> sharedNode;
|
||||
|
||||
tbb::task::spawn_root_and_wait(CreateRootTask<Node>(
|
||||
forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Traversal function for CloneForest */
|
||||
namespace {
|
||||
template<typename NODE>
|
||||
boost::shared_ptr<NODE>
|
||||
CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
|
||||
{
|
||||
// Clone the current node and add it to its cloned parent
|
||||
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
|
||||
clone->children.clear();
|
||||
parentPointer->children.push_back(clone);
|
||||
return clone;
|
||||
}
|
||||
}
|
||||
|
||||
/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child
|
||||
* pointers for a clone of the original tree.
|
||||
* @param forest The forest of trees to clone. The method \c forest.roots() should exist and
|
||||
* return a collection of shared pointers to \c FOREST::Node.
|
||||
* @return The new collection of roots. */
|
||||
template<class FOREST>
|
||||
std::vector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest)
|
||||
{
|
||||
typedef typename FOREST::Node Node;
|
||||
boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
|
||||
DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
|
||||
return std::vector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end());
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Traversal function for PrintForest */
|
||||
namespace {
|
||||
struct PrintForestVisitorPre
|
||||
{
|
||||
const KeyFormatter& formatter;
|
||||
PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {}
|
||||
template<typename NODE> std::string operator()(const boost::shared_ptr<NODE>& node, const std::string& parentString)
|
||||
{
|
||||
// Print the current node
|
||||
node->print(parentString + "-", formatter);
|
||||
// Increment the indentation
|
||||
return parentString + "| ";
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
|
||||
* To print each node, this function calls the \c print function of the tree nodes. */
|
||||
template<class FOREST>
|
||||
void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) {
|
||||
typedef typename FOREST::Node Node;
|
||||
PrintForestVisitorPre visitor(keyFormatter);
|
||||
DepthFirstForest(forest, str, visitor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -20,11 +20,22 @@
|
|||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string _defaultIndexFormatter(Index j) {
|
||||
return boost::lexical_cast<std::string>(j);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string _defaultKeyFormatter(Key key) {
|
||||
const Symbol asSymbol(key);
|
||||
if(asSymbol.chr() > 0)
|
||||
return (std::string)asSymbol;
|
||||
else
|
||||
return boost::lexical_cast<std::string>(key);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
#include <string>
|
||||
#include <boost/function/function1.hpp>
|
||||
#include <boost/range/concepts.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -40,6 +41,26 @@ namespace gtsam {
|
|||
/** The default IndexFormatter outputs the index */
|
||||
static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter;
|
||||
|
||||
|
||||
/// Integer nonlinear key type
|
||||
typedef size_t Key;
|
||||
|
||||
/// Typedef for a function to format a key, i.e. to convert it to a string
|
||||
typedef boost::function<std::string(Key)> KeyFormatter;
|
||||
|
||||
// Helper function for DefaultKeyFormatter
|
||||
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
|
||||
|
||||
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
|
||||
/// a nonlinear 'print' function. Automatically detects plain integer keys
|
||||
/// and Symbol keys.
|
||||
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
|
||||
|
||||
|
||||
/// The index type for Eigen objects
|
||||
typedef ptrdiff_t DenseIndex;
|
||||
|
||||
|
||||
/**
|
||||
* Helper class that uses templates to select between two types based on
|
||||
* whether TEST_TYPE is const or not.
|
||||
|
|
@ -83,6 +104,42 @@ namespace gtsam {
|
|||
operator T() const { return value; }
|
||||
};
|
||||
|
||||
/** A helper class that behaves as a container with one element, and works with
|
||||
* boost::range */
|
||||
template<typename T>
|
||||
class ListOfOneContainer {
|
||||
T element_;
|
||||
public:
|
||||
typedef T value_type;
|
||||
typedef const T* const_iterator;
|
||||
typedef T* iterator;
|
||||
ListOfOneContainer(const T& element) : element_(element) {}
|
||||
const T* begin() const { return &element_; }
|
||||
const T* end() const { return &element_ + 1; }
|
||||
T* begin() { return &element_; }
|
||||
T* end() { return &element_ + 1; }
|
||||
size_t size() const { return 1; }
|
||||
};
|
||||
|
||||
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
|
||||
|
||||
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
|
||||
template<typename T>
|
||||
ListOfOneContainer<T> ListOfOne(const T& element) {
|
||||
return ListOfOneContainer<T>(element);
|
||||
}
|
||||
|
||||
/** An assertion that throws an exception if NDEBUG is not defined and
|
||||
* evaluates to an empty statement otherwise. */
|
||||
#ifdef NDEBUG
|
||||
#define assert_throw(CONDITION, EXCEPTION) ((void)0)
|
||||
#else
|
||||
#define assert_throw(CONDITION, EXCEPTION) \
|
||||
if(!(CONDITION)) { \
|
||||
throw (EXCEPTION); \
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#ifdef _MSC_VER
|
||||
|
|
|
|||
|
|
@ -45,14 +45,14 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool DecisionTreeFactor::equals(const This& other, double tol) const {
|
||||
return IndexFactor::equals(other, tol) && Potentials::equals(other, tol);
|
||||
return IndexFactorOrdered::equals(other, tol) && Potentials::equals(other, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void DecisionTreeFactor::print(const string& s,
|
||||
const IndexFormatter& formatter) const {
|
||||
cout << s;
|
||||
IndexFactor::print("IndexFactor:",formatter);
|
||||
IndexFactorOrdered::print("IndexFactor:",formatter);
|
||||
Potentials::print("Potentials:",formatter);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,13 +18,13 @@
|
|||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
#include <gtsam/inference/BayesNetOrdered-inl.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class BayesNet<DiscreteConditional> ;
|
||||
template class BayesNetOrdered<DiscreteConditional> ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void add_front(DiscreteBayesNet& bayesNet, const Signature& s) {
|
||||
|
|
|
|||
|
|
@ -20,12 +20,12 @@
|
|||
#include <vector>
|
||||
#include <map>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/BayesNetOrdered.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef BayesNet<DiscreteConditional> DiscreteBayesNet;
|
||||
typedef BayesNetOrdered<DiscreteConditional> DiscreteBayesNet;
|
||||
|
||||
/** Add a DiscreteCondtional */
|
||||
GTSAM_EXPORT void add(DiscreteBayesNet&, const Signature& s);
|
||||
|
|
|
|||
|
|
@ -37,14 +37,14 @@ namespace gtsam {
|
|||
/* ******************************************************************************** */
|
||||
DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
|
||||
const DecisionTreeFactor& f) :
|
||||
IndexConditional(f.keys(), nrFrontals), Potentials(
|
||||
IndexConditionalOrdered(f.keys(), nrFrontals), Potentials(
|
||||
f / (*f.sum(nrFrontals))) {
|
||||
}
|
||||
|
||||
/* ******************************************************************************** */
|
||||
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
|
||||
const DecisionTreeFactor& marginal) :
|
||||
IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials(
|
||||
IndexConditionalOrdered(joint.keys(), joint.size() - marginal.size()), Potentials(
|
||||
ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) {
|
||||
if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
|
||||
cout << (firstFrontalKey()) << endl; //TODO Print all keys
|
||||
|
|
@ -52,20 +52,20 @@ namespace gtsam {
|
|||
|
||||
/* ******************************************************************************** */
|
||||
DiscreteConditional::DiscreteConditional(const Signature& signature) :
|
||||
IndexConditional(signature.indices(), 1), Potentials(
|
||||
IndexConditionalOrdered(signature.indices(), 1), Potentials(
|
||||
signature.discreteKeysParentsFirst(), signature.cpt()) {
|
||||
}
|
||||
|
||||
/* ******************************************************************************** */
|
||||
void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const {
|
||||
std::cout << s << std::endl;
|
||||
IndexConditional::print(s, formatter);
|
||||
IndexConditionalOrdered::print(s, formatter);
|
||||
Potentials::print(s);
|
||||
}
|
||||
|
||||
/* ******************************************************************************** */
|
||||
bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const {
|
||||
return IndexConditional::equals(other, tol)
|
||||
return IndexConditionalOrdered::equals(other, tol)
|
||||
&& Potentials::equals(other, tol);
|
||||
}
|
||||
|
||||
|
|
@ -197,7 +197,7 @@ namespace gtsam {
|
|||
|
||||
/* ******************************************************************************** */
|
||||
void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){
|
||||
IndexConditional::permuteWithInverse(inversePermutation);
|
||||
IndexConditionalOrdered::permuteWithInverse(inversePermutation);
|
||||
Potentials::permuteWithInverse(inversePermutation);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/Signature.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
#include <gtsam/inference/IndexConditionalOrdered.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
|
|
@ -30,13 +30,13 @@ namespace gtsam {
|
|||
* Discrete Conditional Density
|
||||
* Derives from DecisionTreeFactor
|
||||
*/
|
||||
class GTSAM_EXPORT DiscreteConditional: public IndexConditional, public Potentials {
|
||||
class GTSAM_EXPORT DiscreteConditional: public IndexConditionalOrdered, public Potentials {
|
||||
|
||||
public:
|
||||
// typedefs needed to play nice with gtsam
|
||||
typedef DiscreteFactor FactorType;
|
||||
typedef boost::shared_ptr<DiscreteConditional> shared_ptr;
|
||||
typedef IndexConditional Base;
|
||||
typedef IndexConditionalOrdered Base;
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef Assignment<Index> Values;
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/discrete/Assignment.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/IndexFactorOrdered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
|||
* Base class for discrete probabilistic factors
|
||||
* The most general one is the derived DecisionTreeFactor
|
||||
*/
|
||||
class GTSAM_EXPORT DiscreteFactor: public IndexFactor {
|
||||
class GTSAM_EXPORT DiscreteFactor: public IndexFactorOrdered {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -47,23 +47,23 @@ namespace gtsam {
|
|||
|
||||
/// Construct n-way factor
|
||||
DiscreteFactor(const std::vector<Index>& js) :
|
||||
IndexFactor(js) {
|
||||
IndexFactorOrdered(js) {
|
||||
}
|
||||
|
||||
/// Construct unary factor
|
||||
DiscreteFactor(Index j) :
|
||||
IndexFactor(j) {
|
||||
IndexFactorOrdered(j) {
|
||||
}
|
||||
|
||||
/// Construct binary factor
|
||||
DiscreteFactor(Index j1, Index j2) :
|
||||
IndexFactor(j1, j2) {
|
||||
IndexFactorOrdered(j1, j2) {
|
||||
}
|
||||
|
||||
/// construct from container
|
||||
template<class KeyIterator>
|
||||
DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) :
|
||||
IndexFactor(beginKey, endKey) {
|
||||
IndexFactorOrdered(beginKey, endKey) {
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
@ -85,7 +85,7 @@ namespace gtsam {
|
|||
virtual void print(const std::string& s = "DiscreteFactor\n",
|
||||
const IndexFormatter& formatter
|
||||
=DefaultIndexFormatter) const {
|
||||
IndexFactor::print(s,formatter);
|
||||
IndexFactorOrdered::print(s,formatter);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -105,14 +105,14 @@ namespace gtsam {
|
|||
* to already be inverted.
|
||||
*/
|
||||
virtual void permuteWithInverse(const Permutation& inversePermutation){
|
||||
IndexFactor::permuteWithInverse(inversePermutation);
|
||||
IndexFactorOrdered::permuteWithInverse(inversePermutation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Apply a reduction, which is a remapping of variable indices.
|
||||
*/
|
||||
virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
|
||||
IndexFactor::reduceWithInverse(inverseReduction);
|
||||
IndexFactorOrdered::reduceWithInverse(inverseReduction);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -19,14 +19,14 @@
|
|||
//#define ENABLE_TIMING
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/discrete/DiscreteConditional.h>
|
||||
#include <gtsam/inference/EliminationTree-inl.h>
|
||||
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<DiscreteFactor> ;
|
||||
template class EliminationTree<DiscreteFactor> ;
|
||||
template class FactorGraphOrdered<DiscreteFactor> ;
|
||||
template class EliminationTreeOrdered<DiscreteFactor> ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph() {
|
||||
|
|
@ -34,8 +34,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph(
|
||||
const BayesNet<DiscreteConditional>& bayesNet) :
|
||||
FactorGraph<DiscreteFactor>(bayesNet) {
|
||||
const BayesNetOrdered<DiscreteConditional>& bayesNet) :
|
||||
FactorGraphOrdered<DiscreteFactor>(bayesNet) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -95,7 +95,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
|
||||
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {
|
||||
EliminateDiscrete(const FactorGraphOrdered<DiscreteFactor>& factors, size_t num) {
|
||||
|
||||
// PRODUCT: multiply all factors
|
||||
gttic(product);
|
||||
|
|
|
|||
|
|
@ -20,13 +20,13 @@
|
|||
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/FactorGraphOrdered.h>
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class DiscreteFactorGraph: public FactorGraph<DiscreteFactor> {
|
||||
class DiscreteFactorGraph: public FactorGraphOrdered<DiscreteFactor> {
|
||||
public:
|
||||
|
||||
/** A map from keys to values */
|
||||
|
|
@ -39,12 +39,12 @@ public:
|
|||
|
||||
/** Constructor from a factor graph of GaussianFactor or a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
|
||||
DiscreteFactorGraph(const FactorGraphOrdered<DERIVEDFACTOR>& fg) {
|
||||
push_back(fg);
|
||||
}
|
||||
|
||||
/** construct from a BayesNet */
|
||||
GTSAM_EXPORT DiscreteFactorGraph(const BayesNet<DiscreteConditional>& bayesNet);
|
||||
GTSAM_EXPORT DiscreteFactorGraph(const BayesNetOrdered<DiscreteConditional>& bayesNet);
|
||||
|
||||
template<class SOURCE>
|
||||
void add(const DiscreteKey& j, SOURCE table) {
|
||||
|
|
@ -90,7 +90,7 @@ public:
|
|||
|
||||
/** Main elimination function for DiscreteFactorGraph */
|
||||
GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
|
||||
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors,
|
||||
EliminateDiscrete(const FactorGraphOrdered<DiscreteFactor>& factors,
|
||||
size_t nrFrontals = 1);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ namespace gtsam {
|
|||
|
||||
protected:
|
||||
|
||||
BayesTree<DiscreteConditional> bayesTree_;
|
||||
BayesTreeOrdered<DiscreteConditional> bayesTree_;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -40,7 +40,7 @@ namespace gtsam {
|
|||
* @param graph The factor graph defining the full joint density on all variables.
|
||||
*/
|
||||
DiscreteMarginals(const DiscreteFactorGraph& graph) {
|
||||
typedef JunctionTree<DiscreteFactorGraph> DiscreteJT;
|
||||
typedef JunctionTreeOrdered<DiscreteFactorGraph> DiscreteJT;
|
||||
GenericMultifrontalSolver<DiscreteFactor, DiscreteJT> solver(graph);
|
||||
bayesTree_ = *solver.eliminate(&EliminateDiscrete);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ namespace gtsam {
|
|||
* Construct the solver for a factor graph. This builds the elimination
|
||||
* tree, which already does some of the work of elimination.
|
||||
*/
|
||||
DiscreteSequentialSolver(const FactorGraph<DiscreteFactor>& factorGraph) :
|
||||
DiscreteSequentialSolver(const FactorGraphOrdered<DiscreteFactor>& factorGraph) :
|
||||
Base(factorGraph) {
|
||||
}
|
||||
|
||||
|
|
@ -57,12 +57,12 @@ namespace gtsam {
|
|||
* is the fastest.
|
||||
*/
|
||||
DiscreteSequentialSolver(
|
||||
const FactorGraph<DiscreteFactor>::shared_ptr& factorGraph,
|
||||
const VariableIndex::shared_ptr& variableIndex) :
|
||||
const FactorGraphOrdered<DiscreteFactor>::shared_ptr& factorGraph,
|
||||
const VariableIndexOrdered::shared_ptr& variableIndex) :
|
||||
Base(factorGraph, variableIndex) {
|
||||
}
|
||||
|
||||
const EliminationTree<DiscreteFactor>& eliminationTree() const {
|
||||
const EliminationTreeOrdered<DiscreteFactor>& eliminationTree() const {
|
||||
return *eliminationTree_;
|
||||
}
|
||||
|
||||
|
|
@ -70,7 +70,7 @@ namespace gtsam {
|
|||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
BayesNet<DiscreteConditional>::shared_ptr eliminate() const {
|
||||
BayesNetOrdered<DiscreteConditional>::shared_ptr eliminate() const {
|
||||
return Base::eliminate(&EliminateDiscrete);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include <gtsam/discrete/AlgebraicDecisionTree.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
#include <gtsam/inference/PermutationOrdered.h>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <set>
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/BayesTreeOrdered.h>
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
using namespace boost::assign;
|
||||
|
|
@ -32,13 +32,13 @@ static bool debug = false;
|
|||
/**
|
||||
* Custom clique class to debug shortcuts
|
||||
*/
|
||||
class Clique: public BayesTreeCliqueBase<Clique, DiscreteConditional> {
|
||||
class Clique: public BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> {
|
||||
|
||||
protected:
|
||||
|
||||
public:
|
||||
|
||||
typedef BayesTreeCliqueBase<Clique, DiscreteConditional> Base;
|
||||
typedef BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> Base;
|
||||
typedef boost::shared_ptr<Clique> shared_ptr;
|
||||
|
||||
// Constructors
|
||||
|
|
@ -56,7 +56,7 @@ public:
|
|||
/// print index signature only
|
||||
void printSignature(const std::string& s = "Clique: ",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const {
|
||||
((IndexConditional::shared_ptr) conditional_)->print(s, indexFormatter);
|
||||
((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter);
|
||||
}
|
||||
|
||||
/// evaluate value of sub-tree
|
||||
|
|
@ -70,7 +70,7 @@ public:
|
|||
|
||||
};
|
||||
|
||||
typedef BayesTree<DiscreteConditional, Clique> DiscreteBayesTree;
|
||||
typedef BayesTreeOrdered<DiscreteConditional, Clique> DiscreteBayesTree;
|
||||
|
||||
/* ************************************************************************* */
|
||||
double evaluate(const DiscreteBayesTree& tree,
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ TEST_UNSAFE( DiscreteFactorGraph, debugScheduler) {
|
|||
//
|
||||
DiscreteSequentialSolver solver(graph);
|
||||
// solver.print("solver:");
|
||||
EliminationTree<DiscreteFactor> eliminationTree = solver.eliminationTree();
|
||||
EliminationTreeOrdered<DiscreteFactor> eliminationTree = solver.eliminationTree();
|
||||
// eliminationTree.print("Elimination tree: ");
|
||||
eliminationTree.eliminate(EliminateDiscrete);
|
||||
// solver.optimize();
|
||||
|
|
@ -155,7 +155,7 @@ TEST( DiscreteFactorGraph, test)
|
|||
// GTSAM_PRINT(expected);
|
||||
|
||||
// Test elimination tree
|
||||
const EliminationTree<DiscreteFactor>& etree = solver.eliminationTree();
|
||||
const EliminationTreeOrdered<DiscreteFactor>& etree = solver.eliminationTree();
|
||||
DiscreteBayesNet::shared_ptr actual = etree.eliminate(&EliminateDiscrete);
|
||||
EXPECT(assert_equal(expected, *actual));
|
||||
|
||||
|
|
@ -221,16 +221,16 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
|
|||
// EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9);
|
||||
|
||||
// Let us create the Bayes tree here, just for fun, because we don't use it now
|
||||
typedef JunctionTree<DiscreteFactorGraph> JT;
|
||||
typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
|
||||
GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph);
|
||||
BayesTree<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
|
||||
BayesTreeOrdered<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
|
||||
// bayesTree->print("Bayes Tree");
|
||||
EXPECT_LONGS_EQUAL(2,bayesTree->size());
|
||||
|
||||
#ifdef OLD
|
||||
// Create the elimination tree manually
|
||||
VariableIndex structure(graph);
|
||||
typedef EliminationTree<DiscreteFactor> ETree;
|
||||
VariableIndexOrdered structure(graph);
|
||||
typedef EliminationTreeOrdered<DiscreteFactor> ETree;
|
||||
ETree::shared_ptr eTree = ETree::Create(graph, structure);
|
||||
//eTree->print(">>>>>>>>>>> Elimination Tree <<<<<<<<<<<<<<<<<");
|
||||
|
||||
|
|
|
|||
|
|
@ -119,9 +119,9 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
|
|||
graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1");
|
||||
graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2");
|
||||
graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1");
|
||||
typedef JunctionTree<DiscreteFactorGraph> JT;
|
||||
typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
|
||||
GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph);
|
||||
BayesTree<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
|
||||
BayesTreeOrdered<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
|
||||
// bayesTree->print("Bayes Tree");
|
||||
typedef BayesTreeClique<DiscreteConditional> Clique;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,36 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 dllexport.h
|
||||
* @brief Symbols for exporting classes and methods from DLLs
|
||||
* @author Richard Roberts
|
||||
* @date Mar 9, 2013
|
||||
*/
|
||||
|
||||
#ifdef _WIN32
|
||||
# ifdef GTSAM_EXPORTS
|
||||
# define GTSAM_EXPORT __declspec(dllexport)
|
||||
# define GTSAM_EXTERN_EXPORT __declspec(dllexport) extern
|
||||
# else
|
||||
# ifndef GTSAM_IMPORT_STATIC
|
||||
# define GTSAM_EXPORT __declspec(dllimport)
|
||||
# define GTSAM_EXTERN_EXPORT __declspec(dllimport)
|
||||
# else /* GTSAM_IMPORT_STATIC */
|
||||
# define GTSAM_EXPORT
|
||||
# define GTSAM_EXTERN_EXPORT extern
|
||||
# endif /* GTSAM_IMPORT_STATIC */
|
||||
# endif /* GTSAM_EXPORTS */
|
||||
#else /* _WIN32 */
|
||||
# define GTSAM_EXPORT
|
||||
# define GTSAM_EXTERN_EXPORT extern
|
||||
#endif
|
||||
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
*.lo
|
||||
testPoint2
|
||||
testSimulated2D
|
||||
testCal3_S2
|
||||
testChordalBayesNet
|
||||
testNonlinearFactorGraph
|
||||
testLinearFactor
|
||||
testConditionalGaussian
|
||||
testPose3
|
||||
testFactorgraph
|
||||
testSimulated3D
|
||||
testFGConfig
|
||||
testNonlinearFactor
|
||||
testPoint3
|
||||
testMatrix
|
||||
testVector
|
||||
testRot3
|
||||
testLinearFactorGraph
|
||||
testVSLAMFactor
|
||||
config.h
|
||||
html
|
||||
config.h.in
|
||||
stamp-h1
|
||||
Makefile
|
||||
Makefile.in
|
||||
libgtsam.la
|
||||
simple_svd_test
|
||||
simple_ublas_resize_test
|
||||
simple_ublas_resize_test.cpp
|
||||
test_svdcmp.cpp
|
||||
.deps
|
||||
.libs
|
||||
resize_test
|
||||
testConstrainedChordalBayesNet
|
||||
testConstrainedLinearFactorGraph
|
||||
testConstrainedNonlinearFactorGraph
|
||||
testDeltaFunction
|
||||
testUnconstrainedLinearFactorGraph
|
||||
testUnconstrainedNonlinearFactorGraph
|
||||
testEqualityFactor
|
||||
.DS_Store
|
||||
testSimpleCamera
|
||||
testCalibratedCamera
|
||||
|
|
@ -1,192 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesNet-inl.h
|
||||
* @brief Bayes net template definitions
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include <boost/assign/std/vector.hpp> // for +=
|
||||
using boost::assign::operator+=;
|
||||
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::print(const std::string& s,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << s;
|
||||
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_)
|
||||
conditional->print("Conditional", formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
bool BayesNet<CONDITIONAL>::equals(const BayesNet& cbn, double tol) const {
|
||||
if (size() != cbn.size())
|
||||
return false;
|
||||
return std::equal(conditionals_.begin(), conditionals_.end(),
|
||||
cbn.conditionals_.begin(), equals_star<CONDITIONAL>(tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::printStats(const std::string& s) const {
|
||||
|
||||
const size_t n = conditionals_.size();
|
||||
size_t max_size = 0;
|
||||
size_t total = 0;
|
||||
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) {
|
||||
max_size = std::max(max_size, conditional->size());
|
||||
total += conditional->size();
|
||||
}
|
||||
std::cout << s
|
||||
<< "maximum conditional size = " << max_size << std::endl
|
||||
<< "average conditional size = " << total / n << std::endl
|
||||
<< "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s,
|
||||
const IndexFormatter& indexFormatter) const {
|
||||
std::ofstream of(s.c_str());
|
||||
of << "digraph G{\n";
|
||||
|
||||
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
|
||||
typename CONDITIONAL::Frontals frontals = conditional->frontals();
|
||||
Index me = frontals.front();
|
||||
typename CONDITIONAL::Parents parents = conditional->parents();
|
||||
BOOST_FOREACH(Index p, parents)
|
||||
of << p << "->" << me << std::endl;
|
||||
}
|
||||
|
||||
of << "}";
|
||||
of.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::const_iterator BayesNet<CONDITIONAL>::find(
|
||||
Index key) const {
|
||||
for (const_iterator it = begin(); it != end(); ++it)
|
||||
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
|
||||
!= (*it)->endFrontals())
|
||||
return it;
|
||||
return end();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::iterator BayesNet<CONDITIONAL>::find(
|
||||
Index key) {
|
||||
for (iterator it = begin(); it != end(); ++it)
|
||||
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
|
||||
!= (*it)->endFrontals())
|
||||
return it;
|
||||
return end();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::permuteWithInverse(
|
||||
const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
|
||||
conditional->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL>& bn) {
|
||||
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
|
||||
push_back(conditional);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::push_front(const BayesNet<CONDITIONAL>& bn) {
|
||||
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
|
||||
push_front(conditional);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::popLeaf(iterator conditional) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) {
|
||||
BOOST_FOREACH(Index key, (*conditional)->frontals()) {
|
||||
if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents())
|
||||
throw std::invalid_argument(
|
||||
"Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf.");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
conditionals_.erase(conditional);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
FastList<Index> BayesNet<CONDITIONAL>::ordering() const {
|
||||
FastList<Index> ord;
|
||||
BOOST_FOREACH(sharedConditional conditional,conditionals_)
|
||||
ord.insert(ord.begin(), conditional->beginFrontals(),
|
||||
conditional->endFrontals());
|
||||
return ord;
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// template<class CONDITIONAL>
|
||||
// void BayesNet<CONDITIONAL>::saveGraph(const std::string &s) const {
|
||||
// ofstream of(s.c_str());
|
||||
// of<< "digraph G{\n";
|
||||
// BOOST_FOREACH(const_sharedConditional conditional,conditionals_) {
|
||||
// Index child = conditional->key();
|
||||
// BOOST_FOREACH(Index parent, conditional->parents()) {
|
||||
// of << parent << "->" << child << endl;
|
||||
// }
|
||||
// }
|
||||
// of<<"}";
|
||||
// of.close();
|
||||
// }
|
||||
//
|
||||
/* ************************************************************************* */
|
||||
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::sharedConditional BayesNet<CONDITIONAL>::operator[](
|
||||
Index key) const {
|
||||
BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
|
||||
typename CONDITIONAL::const_iterator it = std::find(
|
||||
conditional->beginFrontals(), conditional->endFrontals(), key);
|
||||
if (it != conditional->endFrontals()) {
|
||||
return conditional;
|
||||
}
|
||||
}
|
||||
throw(std::invalid_argument(
|
||||
(boost::format("BayesNet::operator['%1%']: not found") % key).str()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesNet.h
|
||||
* @brief Bayes network
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <fstream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
|
||||
{
|
||||
Base::print(s, formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
std::ofstream of(s.c_str());
|
||||
of << "digraph G{\n";
|
||||
|
||||
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
|
||||
typename CONDITIONAL::Frontals frontals = conditional->frontals();
|
||||
Key me = frontals.front();
|
||||
typename CONDITIONAL::Parents parents = conditional->parents();
|
||||
BOOST_FOREACH(Key p, parents)
|
||||
of << p << "->" << me << std::endl;
|
||||
}
|
||||
|
||||
of << "}";
|
||||
of.close();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,244 +1,72 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesNet.h
|
||||
* @brief Bayes network
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A BayesNet is a list of conditionals, stored in elimination order, i.e.
|
||||
* leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are
|
||||
* defined as typedefs of this class, using GaussianConditional and
|
||||
* IndexConditional as the CONDITIONAL template argument.
|
||||
*
|
||||
* todo: Symbolic using Index is a misnomer.
|
||||
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
class BayesNet {
|
||||
|
||||
public:
|
||||
|
||||
typedef typename boost::shared_ptr<BayesNet<CONDITIONAL> > shared_ptr;
|
||||
|
||||
/** We store shared pointers to Conditional densities */
|
||||
typedef typename CONDITIONAL::KeyType KeyType;
|
||||
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
|
||||
typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
|
||||
typedef typename std::list<sharedConditional> Conditionals;
|
||||
|
||||
typedef typename Conditionals::iterator iterator;
|
||||
typedef typename Conditionals::reverse_iterator reverse_iterator;
|
||||
typedef typename Conditionals::const_iterator const_iterator;
|
||||
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* Conditional densities are stored in reverse topological sort order (i.e., leaves first,
|
||||
* parents last), which corresponds to the elimination ordering if so obtained,
|
||||
* and is consistent with the column (block) ordering of an upper triangular matrix.
|
||||
*/
|
||||
Conditionals conditionals_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor as an empty BayesNet */
|
||||
BayesNet() {};
|
||||
|
||||
/** convert from a derived type */
|
||||
template<class DERIVEDCONDITIONAL>
|
||||
BayesNet(const BayesNet<DERIVEDCONDITIONAL>& bn) {
|
||||
conditionals_.assign(bn.begin(), bn.end());
|
||||
}
|
||||
|
||||
/** BayesNet with 1 conditional */
|
||||
explicit BayesNet(const sharedConditional& conditional) { push_back(conditional); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/** check equality */
|
||||
bool equals(const BayesNet& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** size is the number of nodes */
|
||||
size_t size() const {
|
||||
return conditionals_.size();
|
||||
}
|
||||
|
||||
/** @return true if there are no conditionals/nodes */
|
||||
bool empty() const {
|
||||
return conditionals_.empty();
|
||||
}
|
||||
|
||||
/** print statistics */
|
||||
void printStats(const std::string& s = "") const;
|
||||
|
||||
/** save dot graph */
|
||||
void saveGraph(const std::string &s, const IndexFormatter& indexFormatter =
|
||||
DefaultIndexFormatter) const;
|
||||
|
||||
/** return keys in reverse topological sort order, i.e., elimination order */
|
||||
FastList<Index> ordering() const;
|
||||
|
||||
/** SLOW O(n) random access to Conditional by key */
|
||||
sharedConditional operator[](Index key) const;
|
||||
|
||||
/** return last node in ordering */
|
||||
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
|
||||
|
||||
/** return last node in ordering */
|
||||
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
|
||||
|
||||
/** return iterators. FD: breaks encapsulation? */
|
||||
const_iterator begin() const {return conditionals_.begin();} ///<TODO: comment
|
||||
const_iterator end() const {return conditionals_.end();} ///<TODO: comment
|
||||
const_reverse_iterator rbegin() const {return conditionals_.rbegin();} ///<TODO: comment
|
||||
const_reverse_iterator rend() const {return conditionals_.rend();} ///<TODO: comment
|
||||
|
||||
/** Find an iterator pointing to the conditional where the specified key
|
||||
* appears as a frontal variable, or end() if no conditional contains this
|
||||
* key. Running time is approximately \f$ O(n) \f$ in the number of
|
||||
* conditionals in the BayesNet.
|
||||
* @param key The index to find in the frontal variables of a conditional.
|
||||
*/
|
||||
const_iterator find(Index key) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Remove any leaf conditional. The conditional to remove is specified by
|
||||
* iterator. To find the iterator pointing to the conditional containing a
|
||||
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
|
||||
* popLeaf function by itself has \f$ O(1) \f$ complexity.
|
||||
*
|
||||
* If gtsam is compiled with GTSAM_EXTRA_CONSISTENCY_CHECKS defined, this
|
||||
* function will check that the node is indeed a leaf, but otherwise will
|
||||
* not check, because the check has \f$ O(n^2) \f$ complexity.
|
||||
*
|
||||
* Example 1:
|
||||
\code
|
||||
// Remove a leaf node with a known conditional
|
||||
GaussianBayesNet gbn = ...
|
||||
GaussianBayesNet::iterator leafConditional = ...
|
||||
gbn.popLeaf(leafConditional);
|
||||
\endcode
|
||||
* Example 2:
|
||||
\code
|
||||
// Remove the leaf node containing variable index 14
|
||||
GaussianBayesNet gbn = ...
|
||||
gbn.popLeaf(gbn.find(14));
|
||||
\endcode
|
||||
* @param conditional The iterator pointing to the leaf conditional to remove
|
||||
*/
|
||||
void popLeaf(iterator conditional);
|
||||
|
||||
/** return last node in ordering */
|
||||
sharedConditional& front() { return conditionals_.front(); }
|
||||
|
||||
/** return last node in ordering */
|
||||
sharedConditional& back() { return conditionals_.back(); }
|
||||
|
||||
/** Find an iterator pointing to the conditional where the specified key
|
||||
* appears as a frontal variable, or end() if no conditional contains this
|
||||
* key. Running time is approximately \f$ O(n) \f$ in the number of
|
||||
* conditionals in the BayesNet.
|
||||
* @param key The index to find in the frontal variables of a conditional.
|
||||
*/
|
||||
iterator find(Index key);
|
||||
|
||||
/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
|
||||
inline void push_back(const sharedConditional& conditional) {
|
||||
conditionals_.push_back(conditional);
|
||||
}
|
||||
|
||||
/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
|
||||
inline void push_front(const sharedConditional& conditional) {
|
||||
conditionals_.push_front(conditional);
|
||||
}
|
||||
|
||||
/// push_back an entire Bayes net
|
||||
void push_back(const BayesNet<CONDITIONAL>& bn);
|
||||
|
||||
/// push_front an entire Bayes net
|
||||
void push_front(const BayesNet<CONDITIONAL>& bn);
|
||||
|
||||
/** += syntax for push_back, e.g. bayesNet += c1, c2, c3
|
||||
* @param conditional The conditional to add to the back of the BayesNet
|
||||
*/
|
||||
boost::assign::list_inserter<boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >, sharedConditional>
|
||||
operator+=(const sharedConditional& conditional) {
|
||||
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<BayesNet<CONDITIONAL> >(*this))(conditional); }
|
||||
|
||||
/**
|
||||
* pop_front: remove node at the bottom, used in marginalization
|
||||
* For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
|
||||
*/
|
||||
void pop_front() {conditionals_.pop_front();}
|
||||
|
||||
/** Permute the variables in the BayesNet */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
iterator begin() {return conditionals_.begin();} ///<TODO: comment
|
||||
iterator end() {return conditionals_.end();} ///<TODO: comment
|
||||
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
|
||||
reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
|
||||
|
||||
/** saves the bayes to a text file in GraphViz format */
|
||||
// void saveGraph(const std::string& s) const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(conditionals_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
}; // BayesNet
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesNet.h
|
||||
* @brief Bayes network
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A BayesNet is a tree of conditionals, stored in elimination order.
|
||||
*
|
||||
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
class BayesNet : public FactorGraph<CONDITIONAL> {
|
||||
|
||||
private:
|
||||
|
||||
typedef FactorGraph<CONDITIONAL> Base;
|
||||
|
||||
public:
|
||||
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
|
||||
|
||||
protected:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor as an empty BayesNet */
|
||||
BayesNet() {};
|
||||
|
||||
/** Construct from iterator over conditionals */
|
||||
template<typename ITERATOR>
|
||||
BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print out graph */
|
||||
void print(const std::string& s = "BayesNet",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,804 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree-inl.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
* @author Michael Kaess
|
||||
* @author Viorela Ila
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/inference/GenericSequentialSolver.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using boost::assign::operator+=;
|
||||
#include <boost/format.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
|
||||
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const {
|
||||
CliqueData data;
|
||||
getCliqueData(data, root_);
|
||||
return data;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
|
||||
data.conditionalSizes.push_back((*clique)->nrFrontals());
|
||||
data.separatorSizes.push_back((*clique)->nrParents());
|
||||
BOOST_FOREACH(sharedClique c, clique->children_) {
|
||||
getCliqueData(data, c);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
|
||||
return (root_) ? root_->numCachedSeparatorMarginals() : 0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const {
|
||||
if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
|
||||
std::ofstream of(s.c_str());
|
||||
of<< "digraph G{\n";
|
||||
saveGraph(of, root_, indexFormatter);
|
||||
of<<"}";
|
||||
of.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
|
||||
static int num = 0;
|
||||
bool first = true;
|
||||
std::stringstream out;
|
||||
out << num;
|
||||
std::string parent = out.str();
|
||||
parent += "[label=\"";
|
||||
|
||||
BOOST_FOREACH(Index index, clique->conditional_->frontals()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(index);
|
||||
}
|
||||
|
||||
if( clique != root_){
|
||||
parent += " : ";
|
||||
s << parentnum << "->" << num << "\n";
|
||||
}
|
||||
|
||||
first = true;
|
||||
BOOST_FOREACH(Index sep, clique->conditional_->parents()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(sep);
|
||||
}
|
||||
parent += "\"];\n";
|
||||
s << parent;
|
||||
parentnum = num;
|
||||
|
||||
BOOST_FOREACH(sharedClique c, clique->children_) {
|
||||
num++;
|
||||
saveGraph(s, c, indexFormatter, parentnum);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
|
||||
std::cout << s
|
||||
<<"avg Conditional Size: " << avgConditionalSize << std::endl
|
||||
<< "max Conditional Size: " << maxConditionalSize << std::endl
|
||||
<< "avg Separator Size: " << avgSeparatorSize << std::endl
|
||||
<< "max Separator Size: " << maxSeparatorSize << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
|
||||
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
|
||||
CliqueStats stats;
|
||||
|
||||
double sum = 0.0;
|
||||
size_t max = 0;
|
||||
BOOST_FOREACH(size_t s, conditionalSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
|
||||
stats.maxConditionalSize = max;
|
||||
|
||||
sum = 0.0;
|
||||
max = 1;
|
||||
BOOST_FOREACH(size_t s, separatorSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
|
||||
stats.maxSeparatorSize = max;
|
||||
|
||||
return stats;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
|
||||
std::cout << s << ":\n";
|
||||
BOOST_FOREACH(sharedClique clique, *this)
|
||||
clique->printTree("", indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
|
||||
return other == *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
|
||||
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
addClique(new_clique, parent_clique);
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
|
||||
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
|
||||
BOOST_FOREACH(Index j, (*clique)->frontals())
|
||||
nodes_[j] = clique;
|
||||
if (parent_clique != NULL) {
|
||||
clique->parent_ = parent_clique;
|
||||
parent_clique->children_.push_back(clique);
|
||||
} else {
|
||||
assert(!root_);
|
||||
root_ = clique;
|
||||
}
|
||||
clique->assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
|
||||
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
|
||||
BOOST_FOREACH(Index j, conditional->frontals())
|
||||
nodes_[j] = new_clique;
|
||||
new_clique->children_ = child_cliques;
|
||||
BOOST_FOREACH(sharedClique& child, child_cliques)
|
||||
child->parent_ = new_clique;
|
||||
new_clique->assertInvariants();
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
// recursively permute the cliques and internal conditionals
|
||||
if (root_)
|
||||
root_->permuteWithInverse(inversePermutation);
|
||||
|
||||
// need to know what the largest key is to get the right number of cliques
|
||||
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
|
||||
|
||||
// Update the nodes structure
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1);
|
||||
// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation
|
||||
for(size_t i = 0; i < nodes_.size(); ++i)
|
||||
newNodes[inversePermutation[i]] = nodes_[i];
|
||||
|
||||
nodes_ = newNodes;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
|
||||
static const bool debug = false;
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
// Debug check to make sure the conditional variable is ordered lower than
|
||||
// its parents and that all of its parents are present either in this
|
||||
// clique or its separator.
|
||||
BOOST_FOREACH(Index parent, conditional->parents()) {
|
||||
assert(parent > conditional->lastFrontalKey());
|
||||
const Clique& cliquer(*clique);
|
||||
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
|
||||
}
|
||||
#endif
|
||||
if(debug) conditional->print("Adding conditional ");
|
||||
if(debug) clique->print("To clique ");
|
||||
Index j = conditional->lastFrontalKey();
|
||||
bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size()));
|
||||
bayesTree.nodes_[j] = clique;
|
||||
FastVector<Index> newIndices((*clique)->size() + 1);
|
||||
newIndices[0] = j;
|
||||
std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1);
|
||||
clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
|
||||
if(debug) clique->print("Expanded clique is ");
|
||||
clique->assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
|
||||
|
||||
if (clique->isRoot())
|
||||
root_.reset();
|
||||
else { // detach clique from parent
|
||||
sharedClique parent = clique->parent_.lock();
|
||||
typename FastList<typename CLIQUE::shared_ptr>::iterator child = std::find(parent->children().begin(), parent->children().end(), clique);
|
||||
assert(child != parent->children().end());
|
||||
parent->children().erase(child);
|
||||
}
|
||||
|
||||
// orphan my children
|
||||
BOOST_FOREACH(sharedClique child, clique->children_)
|
||||
child->parent_ = typename Clique::weak_ptr();
|
||||
|
||||
BOOST_FOREACH(Index j, clique->conditional()->frontals()) {
|
||||
nodes_[j].reset();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
|
||||
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent) {
|
||||
|
||||
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a
|
||||
// symbolic tree, used in the BT(BN) constructor.
|
||||
|
||||
// Build the current clique
|
||||
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
|
||||
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
|
||||
cliqueConditionals.push_back(conditionals[j]); }
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
|
||||
|
||||
// Add the new clique with the current parent
|
||||
this->addClique(thisClique, parent);
|
||||
|
||||
// Build the children, whose parent is the new clique
|
||||
BOOST_FOREACH(const BayesTree<IndexConditional>::sharedClique& child, symbolic->children()) {
|
||||
this->recursiveTreeBuild(child, conditionals, thisClique); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
// First generate symbolic BT to determine clique structure
|
||||
BayesTree<IndexConditional> sbt(bayesNet);
|
||||
|
||||
// Build index of variables to conditionals
|
||||
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
|
||||
BOOST_FOREACH(const boost::shared_ptr<CONDITIONAL>& c, bayesNet) {
|
||||
if(c->nrFrontals() != 1)
|
||||
throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals");
|
||||
if(c->firstFrontalKey() >= conditionals.size())
|
||||
throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!");
|
||||
if(conditionals[c->firstFrontalKey()])
|
||||
throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!");
|
||||
|
||||
conditionals[c->firstFrontalKey()] = c;
|
||||
}
|
||||
|
||||
// Build the new tree
|
||||
this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<>
|
||||
inline BayesTree<IndexConditional>::BayesTree(const BayesNet<IndexConditional>& bayesNet) {
|
||||
BayesNet<IndexConditional>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
insert(*this, *rit);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees) {
|
||||
if (bayesNet.size() == 0)
|
||||
throw std::invalid_argument("BayesTree::insert: empty bayes net!");
|
||||
|
||||
// get the roots of child subtrees and merge their nodes_
|
||||
std::list<sharedClique> childRoots;
|
||||
typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
|
||||
BOOST_FOREACH(const Tree& subtree, subtrees) {
|
||||
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
|
||||
childRoots.push_back(subtree.root());
|
||||
}
|
||||
|
||||
// create a new clique and add all the conditionals to the clique
|
||||
sharedClique new_clique;
|
||||
typename BayesNet<CONDITIONAL>::sharedConditional conditional;
|
||||
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
|
||||
if (!new_clique.get())
|
||||
new_clique = addClique(conditional,childRoots);
|
||||
else
|
||||
addToCliqueFront(*this, conditional, new_clique);
|
||||
}
|
||||
|
||||
root_ = new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) {
|
||||
this->clear();
|
||||
other.cloneTo(*this);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
|
||||
if (root_.use_count() == 0) {
|
||||
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl;
|
||||
return;
|
||||
}
|
||||
std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl;
|
||||
if (nodes_.empty()) return;
|
||||
root_->printTree("", indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// binary predicate to test equality of a pair for use in equals
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool check_sharedCliques(
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
|
||||
) {
|
||||
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other,
|
||||
double tol) const {
|
||||
return size()==other.size() &&
|
||||
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
inline Index BayesTree<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const {
|
||||
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
|
||||
assert(lowestOrderedParent != parents.end());
|
||||
return *lowestOrderedParent;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
|
||||
{
|
||||
static const bool debug = false;
|
||||
|
||||
// get indices and parents
|
||||
const typename CONDITIONAL::Parents& parents = conditional->parents();
|
||||
|
||||
if(debug) conditional->print("Adding conditional ");
|
||||
|
||||
// if no parents, start a new root clique
|
||||
if (parents.empty()) {
|
||||
if(debug) std::cout << "No parents so making root" << std::endl;
|
||||
bayesTree.root_ = bayesTree.addClique(conditional);
|
||||
return;
|
||||
}
|
||||
|
||||
// otherwise, find the parent clique by using the index data structure
|
||||
// to find the lowest-ordered parent
|
||||
Index parentRepresentative = bayesTree.findParentClique(parents);
|
||||
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
|
||||
sharedClique parent_clique = bayesTree[parentRepresentative];
|
||||
if(debug) parent_clique->print("Parent clique is ");
|
||||
|
||||
// if the parents and parent clique have the same size, add to parent clique
|
||||
if ((*parent_clique)->size() == size_t(parents.size())) {
|
||||
if(debug) std::cout << "Adding to parent clique" << std::endl;
|
||||
addToCliqueFront(bayesTree, conditional, parent_clique);
|
||||
} else {
|
||||
if(debug) std::cout << "Starting new clique" << std::endl;
|
||||
// otherwise, start a new clique and add it to the tree
|
||||
bayesTree.addClique(conditional,parent_clique);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TODO: remove this function after removing TSAM.cpp
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
|
||||
const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) {
|
||||
|
||||
if (clique->nrFrontals() == 0)
|
||||
throw std::invalid_argument("BayesTree::insert: empty clique!");
|
||||
|
||||
// create a new clique and add all the conditionals to the clique
|
||||
sharedClique new_clique = addClique(clique, children);
|
||||
if (isRootClique) root_ = new_clique;
|
||||
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
|
||||
// Add each frontal variable of this root node
|
||||
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
|
||||
// Fill index for each child
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, subtree->children_) {
|
||||
fillNodesIndex(child); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
|
||||
if(subtree) {
|
||||
// Find the parent clique of the new subtree. By the running intersection
|
||||
// property, those separator variables in the subtree that are ordered
|
||||
// lower than the highest frontal variable of the subtree root will all
|
||||
// appear in the separator of the subtree root.
|
||||
if(subtree->conditional()->parents().empty()) {
|
||||
assert(!root_);
|
||||
root_ = subtree;
|
||||
} else {
|
||||
Index parentRepresentative = findParentClique(subtree->conditional()->parents());
|
||||
sharedClique parent = (*this)[parentRepresentative];
|
||||
parent->children_ += subtree;
|
||||
subtree->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
// Now fill in the nodes index
|
||||
if(nodes_.size() == 0 ||
|
||||
*std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) {
|
||||
nodes_.resize(subtree->conditional()->lastFrontalKey() + 1);
|
||||
}
|
||||
fillNodesIndex(subtree);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// First finds clique marginal then marginalizes that
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor(
|
||||
Index j, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTree_marginalFactor);
|
||||
|
||||
// get clique containing Index j
|
||||
sharedClique clique = (*this)[j];
|
||||
|
||||
// calculate or retrieve its marginal P(C) = P(F,S)
|
||||
#ifdef OLD_SHORTCUT_MARGINALS
|
||||
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
|
||||
#else
|
||||
FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function);
|
||||
#endif
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
gttoc(Reduce);
|
||||
|
||||
// now, marginalize out everything that is not variable j
|
||||
GenericSequentialSolver<FactorType> solver(cliqueMarginal);
|
||||
boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
result->permuteWithInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet(
|
||||
Index j, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTree_marginalBayesNet);
|
||||
|
||||
// calculate marginal as a factor graph
|
||||
FactorGraph<FactorType> fg;
|
||||
fg.push_back(this->marginalFactor(j,function));
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(fg.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
gttoc(Reduce);
|
||||
|
||||
// eliminate factor graph marginal to a Bayes net
|
||||
boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
bn->permuteWithInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Find two cliques, their joint, then marginalizes
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
|
||||
BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
|
||||
gttic(BayesTree_joint);
|
||||
|
||||
// get clique C1 and C2
|
||||
sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
|
||||
|
||||
gttic(Lowest_common_ancestor);
|
||||
// Find lowest common ancestor clique
|
||||
sharedClique B; {
|
||||
// Build two paths to the root
|
||||
FastList<sharedClique> path1, path2; {
|
||||
sharedClique p = C1;
|
||||
while(p) {
|
||||
path1.push_front(p);
|
||||
p = p->parent();
|
||||
}
|
||||
} {
|
||||
sharedClique p = C2;
|
||||
while(p) {
|
||||
path2.push_front(p);
|
||||
p = p->parent();
|
||||
}
|
||||
}
|
||||
// Find the path intersection
|
||||
B = this->root();
|
||||
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
|
||||
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
|
||||
B = *p1;
|
||||
++p1;
|
||||
++p2;
|
||||
}
|
||||
}
|
||||
gttoc(Lowest_common_ancestor);
|
||||
|
||||
// Compute marginal on lowest common ancestor clique
|
||||
gttic(LCA_marginal);
|
||||
FactorGraph<FactorType> p_B = B->marginal2(this->root(), function);
|
||||
gttoc(LCA_marginal);
|
||||
|
||||
// Compute shortcuts of the requested cliques given the lowest common ancestor
|
||||
gttic(Clique_shortcuts);
|
||||
BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
|
||||
BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
|
||||
gttoc(Clique_shortcuts);
|
||||
|
||||
// Factor the shortcuts to be conditioned on the full root
|
||||
// Get the set of variables to eliminate, which is C1\B.
|
||||
gttic(Full_root_factoring);
|
||||
sharedConditional p_C1_B; {
|
||||
std::vector<Index> C1_minus_B; {
|
||||
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index j, *B->conditional()) {
|
||||
C1_minus_B_set.erase(j); }
|
||||
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
|
||||
}
|
||||
// Factor into C1\B | B.
|
||||
FactorGraph<FactorType> temp_remaining;
|
||||
boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
|
||||
}
|
||||
sharedConditional p_C2_B; {
|
||||
std::vector<Index> C2_minus_B; {
|
||||
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index j, *B->conditional()) {
|
||||
C2_minus_B_set.erase(j); }
|
||||
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
|
||||
}
|
||||
// Factor into C2\B | B.
|
||||
FactorGraph<FactorType> temp_remaining;
|
||||
boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
|
||||
}
|
||||
gttoc(Full_root_factoring);
|
||||
|
||||
gttic(Variable_joint);
|
||||
// Build joint on all involved variables
|
||||
FactorGraph<FactorType> p_BC1C2;
|
||||
p_BC1C2.push_back(p_B);
|
||||
p_BC1C2.push_back(p_C1_B->toFactor());
|
||||
p_BC1C2.push_back(p_C2_B->toFactor());
|
||||
if(C1 != B)
|
||||
p_BC1C2.push_back(C1->conditional()->toFactor());
|
||||
if(C2 != B)
|
||||
p_BC1C2.push_back(C2->conditional()->toFactor());
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
|
||||
gttoc(Reduce);
|
||||
|
||||
// now, marginalize out everything that is not variable j
|
||||
GenericSequentialSolver<FactorType> solver(p_BC1C2);
|
||||
boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, *result) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return result;
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet(
|
||||
Index j1, Index j2, Eliminate function) const {
|
||||
|
||||
// eliminate factor graph marginal to a Bayes net
|
||||
return GenericSequentialSolver<FactorType> (
|
||||
*this->joint(j1, j2, function)).eliminate(function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::clear() {
|
||||
// Remove all nodes and clear the root pointer
|
||||
nodes_.clear();
|
||||
root_.reset();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// base case is NULL, if so we do nothing and return empties above
|
||||
if (clique!=NULL) {
|
||||
|
||||
// remove the clique from orphans in case it has been added earlier
|
||||
orphans.remove(clique);
|
||||
|
||||
// remove me
|
||||
this->removeClique(clique);
|
||||
|
||||
// remove path above me
|
||||
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
|
||||
|
||||
// add children to list of orphans (splice also removed them from clique->children_)
|
||||
orphans.splice(orphans.begin(), clique->children_);
|
||||
|
||||
bn.push_back(clique->conditional());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Index& j, keys) {
|
||||
|
||||
// get the clique
|
||||
if(j < nodes_.size()) {
|
||||
const sharedClique& clique(nodes_[j]);
|
||||
if(clique) {
|
||||
// remove path from clique to root
|
||||
this->removePath(clique, bn, orphans);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Delete cachedShortcuts for each orphan subtree
|
||||
//TODO: Consider Improving
|
||||
BOOST_FOREACH(sharedClique& orphan, orphans)
|
||||
orphan->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,CLIQUE>::removeSubtree(
|
||||
const sharedClique& subtree)
|
||||
{
|
||||
// Result clique list
|
||||
Cliques cliques;
|
||||
cliques.push_back(subtree);
|
||||
|
||||
// Remove the first clique from its parents
|
||||
if(!subtree->isRoot())
|
||||
subtree->parent()->children().remove(subtree);
|
||||
else
|
||||
root_.reset();
|
||||
|
||||
// Add all subtree cliques and erase the children and parent of each
|
||||
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
|
||||
{
|
||||
// Add children
|
||||
BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
|
||||
cliques.push_back(child); }
|
||||
|
||||
// Delete cached shortcuts
|
||||
(*clique)->deleteCachedShortcutsNonRecursive();
|
||||
|
||||
// Remove this node from the nodes index
|
||||
BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) {
|
||||
nodes_[j].reset(); }
|
||||
|
||||
// Erase the parent and children pointers
|
||||
(*clique)->parent_.reset();
|
||||
(*clique)->children_.clear();
|
||||
}
|
||||
|
||||
return cliques;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
|
||||
if(root())
|
||||
cloneTo(newTree, root(), sharedClique());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(
|
||||
This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
|
||||
sharedClique newClique(subtree->clone());
|
||||
newTree.addClique(newClique, parent);
|
||||
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
|
||||
cloneTo(newTree, childClique, newClique);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,509 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree-inl.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
* @author Michael Kaess
|
||||
* @author Viorela Ila
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <fstream>
|
||||
|
||||
using boost::assign::cref_list_of;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
|
||||
BayesTreeCliqueData data;
|
||||
BOOST_FOREACH(const sharedClique& root, roots_)
|
||||
getCliqueData(data, root);
|
||||
return data;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
|
||||
data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
|
||||
data.separatorSizes.push_back(clique->conditional()->nrParents());
|
||||
BOOST_FOREACH(sharedClique c, clique->children) {
|
||||
getCliqueData(data, c);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(const sharedClique& root, roots_)
|
||||
count += root->numCachedSeparatorMarginals();
|
||||
return count;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const {
|
||||
if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
|
||||
std::ofstream of(s.c_str());
|
||||
of<< "digraph G{\n";
|
||||
BOOST_FOREACH(const sharedClique& root, roots_)
|
||||
saveGraph(of, root, keyFormatter);
|
||||
of<<"}";
|
||||
of.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const {
|
||||
static int num = 0;
|
||||
bool first = true;
|
||||
std::stringstream out;
|
||||
out << num;
|
||||
std::string parent = out.str();
|
||||
parent += "[label=\"";
|
||||
|
||||
BOOST_FOREACH(Key index, clique->conditional_->frontals()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(index);
|
||||
}
|
||||
|
||||
if(clique->parent()){
|
||||
parent += " : ";
|
||||
s << parentnum << "->" << num << "\n";
|
||||
}
|
||||
|
||||
first = true;
|
||||
BOOST_FOREACH(Key sep, clique->conditional_->parents()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(sep);
|
||||
}
|
||||
parent += "\"];\n";
|
||||
s << parent;
|
||||
parentnum = num;
|
||||
|
||||
BOOST_FOREACH(sharedClique c, clique->children) {
|
||||
num++;
|
||||
saveGraph(s, c, indexFormatter, parentnum);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
size_t BayesTree<CLIQUE>::size() const {
|
||||
size_t size = 0;
|
||||
BOOST_FOREACH(const sharedClique& clique, roots_)
|
||||
size += clique->treeSize();
|
||||
return size;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
|
||||
BOOST_FOREACH(Index j, clique->conditional()->frontals())
|
||||
nodes_[j] = clique;
|
||||
if (parent_clique != NULL) {
|
||||
clique->parent_ = parent_clique;
|
||||
parent_clique->children.push_back(clique);
|
||||
} else {
|
||||
roots_.push_back(clique);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: Clean up
|
||||
namespace {
|
||||
template<class FACTOR, class CLIQUE>
|
||||
int _pushClique(FactorGraph<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {
|
||||
fg.push_back(clique->conditional_);
|
||||
return 0;
|
||||
}
|
||||
|
||||
template<class FACTOR, class CLIQUE>
|
||||
struct _pushCliqueFunctor {
|
||||
_pushCliqueFunctor(FactorGraph<FACTOR>& graph_) : graph(graph_) {}
|
||||
FactorGraph<FACTOR>& graph;
|
||||
int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
|
||||
graph.push_back(clique->conditional_);
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::addFactorsToGraph(FactorGraph<FactorType>& graph) const
|
||||
{
|
||||
// Traverse the BayesTree and add all conditionals to this graph
|
||||
int data = 0; // Unused
|
||||
_pushCliqueFunctor<FactorType,CLIQUE> functor(graph);
|
||||
treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works?
|
||||
// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, boost::ref(graph), _1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
BayesTree<CLIQUE>::BayesTree(const This& other) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
template<typename NODE>
|
||||
boost::shared_ptr<NODE>
|
||||
BayesTreeCloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
|
||||
{
|
||||
// Clone the current node and add it to its cloned parent
|
||||
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
|
||||
clone->children.clear();
|
||||
clone->parent_ = parentPointer;
|
||||
parentPointer->children.push_back(clone);
|
||||
return clone;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
|
||||
this->clear();
|
||||
boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
|
||||
treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
|
||||
BOOST_FOREACH(const sharedClique& root, rootContainer->children) {
|
||||
root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
|
||||
insertRoot(root);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
|
||||
treeTraversal::PrintForest(*this, s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// binary predicate to test equality of a pair for use in equals
|
||||
template<class CLIQUE>
|
||||
bool check_sharedCliques(
|
||||
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
|
||||
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
|
||||
) {
|
||||
return v1.first == v2.first &&
|
||||
((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
|
||||
return size()==other.size() &&
|
||||
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
Key BayesTree<CLIQUE>::findParentClique(const CONTAINER& parents) const {
|
||||
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
|
||||
assert(lowestOrderedParent != parents.end());
|
||||
return *lowestOrderedParent;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
|
||||
// Add each frontal variable of this root node
|
||||
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) {
|
||||
bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
|
||||
assert(inserted); (void)inserted;
|
||||
}
|
||||
// Fill index for each child
|
||||
typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, subtree->children) {
|
||||
fillNodesIndex(child); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
|
||||
roots_.push_back(subtree); // Add to roots
|
||||
fillNodesIndex(subtree); // Populate nodes index
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// First finds clique marginal then marginalizes that
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::sharedConditional
|
||||
BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
|
||||
{
|
||||
gttic(BayesTree_marginalFactor);
|
||||
|
||||
// get clique containing Index j
|
||||
sharedClique clique = this->clique(j);
|
||||
|
||||
// calculate or retrieve its marginal P(C) = P(F,S)
|
||||
FactorGraphType cliqueMarginal = clique->marginal2(function);
|
||||
|
||||
// Now, marginalize out everything that is not variable j
|
||||
BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet(
|
||||
Ordering(cref_list_of<1,Key>(j)), boost::none, function);
|
||||
|
||||
// The Bayes net should contain only one conditional for variable j, so return it
|
||||
return marginalBN.front();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Find two cliques, their joint, then marginalizes
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::sharedFactorGraph
|
||||
BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
|
||||
{
|
||||
gttic(BayesTree_joint);
|
||||
return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::sharedBayesNet
|
||||
BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
|
||||
{
|
||||
gttic(BayesTree_jointBayesNet);
|
||||
// get clique C1 and C2
|
||||
sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
|
||||
|
||||
gttic(Lowest_common_ancestor);
|
||||
// Find lowest common ancestor clique
|
||||
sharedClique B; {
|
||||
// Build two paths to the root
|
||||
FastList<sharedClique> path1, path2; {
|
||||
sharedClique p = C1;
|
||||
while(p) {
|
||||
path1.push_front(p);
|
||||
p = p->parent();
|
||||
}
|
||||
} {
|
||||
sharedClique p = C2;
|
||||
while(p) {
|
||||
path2.push_front(p);
|
||||
p = p->parent();
|
||||
}
|
||||
}
|
||||
// Find the path intersection
|
||||
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
|
||||
if(*p1 == *p2)
|
||||
B = *p1;
|
||||
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
|
||||
B = *p1;
|
||||
++p1;
|
||||
++p2;
|
||||
}
|
||||
}
|
||||
if(!B)
|
||||
throw std::invalid_argument("BayesTree::jointBayesNet does not yet work for joints across a forest");
|
||||
gttoc(Lowest_common_ancestor);
|
||||
|
||||
// Compute marginal on lowest common ancestor clique
|
||||
gttic(LCA_marginal);
|
||||
FactorGraphType p_B = B->marginal2(function);
|
||||
gttoc(LCA_marginal);
|
||||
|
||||
// Compute shortcuts of the requested cliques given the lowest common ancestor
|
||||
gttic(Clique_shortcuts);
|
||||
BayesNetType p_C1_Bred = C1->shortcut(B, function);
|
||||
BayesNetType p_C2_Bred = C2->shortcut(B, function);
|
||||
gttoc(Clique_shortcuts);
|
||||
|
||||
// Factor the shortcuts to be conditioned on the full root
|
||||
// Get the set of variables to eliminate, which is C1\B.
|
||||
gttic(Full_root_factoring);
|
||||
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
|
||||
std::vector<Index> C1_minus_B; {
|
||||
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index j, *B->conditional()) {
|
||||
C1_minus_B_set.erase(j); }
|
||||
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
|
||||
}
|
||||
// Factor into C1\B | B.
|
||||
sharedFactorGraph temp_remaining;
|
||||
boost::tie(p_C1_B, temp_remaining) =
|
||||
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
|
||||
}
|
||||
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
|
||||
std::vector<Index> C2_minus_B; {
|
||||
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index j, *B->conditional()) {
|
||||
C2_minus_B_set.erase(j); }
|
||||
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
|
||||
}
|
||||
// Factor into C2\B | B.
|
||||
sharedFactorGraph temp_remaining;
|
||||
boost::tie(p_C2_B, temp_remaining) =
|
||||
FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
|
||||
}
|
||||
gttoc(Full_root_factoring);
|
||||
|
||||
gttic(Variable_joint);
|
||||
// Build joint on all involved variables
|
||||
FactorGraphType p_BC1C2;
|
||||
p_BC1C2 += p_B;
|
||||
p_BC1C2 += *p_C1_B;
|
||||
p_BC1C2 += *p_C2_B;
|
||||
if(C1 != B)
|
||||
p_BC1C2 += C1->conditional();
|
||||
if(C2 != B)
|
||||
p_BC1C2 += C2->conditional();
|
||||
|
||||
// now, marginalize out everything that is not variable j1 or j2
|
||||
return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::clear() {
|
||||
// Remove all nodes and clear the root pointer
|
||||
nodes_.clear();
|
||||
roots_.clear();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::deleteCachedShortcuts() {
|
||||
BOOST_FOREACH(const sharedClique& root, roots_) {
|
||||
root->deleteCachedShortcuts();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::removeClique(sharedClique clique)
|
||||
{
|
||||
if (clique->isRoot()) {
|
||||
typename std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
|
||||
if(root != roots_.end())
|
||||
roots_.erase(root);
|
||||
} else { // detach clique from parent
|
||||
sharedClique parent = clique->parent_.lock();
|
||||
typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
|
||||
assert(child != parent->children.end());
|
||||
parent->children.erase(child);
|
||||
}
|
||||
|
||||
// orphan my children
|
||||
BOOST_FOREACH(sharedClique child, clique->children)
|
||||
child->parent_ = typename Clique::weak_ptr();
|
||||
|
||||
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
|
||||
nodes_.erase(j);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
|
||||
{
|
||||
// base case is NULL, if so we do nothing and return empties above
|
||||
if (clique) {
|
||||
|
||||
// remove the clique from orphans in case it has been added earlier
|
||||
orphans.remove(clique);
|
||||
|
||||
// remove me
|
||||
this->removeClique(clique);
|
||||
|
||||
// remove path above me
|
||||
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
|
||||
|
||||
// add children to list of orphans (splice also removed them from clique->children_)
|
||||
orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end());
|
||||
clique->children.clear();
|
||||
|
||||
bn.push_back(clique->conditional_);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans)
|
||||
{
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Key& j, keys)
|
||||
{
|
||||
// get the clique
|
||||
// TODO: Nodes will be searched again in removeClique
|
||||
typename Nodes::const_iterator node = nodes_.find(j);
|
||||
if(node != nodes_.end()) {
|
||||
// remove path from clique to root
|
||||
this->removePath(node->second, bn, orphans);
|
||||
}
|
||||
}
|
||||
|
||||
// Delete cachedShortcuts for each orphan subtree
|
||||
//TODO: Consider Improving
|
||||
BOOST_FOREACH(sharedClique& orphan, orphans)
|
||||
orphan->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
|
||||
const sharedClique& subtree)
|
||||
{
|
||||
// Result clique list
|
||||
Cliques cliques;
|
||||
cliques.push_back(subtree);
|
||||
|
||||
// Remove the first clique from its parents
|
||||
if(!subtree->isRoot())
|
||||
subtree->parent()->children.erase(std::find(
|
||||
subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
|
||||
else
|
||||
roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
|
||||
|
||||
// Add all subtree cliques and erase the children and parent of each
|
||||
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
|
||||
{
|
||||
// Add children
|
||||
BOOST_FOREACH(const sharedClique& child, (*clique)->children) {
|
||||
cliques.push_back(child); }
|
||||
|
||||
// Delete cached shortcuts
|
||||
(*clique)->deleteCachedShortcutsNonRecursive();
|
||||
|
||||
// Remove this node from the nodes index
|
||||
BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
|
||||
nodes_.erase(j); }
|
||||
|
||||
// Erase the parent and children pointers
|
||||
(*clique)->parent_.reset();
|
||||
(*clique)->children.clear();
|
||||
}
|
||||
|
||||
return cliques;
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree.cpp
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
* @author Michael Kaess
|
||||
* @author Viorela Ila
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BayesTreeCliqueStats::print(const std::string& s) const {
|
||||
std::cout << s
|
||||
<< "avg Conditional Size: " << avgConditionalSize << std::endl
|
||||
<< "max Conditional Size: " << maxConditionalSize << std::endl
|
||||
<< "avg Separator Size: " << avgSeparatorSize << std::endl
|
||||
<< "max Separator Size: " << maxSeparatorSize << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
|
||||
{
|
||||
BayesTreeCliqueStats stats;
|
||||
|
||||
double sum = 0.0;
|
||||
size_t max = 0;
|
||||
BOOST_FOREACH(size_t s, conditionalSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
|
||||
stats.maxConditionalSize = max;
|
||||
|
||||
sum = 0.0;
|
||||
max = 1;
|
||||
BOOST_FOREACH(size_t s, separatorSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
|
||||
stats.maxSeparatorSize = max;
|
||||
|
||||
return stats;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,418 +1,281 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
#include <deque>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
|
||||
template<class CONDITIONAL> struct BayesTreeClique;
|
||||
|
||||
/**
|
||||
* Bayes tree
|
||||
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
|
||||
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
|
||||
* @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
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
|
||||
class BayesTree {
|
||||
|
||||
public:
|
||||
|
||||
typedef BayesTree<CONDITIONAL, CLIQUE> This;
|
||||
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
|
||||
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
|
||||
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
|
||||
typedef CONDITIONAL ConditionalType;
|
||||
typedef typename CONDITIONAL::FactorType FactorType;
|
||||
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
|
||||
|
||||
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
|
||||
|
||||
// typedef for shared pointers to cliques
|
||||
typedef boost::shared_ptr<Clique> sharedClique;
|
||||
|
||||
// A convenience class for a list of shared cliques
|
||||
struct Cliques : public FastList<sharedClique> {
|
||||
void print(const std::string& s = "Cliques",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
|
||||
bool equals(const Cliques& other, double tol = 1e-9) const;
|
||||
};
|
||||
|
||||
/** clique statistics */
|
||||
struct CliqueStats {
|
||||
double avgConditionalSize;
|
||||
std::size_t maxConditionalSize;
|
||||
double avgSeparatorSize;
|
||||
std::size_t maxSeparatorSize;
|
||||
void print(const std::string& s = "") const ;
|
||||
};
|
||||
|
||||
/** store all the sizes */
|
||||
struct CliqueData {
|
||||
std::vector<std::size_t> conditionalSizes;
|
||||
std::vector<std::size_t> separatorSizes;
|
||||
CliqueStats getStats() const;
|
||||
};
|
||||
|
||||
/** Map from indices to Clique */
|
||||
typedef std::vector<sharedClique> Nodes;
|
||||
|
||||
protected:
|
||||
|
||||
/** Map from indices to Clique */
|
||||
Nodes nodes_;
|
||||
|
||||
/** Root clique */
|
||||
sharedClique root_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
BayesTree() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
|
||||
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
/** Copy constructor */
|
||||
BayesTree(const This& other);
|
||||
|
||||
/** Assignment operator */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
|
||||
* new root clique and the subtrees are connected to the root clique.
|
||||
*/
|
||||
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~BayesTree() {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** check equality */
|
||||
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Find parent clique of a conditional. It will look at all parents and
|
||||
* return the one with the lowest index in the ordering.
|
||||
*/
|
||||
template<class CONTAINER>
|
||||
Index findParentClique(const CONTAINER& parents) const;
|
||||
|
||||
/** number of cliques */
|
||||
inline size_t size() const {
|
||||
if(root_)
|
||||
return root_->treeSize();
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** number of nodes */
|
||||
inline size_t nrNodes() const { return nodes_.size(); }
|
||||
|
||||
/** Check if there are any cliques in the tree */
|
||||
inline bool empty() const {
|
||||
return nodes_.empty();
|
||||
}
|
||||
|
||||
/** return nodes */
|
||||
const Nodes& nodes() const { return nodes_; }
|
||||
|
||||
/** return root clique */
|
||||
const sharedClique& root() const { return root_; }
|
||||
|
||||
/** find the clique that contains the variable with Index j */
|
||||
inline sharedClique operator[](Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
|
||||
inline sharedClique clique(Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
/** Gather data on all cliques */
|
||||
CliqueData getCliqueData() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
||||
/** return marginal on any variable */
|
||||
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return marginal on any variable, as a Bayes Net
|
||||
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
|
||||
* This is more expensive than the above function
|
||||
*/
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
typename FactorGraph<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables as a BayesNet
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Read only with side effects
|
||||
*/
|
||||
|
||||
/** saves the Tree to a text file in GraphViz format */
|
||||
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Access the root clique (non-const version) */
|
||||
sharedClique& root() { return root_; }
|
||||
|
||||
/** Access the nodes (non-cost version) */
|
||||
Nodes& nodes() { return nodes_; }
|
||||
|
||||
/** Remove all nodes */
|
||||
void clear();
|
||||
|
||||
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
|
||||
void deleteCachedShortcuts() {
|
||||
root_->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
/** Apply a permutation to the full tree - also updates the nodes structure */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/**
|
||||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
*/
|
||||
void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
|
||||
* Factors and orphans are added to the in/out arguments.
|
||||
*/
|
||||
template<class CONTAINER>
|
||||
void removeTop(const CONTAINER& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Remove the requested subtree. */
|
||||
Cliques removeSubtree(const sharedClique& subtree);
|
||||
|
||||
/**
|
||||
* Hang a new subtree off of the existing tree. This finds the appropriate
|
||||
* parent clique for the subtree (which may be the root), and updates the
|
||||
* nodes index with the new cliques in the subtree. None of the frontal
|
||||
* variables in the subtree may appear in the separators of the existing
|
||||
* BayesTree.
|
||||
*/
|
||||
void insert(const sharedClique& subtree);
|
||||
|
||||
/** Insert a new conditional
|
||||
* This function only applies for Symbolic case with IndexCondtional,
|
||||
* We make it static so that it won't be compiled in GaussianConditional case.
|
||||
* */
|
||||
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
|
||||
|
||||
/**
|
||||
* Insert a new clique corresponding to the given Bayes net.
|
||||
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
|
||||
* i.e. all the variables (frontal and separator) are connected
|
||||
*/
|
||||
sharedClique insert(const sharedConditional& clique,
|
||||
std::list<sharedClique>& children, bool isRootClique = false);
|
||||
|
||||
/**
|
||||
* Create a clone of this object as a shared pointer
|
||||
* Necessary for inheritance in matlab interface
|
||||
*/
|
||||
virtual shared_ptr clone() const {
|
||||
return shared_ptr(new This(*this));
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/** private helper method for saving the Tree to a text file in GraphViz format */
|
||||
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
|
||||
int parentnum = 0) const;
|
||||
|
||||
/** Gather data on a single clique */
|
||||
void getCliqueData(CliqueData& stats, sharedClique clique) const;
|
||||
|
||||
/** remove a clique: warning, can result in a forest */
|
||||
void removeClique(sharedClique clique);
|
||||
|
||||
/** add a clique (top down) */
|
||||
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
|
||||
|
||||
/** add a clique (top down) */
|
||||
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
|
||||
|
||||
/** add a clique (bottom up) */
|
||||
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
|
||||
|
||||
/**
|
||||
* Add a conditional to the front of a clique, i.e. a conditional whose
|
||||
* parents are already in the clique or its separators. This function does
|
||||
* not check for this condition, it just updates the data structures.
|
||||
*/
|
||||
static void addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree,
|
||||
const sharedConditional& conditional, const sharedClique& clique);
|
||||
|
||||
/** Fill the nodes index for a subtree */
|
||||
void fillNodesIndex(const sharedClique& subtree);
|
||||
|
||||
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
|
||||
* symbolic tree, used in the BT(BN) constructor.
|
||||
*/
|
||||
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
|
||||
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
|
||||
|
||||
private:
|
||||
|
||||
/** deep copy to another tree */
|
||||
void cloneTo(This& newTree) const;
|
||||
|
||||
/** deep copy to another tree */
|
||||
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(nodes_);
|
||||
ar & BOOST_SERIALIZATION_NVP(root_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void _BayesTree_dim_adder(
|
||||
std::vector<size_t>& dims,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add dims from this clique
|
||||
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
|
||||
dims[*it] = (*clique)->dim(it);
|
||||
|
||||
// Traverse children
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children()) {
|
||||
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL,class CLIQUE>
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
|
||||
std::vector<size_t> dimensions(bt.nodes().size(), 0);
|
||||
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
|
||||
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* A Clique in the tree is an incomplete Bayes net: the variables
|
||||
* in the Bayes net are the frontal nodes, and the variables conditioned
|
||||
* on are the separator. We also have pointers up and down the tree.
|
||||
*
|
||||
* Since our Conditional class already handles multiple frontal variables,
|
||||
* this Clique contains exactly 1 conditional.
|
||||
*
|
||||
* This is the default clique type in a BayesTree, but some algorithms, like
|
||||
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
|
||||
* extra data along with the clique.
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
|
||||
public:
|
||||
typedef CONDITIONAL ConditionalType;
|
||||
typedef BayesTreeClique<CONDITIONAL> This;
|
||||
typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
BayesTreeClique() {}
|
||||
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
|
||||
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
|
||||
|
||||
private:
|
||||
/** 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);
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
#include <gtsam/inference/BayesTree-inl.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
template<class FACTOR> class FactorGraph;
|
||||
template<class BAYESTREE, class GRAPH> class JunctionTree;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** clique statistics */
|
||||
struct GTSAM_EXPORT BayesTreeCliqueStats {
|
||||
double avgConditionalSize;
|
||||
std::size_t maxConditionalSize;
|
||||
double avgSeparatorSize;
|
||||
std::size_t maxSeparatorSize;
|
||||
void print(const std::string& s = "") const ;
|
||||
};
|
||||
|
||||
/** store all the sizes */
|
||||
struct GTSAM_EXPORT BayesTreeCliqueData {
|
||||
std::vector<std::size_t> conditionalSizes;
|
||||
std::vector<std::size_t> separatorSizes;
|
||||
BayesTreeCliqueStats getStats() const;
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Bayes tree
|
||||
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
|
||||
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
|
||||
* @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
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CLIQUE>
|
||||
class BayesTree
|
||||
{
|
||||
protected:
|
||||
typedef BayesTree<CLIQUE> This;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
public:
|
||||
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
|
||||
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
|
||||
typedef Clique Node; ///< Synonym for Clique (TODO: remove)
|
||||
typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
|
||||
typedef typename CLIQUE::ConditionalType ConditionalType;
|
||||
typedef boost::shared_ptr<ConditionalType> sharedConditional;
|
||||
typedef typename CLIQUE::BayesNetType BayesNetType;
|
||||
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
|
||||
typedef typename CLIQUE::FactorType FactorType;
|
||||
typedef boost::shared_ptr<FactorType> sharedFactor;
|
||||
typedef typename CLIQUE::FactorGraphType FactorGraphType;
|
||||
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
|
||||
typedef typename FactorGraphType::Eliminate Eliminate;
|
||||
typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
|
||||
|
||||
/** A convenience class for a list of shared cliques */
|
||||
typedef FastList<sharedClique> Cliques;
|
||||
|
||||
/** Map from keys to Clique */
|
||||
typedef FastMap<Key, sharedClique> Nodes;
|
||||
|
||||
protected:
|
||||
|
||||
/** Map from indices to Clique */
|
||||
Nodes nodes_;
|
||||
|
||||
/** Root cliques */
|
||||
std::vector<sharedClique> roots_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
BayesTree() {}
|
||||
|
||||
/** Copy constructor */
|
||||
BayesTree(const This& other);
|
||||
|
||||
/// @}
|
||||
|
||||
/** Assignment operator */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** check equality */
|
||||
bool equals(const This& other, double tol = 1e-9) const;
|
||||
|
||||
public:
|
||||
/** print */
|
||||
void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
/// @}
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** number of cliques */
|
||||
size_t size() const;
|
||||
|
||||
/** Check if there are any cliques in the tree */
|
||||
inline bool empty() const {
|
||||
return nodes_.empty();
|
||||
}
|
||||
|
||||
/** return nodes */
|
||||
const Nodes& nodes() const { return nodes_; }
|
||||
|
||||
/** Access node by variable */
|
||||
const sharedNode operator[](Key j) const { return nodes_.at(j); }
|
||||
|
||||
/** return root cliques */
|
||||
const std::vector<sharedClique>& roots() const { return roots_; }
|
||||
|
||||
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
|
||||
const sharedClique& clique(Key j) const {
|
||||
typename Nodes::const_iterator c = nodes_.find(j);
|
||||
if(c == nodes_.end())
|
||||
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
|
||||
else
|
||||
return c->second;
|
||||
}
|
||||
|
||||
/** Gather data on all cliques */
|
||||
BayesTreeCliqueData getCliqueData() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
||||
/** Return marginal on any variable. Note that this actually returns a conditional, for which a
|
||||
* solution may be directly obtained by calling .solve() on the returned object.
|
||||
* Alternatively, it may be directly used as its factor base class. For example, for Gaussian
|
||||
* systems, this returns a GaussianConditional, which inherits from JacobianFactor and
|
||||
* GaussianFactor. */
|
||||
sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables as a BayesNet
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/**
|
||||
* Read only with side effects
|
||||
*/
|
||||
|
||||
/** saves the Tree to a text file in GraphViz format */
|
||||
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Find parent clique of a conditional. It will look at all parents and
|
||||
* return the one with the lowest index in the ordering.
|
||||
*/
|
||||
template<class CONTAINER>
|
||||
Index findParentClique(const CONTAINER& parents) const;
|
||||
|
||||
/** Remove all nodes */
|
||||
void clear();
|
||||
|
||||
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
|
||||
void deleteCachedShortcuts();
|
||||
|
||||
/**
|
||||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
*/
|
||||
void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
|
||||
* Factors and orphans are added to the in/out arguments.
|
||||
*/
|
||||
void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Remove the requested subtree. */
|
||||
Cliques removeSubtree(const sharedClique& subtree);
|
||||
|
||||
/** Insert a new subtree with known parent clique. This function does not check that the
|
||||
* specified parent is the correct parent. This function updates all of the internal data
|
||||
* structures associated with adding a subtree, such as populating the nodes index. */
|
||||
void insertRoot(const sharedClique& subtree);
|
||||
|
||||
/** add a clique (top down) */
|
||||
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
|
||||
|
||||
/** Add all cliques in this BayesTree to the specified factor graph */
|
||||
void addFactorsToGraph(FactorGraph<FactorType>& graph) const;
|
||||
|
||||
protected:
|
||||
|
||||
/** private helper method for saving the Tree to a text file in GraphViz format */
|
||||
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
|
||||
int parentnum = 0) const;
|
||||
|
||||
/** Gather data on a single clique */
|
||||
void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const;
|
||||
|
||||
/** remove a clique: warning, can result in a forest */
|
||||
void removeClique(sharedClique clique);
|
||||
|
||||
/** Fill the nodes index for a subtree */
|
||||
void fillNodesIndex(const sharedClique& subtree);
|
||||
|
||||
// Friend JunctionTree because it directly fills roots and nodes index.
|
||||
template<class BAYESRTEE, class GRAPH> friend class JunctionTree;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(nodes_);
|
||||
ar & BOOST_SERIALIZATION_NVP(roots_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
|
||||
{
|
||||
public:
|
||||
typedef CLIQUE CliqueType;
|
||||
typedef typename CLIQUE::ConditionalType Base;
|
||||
|
||||
boost::shared_ptr<CliqueType> clique;
|
||||
|
||||
BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) :
|
||||
clique(clique)
|
||||
{
|
||||
// Store parent keys in our base type factor so that eliminating those parent keys will pull
|
||||
// this subtree into the elimination.
|
||||
this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,301 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTreeCliqueBase-inl.h
|
||||
* @brief Base class for cliques of a BayesTree
|
||||
* @author Richard Roberts and Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/GenericSequentialSolver.h>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::assertInvariants() const {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
std::vector<Index> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::separator_setminus_B(
|
||||
derived_ptr B) const {
|
||||
sharedConditional p_F_S = this->conditional();
|
||||
std::vector<Index> &indicesB = B->conditional()->keys();
|
||||
std::vector<Index> S_setminus_B;
|
||||
std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), //
|
||||
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
|
||||
return S_setminus_B;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
std::vector<Index> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut_indices(
|
||||
derived_ptr B, const FactorGraph<FactorType>& p_Cp_B) const
|
||||
{
|
||||
gttic(shortcut_indices);
|
||||
std::set<Index> allKeys = p_Cp_B.keys();
|
||||
std::vector<Index> &indicesB = B->conditional()->keys();
|
||||
std::vector<Index> S_setminus_B = separator_setminus_B(B); // TODO, get as argument?
|
||||
std::vector<Index> keep;
|
||||
// keep = S\B intersect allKeys
|
||||
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
|
||||
allKeys.begin(), allKeys.end(), back_inserter(keep));
|
||||
// keep += B intersect allKeys
|
||||
std::set_intersection(indicesB.begin(), indicesB.end(), //
|
||||
allKeys.begin(), allKeys.end(), back_inserter(keep));
|
||||
// BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl;
|
||||
return keep;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase(
|
||||
const sharedConditional& conditional) :
|
||||
conditional_(conditional) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
BayesTreeCliqueBase<DERIVED, CONDITIONAL>::BayesTreeCliqueBase(
|
||||
const std::pair<sharedConditional,
|
||||
boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
|
||||
conditional_(result.first) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::print(const std::string& s,
|
||||
const IndexFormatter& indexFormatter) const {
|
||||
conditional_->print(s, indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::treeSize() const {
|
||||
size_t size = 1;
|
||||
BOOST_FOREACH(const derived_ptr& child, children_)
|
||||
size += child->treeSize();
|
||||
return size;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
size_t BayesTreeCliqueBase<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const {
|
||||
if (!cachedSeparatorMarginal_)
|
||||
return 0;
|
||||
|
||||
size_t subtree_count = 1;
|
||||
BOOST_FOREACH(const derived_ptr& child, children_)
|
||||
subtree_count += child->numCachedSeparatorMarginals();
|
||||
|
||||
return subtree_count;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::printTree(
|
||||
const std::string& indent, const IndexFormatter& indexFormatter) const {
|
||||
asDerived(this)->print(indent, indexFormatter);
|
||||
BOOST_FOREACH(const derived_ptr& child, children_)
|
||||
child->printTree(indent + " ", indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteWithInverse(
|
||||
const Permutation& inversePermutation) {
|
||||
conditional_->permuteWithInverse(inversePermutation);
|
||||
BOOST_FOREACH(const derived_ptr& child, children_) {
|
||||
child->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
|
||||
const internal::Reduction& inverseReduction)
|
||||
{
|
||||
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
if(!changed) {
|
||||
BOOST_FOREACH(const derived_ptr& child, children_) {
|
||||
assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
|
||||
}
|
||||
#endif
|
||||
if(changed) {
|
||||
BOOST_FOREACH(const derived_ptr& child, children_) {
|
||||
(void) child->reduceSeparatorWithInverse(inverseReduction); }
|
||||
}
|
||||
assertInvariants();
|
||||
return changed;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// The shortcut density is a conditional P(S|R) of the separator of this
|
||||
// clique on the root. We can compute it recursively from the parent shortcut
|
||||
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut(
|
||||
derived_ptr B, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_shortcut);
|
||||
|
||||
// We only calculate the shortcut when this clique is not B
|
||||
// and when the S\B is not empty
|
||||
std::vector<Index> S_setminus_B = separator_setminus_B(B);
|
||||
if (B.get() != this && !S_setminus_B.empty()) {
|
||||
|
||||
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
|
||||
derived_ptr parent(parent_.lock());
|
||||
gttoc(BayesTreeCliqueBase_shortcut);
|
||||
FactorGraph<FactorType> p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
|
||||
gttic(BayesTreeCliqueBase_shortcut);
|
||||
p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
|
||||
|
||||
// Determine the variables we want to keepSet, S union B
|
||||
std::vector<Index> keep = shortcut_indices(B, p_Cp_B);
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
inverseReduction.applyInverse(keep);
|
||||
gttoc(Reduce);
|
||||
|
||||
// Create solver that will marginalize for us
|
||||
GenericSequentialSolver<FactorType> solver(p_Cp_B);
|
||||
|
||||
// Finally, we only want to have S\B variables in the Bayes net, so
|
||||
size_t nrFrontals = S_setminus_B.size();
|
||||
BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
|
||||
if (factor) factor->permuteWithInverse(reduction); }
|
||||
result.permuteWithInverse(reduction);
|
||||
gttoc(Undo_Reduce);
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return result;
|
||||
} else {
|
||||
return BayesNet<CONDITIONAL>();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// separator marginal, uses separator marginal of parent recursively
|
||||
// P(C) = P(F|S) P(S)
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
FactorGraph<typename BayesTreeCliqueBase<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBase<
|
||||
DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal);
|
||||
// Check if the Separator marginal was already calculated
|
||||
if (!cachedSeparatorMarginal_) {
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
|
||||
// If this is the root, there is no separator
|
||||
if (R.get() == this) {
|
||||
// we are root, return empty
|
||||
FactorGraph<FactorType> empty;
|
||||
cachedSeparatorMarginal_ = empty;
|
||||
} else {
|
||||
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
|
||||
// initialize P(Cp) with the parent separator marginal
|
||||
derived_ptr parent(parent_.lock());
|
||||
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
|
||||
gttoc(BayesTreeCliqueBase_separatorMarginal);
|
||||
FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal);
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
|
||||
// now add the parent conditional
|
||||
p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
|
||||
// The variables we want to keepSet are exactly the ones in S
|
||||
sharedConditional p_F_S = this->conditional();
|
||||
std::vector<Index> indicesS(p_F_S->beginParents(), p_F_S->endParents());
|
||||
inverseReduction.applyInverse(indicesS);
|
||||
gttoc(Reduce);
|
||||
|
||||
// Create solver that will marginalize for us
|
||||
GenericSequentialSolver<FactorType> solver(p_Cp);
|
||||
|
||||
cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
|
||||
if (factor) factor->permuteWithInverse(reduction); }
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
|
||||
if (factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
}
|
||||
} else {
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal_cachehit);
|
||||
}
|
||||
|
||||
// return the shortcut P(S||B)
|
||||
return *cachedSeparatorMarginal_; // return the cached version
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// marginal2, uses separator marginal of parent recursively
|
||||
// P(C) = P(F|S) P(S)
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
FactorGraph<typename BayesTreeCliqueBase<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBase<
|
||||
DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_marginal2);
|
||||
// initialize with separator marginal P(S)
|
||||
FactorGraph<FactorType> p_C(this->separatorMarginal(R, function));
|
||||
// add the conditional P(F|S)
|
||||
p_C.push_back(this->conditional()->toFactor());
|
||||
return p_C;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::deleteCachedShortcuts() {
|
||||
|
||||
// When a shortcut is requested, all of the shortcuts between it and the
|
||||
// root are also generated. So, if this clique's cached shortcut is set,
|
||||
// recursively call over all child cliques. Otherwise, it is unnecessary.
|
||||
if (cachedSeparatorMarginal_) {
|
||||
BOOST_FOREACH(derived_ptr& child, children_) {
|
||||
child->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
//Delete CachedShortcut for this clique
|
||||
cachedSeparatorMarginal_ = boost::none;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,217 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTreeCliqueBase-inst.h
|
||||
* @brief Base class for cliques of a BayesTree
|
||||
* @author Richard Roberts and Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/BayesTreeCliqueBase.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::setEliminationResult(
|
||||
const typename FactorGraphType::EliminationResult& eliminationResult)
|
||||
{
|
||||
conditional_ = eliminationResult.first;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
|
||||
const DERIVED& other, double tol) const
|
||||
{
|
||||
return (!conditional_ && !other.conditional())
|
||||
|| conditional_->equals(*other.conditional(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
std::vector<Key>
|
||||
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
|
||||
{
|
||||
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
|
||||
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
|
||||
std::vector<Key> S_setminus_B;
|
||||
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
|
||||
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
|
||||
return S_setminus_B;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
std::vector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
|
||||
const derived_ptr& B, const FactorGraphType& p_Cp_B) const
|
||||
{
|
||||
gttic(shortcut_indices);
|
||||
FastSet<Key> allKeys = p_Cp_B.keys();
|
||||
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
|
||||
std::vector<Key> S_setminus_B = separator_setminus_B(B);
|
||||
std::vector<Key> keep;
|
||||
// keep = S\B intersect allKeys (S_setminus_B is already sorted)
|
||||
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
|
||||
allKeys.begin(), allKeys.end(), back_inserter(keep));
|
||||
// keep += B intersect allKeys
|
||||
std::set_intersection(indicesB.begin(), indicesB.end(), //
|
||||
allKeys.begin(), allKeys.end(), back_inserter(keep));
|
||||
return keep;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
conditional_->print(s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
|
||||
size_t size = 1;
|
||||
BOOST_FOREACH(const derived_ptr& child, children)
|
||||
size += child->treeSize();
|
||||
return size;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
|
||||
{
|
||||
if (!cachedSeparatorMarginal_)
|
||||
return 0;
|
||||
|
||||
size_t subtree_count = 1;
|
||||
BOOST_FOREACH(const derived_ptr& child, children)
|
||||
subtree_count += child->numCachedSeparatorMarginals();
|
||||
|
||||
return subtree_count;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// The shortcut density is a conditional P(S|R) of the separator of this
|
||||
// clique on the root. We can compute it recursively from the parent shortcut
|
||||
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
|
||||
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_shortcut);
|
||||
// We only calculate the shortcut when this clique is not B
|
||||
// and when the S\B is not empty
|
||||
std::vector<Key> S_setminus_B = separator_setminus_B(B);
|
||||
if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
|
||||
{
|
||||
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
|
||||
derived_ptr parent(parent_.lock());
|
||||
gttoc(BayesTreeCliqueBase_shortcut);
|
||||
FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
|
||||
gttic(BayesTreeCliqueBase_shortcut);
|
||||
p_Cp_B += parent->conditional_; // P(Fp|Sp)
|
||||
|
||||
// Determine the variables we want to keepSet, S union B
|
||||
std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
|
||||
|
||||
// Marginalize out everything except S union B
|
||||
boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
|
||||
return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
|
||||
}
|
||||
else
|
||||
{
|
||||
return BayesNetType();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// separator marginal, uses separator marginal of parent recursively
|
||||
// P(C) = P(F|S) P(S)
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
|
||||
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal);
|
||||
// Check if the Separator marginal was already calculated
|
||||
if (!cachedSeparatorMarginal_)
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
|
||||
// If this is the root, there is no separator
|
||||
if (parent_.expired() /*(if we're the root)*/)
|
||||
{
|
||||
// we are root, return empty
|
||||
FactorGraphType empty;
|
||||
cachedSeparatorMarginal_ = empty;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
|
||||
// initialize P(Cp) with the parent separator marginal
|
||||
derived_ptr parent(parent_.lock());
|
||||
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
|
||||
gttoc(BayesTreeCliqueBase_separatorMarginal);
|
||||
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal);
|
||||
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
|
||||
// now add the parent conditional
|
||||
p_Cp += parent->conditional_; // P(Fp|Sp)
|
||||
|
||||
// The variables we want to keepSet are exactly the ones in S
|
||||
std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
|
||||
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
|
||||
}
|
||||
}
|
||||
|
||||
// return the shortcut P(S||B)
|
||||
return *cachedSeparatorMarginal_; // return the cached version
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// marginal2, uses separator marginal of parent recursively
|
||||
// P(C) = P(F|S) P(S)
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
|
||||
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
|
||||
{
|
||||
gttic(BayesTreeCliqueBase_marginal2);
|
||||
// initialize with separator marginal P(S)
|
||||
FactorGraphType p_C = this->separatorMarginal(function);
|
||||
// add the conditional P(F|S)
|
||||
p_C += boost::shared_ptr<FactorType>(this->conditional_);
|
||||
return p_C;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
|
||||
|
||||
// When a shortcut is requested, all of the shortcuts between it and the
|
||||
// root are also generated. So, if this clique's cached shortcut is set,
|
||||
// recursively call over all child cliques. Otherwise, it is unnecessary.
|
||||
if (cachedSeparatorMarginal_) {
|
||||
BOOST_FOREACH(derived_ptr& child, children) {
|
||||
child->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
//Delete CachedShortcut for this clique
|
||||
cachedSeparatorMarginal_ = boost::none;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,273 +1,168 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTreeCliqueBase.h
|
||||
* @brief Base class for cliques of a BayesTree
|
||||
* @author Richard Roberts and Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
namespace gtsam {
|
||||
template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
||||
}
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This is the base class for BayesTree cliques. The default and standard
|
||||
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a
|
||||
* different clique type in order to store extra data along with the clique.
|
||||
*
|
||||
* This class is templated on the derived class (i.e. the curiously recursive
|
||||
* template pattern). The advantage of this over using virtual classes is
|
||||
* that it avoids the need for casting to get the derived type. This is
|
||||
* possible because all cliques in a BayesTree are the same type - if they
|
||||
* were not then we'd need a virtual class.
|
||||
*
|
||||
* @tparam DERIVED The derived clique type.
|
||||
* @tparam CONDITIONAL The conditional type.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
struct BayesTreeCliqueBase {
|
||||
|
||||
public:
|
||||
typedef BayesTreeCliqueBase<DERIVED, CONDITIONAL> This;
|
||||
typedef DERIVED DerivedType;
|
||||
typedef CONDITIONAL ConditionalType;
|
||||
typedef boost::shared_ptr<ConditionalType> sharedConditional;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
typedef boost::shared_ptr<DerivedType> derived_ptr;
|
||||
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
|
||||
typedef typename ConditionalType::FactorType FactorType;
|
||||
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
|
||||
|
||||
protected:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
BayesTreeCliqueBase() {}
|
||||
|
||||
/** Construct from a conditional, leaving parent and child pointers uninitialized */
|
||||
BayesTreeCliqueBase(const sharedConditional& conditional);
|
||||
|
||||
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
|
||||
BayesTreeCliqueBase(
|
||||
const std::pair<sharedConditional,
|
||||
boost::shared_ptr<typename ConditionalType::FactorType> >& result);
|
||||
|
||||
/// @}
|
||||
|
||||
/// This stores the Cached separator margnal P(S)
|
||||
mutable boost::optional<FactorGraph<FactorType> > cachedSeparatorMarginal_;
|
||||
|
||||
public:
|
||||
sharedConditional conditional_;
|
||||
derived_weak_ptr parent_;
|
||||
FastList<derived_ptr> children_;
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** check equality */
|
||||
bool equals(const This& other, double tol = 1e-9) const {
|
||||
return (!conditional_ && !other.conditional())
|
||||
|| conditional_->equals(*other.conditional(), tol);
|
||||
}
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "", const IndexFormatter& indexFormatter =
|
||||
DefaultIndexFormatter) const;
|
||||
|
||||
/** print this node and entire subtree below it */
|
||||
void printTree(const std::string& indent = "",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Access the conditional */
|
||||
const sharedConditional& conditional() const {
|
||||
return conditional_;
|
||||
}
|
||||
|
||||
/** is this the root of a Bayes tree ? */
|
||||
inline bool isRoot() const {
|
||||
return parent_.expired();
|
||||
}
|
||||
|
||||
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
|
||||
size_t treeSize() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
||||
/** The arrow operator accesses the conditional */
|
||||
const ConditionalType* operator->() const {
|
||||
return conditional_.get();
|
||||
}
|
||||
|
||||
/** return the const reference of children */
|
||||
const std::list<derived_ptr>& children() const {
|
||||
return children_;
|
||||
}
|
||||
|
||||
/** return a shared_ptr to the parent clique */
|
||||
derived_ptr parent() const {
|
||||
return parent_.lock();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** The arrow operator accesses the conditional */
|
||||
ConditionalType* operator->() {
|
||||
return conditional_.get();
|
||||
}
|
||||
|
||||
/** return the reference of children non-const version*/
|
||||
FastList<derived_ptr>& children() {
|
||||
return children_;
|
||||
}
|
||||
|
||||
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
|
||||
static derived_ptr Create(const sharedConditional& conditional) {
|
||||
return boost::make_shared<DerivedType>(conditional);
|
||||
}
|
||||
|
||||
/** Construct shared_ptr from a FactorGraph<FACTOR>::EliminationResult. In this class
|
||||
* the conditional part is kept and the factor part is ignored, but in derived clique
|
||||
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
|
||||
* @param result An elimination result, which is a pair<CONDITIONAL,FACTOR>
|
||||
*/
|
||||
static derived_ptr Create(const std::pair<sharedConditional,
|
||||
boost::shared_ptr<typename ConditionalType::FactorType> >& result) {
|
||||
return boost::make_shared<DerivedType>(result);
|
||||
}
|
||||
|
||||
/** Returns a new clique containing a copy of the conditional but without
|
||||
* the parent and child clique pointers.
|
||||
*/
|
||||
derived_ptr clone() const {
|
||||
return Create(sharedConditional(new ConditionalType(*conditional_)));
|
||||
}
|
||||
|
||||
/** Permute the variables in the whole subtree rooted at this clique */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/** Permute variables when they only appear in the separators. In this
|
||||
* case the running intersection property will be used to prevent always
|
||||
* traversing the whole tree. Returns whether any separator variables in
|
||||
* this subtree were reordered.
|
||||
*/
|
||||
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
|
||||
|
||||
/** return the conditional P(S|Root) on the separator given the root */
|
||||
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
|
||||
|
||||
/** return the marginal P(S) on the separator */
|
||||
FactorGraph<FactorType> separatorMarginal(derived_ptr root, Eliminate function) const;
|
||||
|
||||
/** return the marginal P(C) of the clique, using marginal caching */
|
||||
FactorGraph<FactorType> marginal2(derived_ptr root, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
|
||||
* This is performed when the bayes tree is modified.
|
||||
*/
|
||||
void deleteCachedShortcuts();
|
||||
|
||||
const boost::optional<FactorGraph<FactorType> >& cachedSeparatorMarginal() const {
|
||||
return cachedSeparatorMarginal_; }
|
||||
|
||||
friend class BayesTree<ConditionalType, DerivedType> ;
|
||||
|
||||
protected:
|
||||
|
||||
/// assert invariants that have to hold in a clique
|
||||
void assertInvariants() const;
|
||||
|
||||
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
|
||||
std::vector<Index> separator_setminus_B(derived_ptr B) const;
|
||||
|
||||
/// Calculate set \f$ S_p \cap B \f$ for shortcut calculations
|
||||
std::vector<Index> parent_separator_intersection_B(derived_ptr B) const;
|
||||
|
||||
/**
|
||||
* Determine variable indices to keep in recursive separator shortcut calculation
|
||||
* The factor graph p_Cp_B has keys from the parent clique Cp and from B.
|
||||
* But we only keep the variables not in S union B.
|
||||
*/
|
||||
std::vector<Index> shortcut_indices(derived_ptr B,
|
||||
const FactorGraph<FactorType>& p_Cp_B) const;
|
||||
|
||||
/** Non-recursive delete cached shortcuts and marginals - internal only. */
|
||||
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Cliques cannot be copied except by the clone() method, which does not
|
||||
* copy the parent and child pointers.
|
||||
*/
|
||||
BayesTreeCliqueBase(const This& other) {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
/** Cliques cannot be copied except by the clone() method, which does not
|
||||
* copy the parent and child pointers.
|
||||
*/
|
||||
This& operator=(const This& other) {
|
||||
assert(false);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(conditional_);
|
||||
ar & BOOST_SERIALIZATION_NVP(parent_);
|
||||
ar & BOOST_SERIALIZATION_NVP(children_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
// \struct Clique
|
||||
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
const DERIVED* asDerived(
|
||||
const BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
|
||||
return static_cast<const DERIVED*>(base);
|
||||
}
|
||||
|
||||
template<class DERIVED, class CONDITIONAL>
|
||||
DERIVED* asDerived(BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
|
||||
return static_cast<DERIVED*>(base);
|
||||
}
|
||||
|
||||
}
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTreeCliqueBase.h
|
||||
* @brief Base class for cliques of a BayesTree
|
||||
* @author Richard Roberts and Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <gtsam/base/types.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
template<class CLIQUE> class BayesTree;
|
||||
template<class GRAPH> struct EliminationTraits;
|
||||
|
||||
/**
|
||||
* This is the base class for BayesTree cliques. The default and standard derived type is
|
||||
* BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store
|
||||
* extra data along with the clique.
|
||||
*
|
||||
* This class is templated on the derived class (i.e. the curiously recursive template pattern).
|
||||
* The advantage of this over using virtual classes is that it avoids the need for casting to get
|
||||
* the derived type. This is possible because all cliques in a BayesTree are the same type - if
|
||||
* they were not then we'd need a virtual class.
|
||||
*
|
||||
* @tparam DERIVED The derived clique type.
|
||||
* @tparam CONDITIONAL The conditional type.
|
||||
* \nosubgrouping */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
class BayesTreeCliqueBase
|
||||
{
|
||||
private:
|
||||
typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
|
||||
typedef DERIVED DerivedType;
|
||||
typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
typedef boost::shared_ptr<DerivedType> derived_ptr;
|
||||
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
|
||||
|
||||
public:
|
||||
typedef FACTORGRAPH FactorGraphType;
|
||||
typedef typename EliminationTraitsType::BayesNetType BayesNetType;
|
||||
typedef typename BayesNetType::ConditionalType ConditionalType;
|
||||
typedef boost::shared_ptr<ConditionalType> sharedConditional;
|
||||
typedef typename FactorGraphType::FactorType FactorType;
|
||||
typedef typename FactorGraphType::Eliminate Eliminate;
|
||||
|
||||
protected:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
BayesTreeCliqueBase() {}
|
||||
|
||||
/** Construct from a conditional, leaving parent and child pointers uninitialized */
|
||||
BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional) {}
|
||||
|
||||
/// @}
|
||||
|
||||
/// This stores the Cached separator margnal P(S)
|
||||
mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
|
||||
|
||||
public:
|
||||
sharedConditional conditional_;
|
||||
derived_weak_ptr parent_;
|
||||
std::vector<derived_ptr> children;
|
||||
|
||||
/// Fill the elimination result produced during elimination. Here this just stores the
|
||||
/// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
|
||||
/// to also cache the remaining factor.
|
||||
void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** check equality */
|
||||
bool equals(const DERIVED& other, double tol = 1e-9) const;
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Access the conditional */
|
||||
const sharedConditional& conditional() const { return conditional_; }
|
||||
|
||||
/** is this the root of a Bayes tree ? */
|
||||
inline bool isRoot() const { return parent_.expired(); }
|
||||
|
||||
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
|
||||
size_t treeSize() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
||||
/** return a shared_ptr to the parent clique */
|
||||
derived_ptr parent() const { return parent_.lock(); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** return the conditional P(S|Root) on the separator given the root */
|
||||
BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/** return the marginal P(S) on the separator */
|
||||
FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/** return the marginal P(C) of the clique, using marginal caching */
|
||||
FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
|
||||
|
||||
/**
|
||||
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
|
||||
* This is performed when the bayes tree is modified.
|
||||
*/
|
||||
void deleteCachedShortcuts();
|
||||
|
||||
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
|
||||
return cachedSeparatorMarginal_; }
|
||||
|
||||
friend class BayesTree<DerivedType>;
|
||||
|
||||
protected:
|
||||
|
||||
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
|
||||
std::vector<Key> separator_setminus_B(const derived_ptr& B) const;
|
||||
|
||||
/** Determine variable indices to keep in recursive separator shortcut calculation The factor
|
||||
* graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
|
||||
* not in S union B. */
|
||||
std::vector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
|
||||
|
||||
/** Non-recursive delete cached shortcuts and marginals - internal only. */
|
||||
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(conditional_);
|
||||
ar & BOOST_SERIALIZATION_NVP(parent_);
|
||||
ar & BOOST_SERIALIZATION_NVP(children);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,112 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ClusterTree-inl.h
|
||||
* @date July 13, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* *
|
||||
* Cluster
|
||||
* ************************************************************************* */
|
||||
template<class FG>
|
||||
template<class Iterator>
|
||||
ClusterTree<FG>::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) :
|
||||
FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
template<typename FRONTALIT, typename SEPARATORIT>
|
||||
ClusterTree<FG>::Cluster::Cluster(
|
||||
const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
|
||||
FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
template<typename FRONTALIT, typename SEPARATORIT>
|
||||
ClusterTree<FG>::Cluster::Cluster(
|
||||
FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
|
||||
frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void ClusterTree<FG>::Cluster::addChild(typename ClusterTree<FG>::Cluster::shared_ptr child) {
|
||||
children_.push_back(child);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
bool ClusterTree<FG>::Cluster::equals(const Cluster& other) const {
|
||||
if (frontal != other.frontal) return false;
|
||||
if (separator != other.separator) return false;
|
||||
if (children_.size() != other.children_.size()) return false;
|
||||
|
||||
typename std::list<shared_ptr>::const_iterator it1 = children_.begin();
|
||||
typename std::list<shared_ptr>::const_iterator it2 = other.children_.begin();
|
||||
for (; it1 != children_.end(); it1++, it2++)
|
||||
if (!(*it1)->equals(**it2)) return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void ClusterTree<FG>::Cluster::print(const std::string& indent,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << indent;
|
||||
BOOST_FOREACH(const Index key, frontal)
|
||||
std::cout << formatter(key) << " ";
|
||||
std::cout << ": ";
|
||||
BOOST_FOREACH(const Index key, separator)
|
||||
std::cout << key << " ";
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void ClusterTree<FG>::Cluster::printTree(const std::string& indent,
|
||||
const IndexFormatter& formatter) const {
|
||||
print(indent, formatter);
|
||||
BOOST_FOREACH(const shared_ptr& child, children_)
|
||||
child->printTree(indent + " ", formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
* ClusterTree
|
||||
* ************************************************************************* */
|
||||
template<class FG>
|
||||
void ClusterTree<FG>::print(const std::string& str,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << str << std::endl;
|
||||
if (root_) root_->printTree("", formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
bool ClusterTree<FG>::equals(const ClusterTree<FG>& other, double tol) const {
|
||||
if (!root_ && !other.root_) return true;
|
||||
if (!root_ || !other.root_) return false;
|
||||
return root_->equals(*other.root_);
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
@ -1,141 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ClusterTree.h
|
||||
* @date July 13, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman:
|
||||
* each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that
|
||||
* each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template <class FG>
|
||||
class ClusterTree {
|
||||
public:
|
||||
// Access to factor types
|
||||
typedef typename FG::KeyType KeyType;
|
||||
|
||||
protected:
|
||||
|
||||
// the class for subgraphs that also include the pointers to the parents and two children
|
||||
class Cluster : public FG {
|
||||
public:
|
||||
typedef typename boost::shared_ptr<Cluster> shared_ptr;
|
||||
typedef typename boost::weak_ptr<Cluster> weak_ptr;
|
||||
|
||||
const std::vector<Index> frontal; // the frontal variables
|
||||
const std::vector<Index> separator; // the separator variables
|
||||
|
||||
protected:
|
||||
|
||||
weak_ptr parent_; // the parent cluster
|
||||
std::list<shared_ptr> children_; // the child clusters
|
||||
const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
|
||||
|
||||
public:
|
||||
|
||||
/// Construct empty clique
|
||||
Cluster() {}
|
||||
|
||||
/** Create a node with a single frontal variable */
|
||||
template<typename Iterator>
|
||||
Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
|
||||
|
||||
/** Create a node with several frontal variables */
|
||||
template<typename FRONTALIT, typename SEPARATORIT>
|
||||
Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
|
||||
|
||||
/** Create a node with several frontal variables */
|
||||
template<typename FRONTALIT, typename SEPARATORIT>
|
||||
Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
|
||||
|
||||
/// print
|
||||
void print(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/// print the enire tree
|
||||
void printTree(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/// check equality
|
||||
bool equals(const Cluster& other) const;
|
||||
|
||||
/// get a reference to the children
|
||||
const std::list<shared_ptr>& children() const { return children_; }
|
||||
|
||||
/// add a child
|
||||
void addChild(shared_ptr child);
|
||||
|
||||
/// get or set the parent
|
||||
weak_ptr& parent() { return parent_; }
|
||||
|
||||
};
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/// typedef for shared pointers to clusters
|
||||
typedef typename Cluster::shared_ptr sharedCluster;
|
||||
|
||||
/// Root cluster
|
||||
sharedCluster root_;
|
||||
|
||||
public:
|
||||
|
||||
/// @}
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// constructor of empty tree
|
||||
ClusterTree() {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// return the root cluster
|
||||
sharedCluster root() const { return root_; }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print the object
|
||||
void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/** check equality */
|
||||
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
}; // ClusterTree
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/ClusterTree-inl.h>
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Conditional.h
|
||||
* @brief Base class for conditional densities
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR, class DERIVEDFACTOR>
|
||||
void Conditional<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
|
||||
std::cout << s << " P(";
|
||||
BOOST_FOREACH(Key key, frontals())
|
||||
std::cout << " " << formatter(key);
|
||||
if (nrParents() > 0)
|
||||
std::cout << " |";
|
||||
BOOST_FOREACH(Key parent, parents())
|
||||
std::cout << " " << formatter(parent);
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR, class DERIVEDFACTOR>
|
||||
bool Conditional<FACTOR,DERIVEDFACTOR>::equals(const This& c, double tol) const
|
||||
{
|
||||
return nrFrontals_ == c.nrFrontals_;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,266 +1,147 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Conditional.h
|
||||
* @brief Base class for conditional densities
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Factor.h>
|
||||
|
||||
#include <boost/range.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <list>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class for conditional densities, templated on KEY type. This class
|
||||
* provides storage for the keys involved in a conditional, and iterators and
|
||||
* access to the frontal and separator keys.
|
||||
*
|
||||
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
|
||||
* to the associated factor type and shared_ptr type of the derived class. See
|
||||
* IndexConditional and GaussianConditional for examples.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<typename KEY>
|
||||
class Conditional: public gtsam::Factor<KEY> {
|
||||
|
||||
private:
|
||||
|
||||
/** Create keys by adding key in front */
|
||||
template<typename ITERATOR>
|
||||
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent,
|
||||
ITERATOR lastParent) {
|
||||
std::vector<KeyType> keys((lastParent - firstParent) + 1);
|
||||
std::copy(firstParent, lastParent, keys.begin() + 1);
|
||||
keys[0] = key;
|
||||
return keys;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/** The first nFrontal variables are frontal and the rest are parents. */
|
||||
size_t nrFrontals_;
|
||||
|
||||
// Calls the base class assertInvariants, which checks for unique keys
|
||||
void assertInvariants() const {
|
||||
Factor<KEY>::assertInvariants();
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
typedef KEY KeyType;
|
||||
typedef Conditional<KeyType> This;
|
||||
typedef Factor<KeyType> Base;
|
||||
|
||||
/**
|
||||
* Typedef to the factor type that produces this conditional and that this
|
||||
* conditional can be converted to using a factor constructor. Derived
|
||||
* classes must redefine this.
|
||||
*/
|
||||
typedef gtsam::Factor<KeyType> FactorType;
|
||||
|
||||
/** A shared_ptr to this class. Derived classes must redefine this. */
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/** Iterator over keys */
|
||||
typedef typename FactorType::iterator iterator;
|
||||
|
||||
/** Const iterator over keys */
|
||||
typedef typename FactorType::const_iterator const_iterator;
|
||||
|
||||
/** View of the frontal keys (call frontals()) */
|
||||
typedef boost::iterator_range<const_iterator> Frontals;
|
||||
|
||||
/** View of the separator keys (call parents()) */
|
||||
typedef boost::iterator_range<const_iterator> Parents;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Empty Constructor to make serialization possible */
|
||||
Conditional() :
|
||||
nrFrontals_(0) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** No parents */
|
||||
Conditional(KeyType key) :
|
||||
FactorType(key), nrFrontals_(1) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Single parent */
|
||||
Conditional(KeyType key, KeyType parent) :
|
||||
FactorType(key, parent), nrFrontals_(1) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Two parents */
|
||||
Conditional(KeyType key, KeyType parent1, KeyType parent2) :
|
||||
FactorType(key, parent1, parent2), nrFrontals_(1) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Three parents */
|
||||
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) :
|
||||
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** Constructor from a frontal variable and a vector of parents */
|
||||
Conditional(KeyType key, const std::vector<KeyType>& parents) :
|
||||
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
|
||||
1) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from keys and nr of frontal variables */
|
||||
Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
||||
FactorType(keys), nrFrontals_(nrFrontals) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from keys and nr of frontal variables */
|
||||
Conditional(const std::list<Index>& keys, size_t nrFrontals) :
|
||||
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional formatter */
|
||||
void print(const std::string& s = "Conditional",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/** check equality */
|
||||
template<class DERIVED>
|
||||
bool equals(const DERIVED& c, double tol = 1e-9) const {
|
||||
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return the number of frontals */
|
||||
size_t nrFrontals() const {
|
||||
return nrFrontals_;
|
||||
}
|
||||
|
||||
/** return the number of parents */
|
||||
size_t nrParents() const {
|
||||
return FactorType::size() - nrFrontals_;
|
||||
}
|
||||
|
||||
/** Special accessor when there is only one frontal variable. */
|
||||
KeyType firstFrontalKey() const {
|
||||
assert(nrFrontals_>0);
|
||||
return FactorType::front();
|
||||
}
|
||||
KeyType lastFrontalKey() const {
|
||||
assert(nrFrontals_>0);
|
||||
return *(endFrontals() - 1);
|
||||
}
|
||||
|
||||
/** return a view of the frontal keys */
|
||||
Frontals frontals() const {
|
||||
return boost::make_iterator_range(beginFrontals(), endFrontals());
|
||||
}
|
||||
|
||||
/** return a view of the parent keys */
|
||||
Parents parents() const {
|
||||
return boost::make_iterator_range(beginParents(), endParents());
|
||||
}
|
||||
|
||||
/** Iterators over frontal and parent variables. */
|
||||
const_iterator beginFrontals() const {
|
||||
return FactorType::begin();
|
||||
} ///<TODO: comment
|
||||
const_iterator endFrontals() const {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
const_iterator beginParents() const {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
const_iterator endParents() const {
|
||||
return FactorType::end();
|
||||
} ///<TODO: comment
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Mutable iterators and accessors */
|
||||
iterator beginFrontals() {
|
||||
return FactorType::begin();
|
||||
} ///<TODO: comment
|
||||
iterator endFrontals() {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator beginParents() {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator endParents() {
|
||||
return FactorType::end();
|
||||
} ///<TODO: comment
|
||||
|
||||
///TODO: comment
|
||||
boost::iterator_range<iterator> frontals() {
|
||||
return boost::make_iterator_range(beginFrontals(), endFrontals());
|
||||
}
|
||||
|
||||
///TODO: comment
|
||||
boost::iterator_range<iterator> parents() {
|
||||
return boost::make_iterator_range(beginParents(), endParents());
|
||||
}
|
||||
|
||||
private:
|
||||
/** 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(nrFrontals_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
void Conditional<KEY>::print(const std::string& s,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << s << " P(";
|
||||
BOOST_FOREACH(KeyType key, frontals())
|
||||
std::cout << " " << formatter(key);
|
||||
if (nrParents() > 0)
|
||||
std::cout << " |";
|
||||
BOOST_FOREACH(KeyType parent, parents())
|
||||
std::cout << " " << formatter(parent);
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
} // gtsam
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Conditional.h
|
||||
* @brief Base class for conditional densities
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <boost/range.hpp>
|
||||
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class for conditional densities, templated on KEY type. This class
|
||||
* provides storage for the keys involved in a conditional, and iterators and
|
||||
* access to the frontal and separator keys.
|
||||
*
|
||||
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
|
||||
* to the associated factor type and shared_ptr type of the derived class. See
|
||||
* IndexConditional and GaussianConditional for examples.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FACTOR, class DERIVEDCONDITIONAL>
|
||||
class Conditional
|
||||
{
|
||||
protected:
|
||||
/** The first nrFrontal variables are frontal and the rest are parents. */
|
||||
size_t nrFrontals_;
|
||||
|
||||
private:
|
||||
/// Typedef to this class
|
||||
typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
|
||||
|
||||
public:
|
||||
/** View of the frontal keys (call frontals()) */
|
||||
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
|
||||
|
||||
/** View of the separator keys (call parents()) */
|
||||
typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
|
||||
|
||||
protected:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Empty Constructor to make serialization possible */
|
||||
Conditional() : nrFrontals_(0) {}
|
||||
|
||||
/** Constructor */
|
||||
Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional formatter */
|
||||
void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** check equality */
|
||||
bool equals(const This& c, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return the number of frontals */
|
||||
size_t nrFrontals() const { return nrFrontals_; }
|
||||
|
||||
/** return the number of parents */
|
||||
size_t nrParents() const { return asFactor().size() - nrFrontals_; }
|
||||
|
||||
/** Convenience function to get the first frontal key */
|
||||
Key firstFrontalKey() const {
|
||||
if(nrFrontals_ > 0)
|
||||
return asFactor().front();
|
||||
else
|
||||
throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
|
||||
}
|
||||
|
||||
/** return a view of the frontal keys */
|
||||
Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
|
||||
|
||||
/** return a view of the parent keys */
|
||||
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
|
||||
|
||||
/** Iterator pointing to first frontal key. */
|
||||
typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
|
||||
|
||||
/** Iterator pointing past the last frontal key. */
|
||||
typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Iterator pointing to the first parent key. */
|
||||
typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
|
||||
|
||||
/** Iterator pointing past the last parent key. */
|
||||
typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Mutable iterator pointing to first frontal key. */
|
||||
typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
|
||||
|
||||
/** Mutable iterator pointing past the last frontal key. */
|
||||
typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Mutable iterator pointing to the first parent key. */
|
||||
typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Mutable iterator pointing past the last parent key. */
|
||||
typename FACTOR::iterator endParents() { return asFactor().end(); }
|
||||
|
||||
private:
|
||||
// Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
|
||||
FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
|
||||
|
||||
// Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
|
||||
const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,298 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminateableFactorGraph.h
|
||||
* @brief Variable elimination algorithms for factor graphs
|
||||
* @author Richard Roberts
|
||||
* @date Apr 21, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/inference/inferenceExceptions.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
|
||||
OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(ordering && variableIndex) {
|
||||
gttic(eliminateSequential);
|
||||
// Do elimination
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= EliminationTreeType(asDerived(), *variableIndex, *ordering).eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
// Return the Bayes net
|
||||
return result.first;
|
||||
}
|
||||
else if(!variableIndex) {
|
||||
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
|
||||
// for no variable index first so that it's always computed if we need to call COLAMD because
|
||||
// no Ordering is provided.
|
||||
return eliminateSequential(ordering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
else /*if(!ordering)*/ {
|
||||
// If no Ordering provided, compute one and call this function again. We are guaranteed to
|
||||
// have a VariableIndex already here because we computed one if needed in the previous 'else'
|
||||
// block.
|
||||
return eliminateSequential(Ordering::COLAMD(*variableIndex), function);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
|
||||
OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(ordering && variableIndex) {
|
||||
gttic(eliminateMultifrontal);
|
||||
// Do elimination with given ordering
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, *ordering)).eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
// Return the Bayes tree
|
||||
return result.first;
|
||||
}
|
||||
else if(!variableIndex) {
|
||||
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
|
||||
// for no variable index first so that it's always computed if we need to call COLAMD because
|
||||
// no Ordering is provided.
|
||||
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
else /*if(!ordering)*/ {
|
||||
// If no Ordering provided, compute one and call this function again. We are guaranteed to
|
||||
// have a VariableIndex already here because we computed one if needed in the previous 'else'
|
||||
// block.
|
||||
return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
|
||||
const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialSequential);
|
||||
// Do elimination
|
||||
return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialSequential(ordering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
|
||||
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialSequential);
|
||||
// Compute full ordering
|
||||
Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
|
||||
|
||||
// Split off the part of the ordering for the variables being eliminated
|
||||
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
|
||||
return eliminatePartialSequential(ordering, function, variableIndex);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialSequential(variables, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
|
||||
const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialMultifrontal);
|
||||
// Do elimination
|
||||
return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
|
||||
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialMultifrontal);
|
||||
// Compute full ordering
|
||||
Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
|
||||
|
||||
// Split off the part of the ordering for the variables being eliminated
|
||||
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
|
||||
return eliminatePartialMultifrontal(ordering, function, variableIndex);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex)
|
||||
{
|
||||
if(marginalizedVariableOrdering)
|
||||
{
|
||||
gttic(marginalMultifrontalBayesNet);
|
||||
// An ordering was provided for the marginalized variables, so we can first eliminate them
|
||||
// in the order requested.
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
|
||||
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
|
||||
|
||||
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
||||
{
|
||||
// An ordering was also provided for the unmarginalized variables, so we can also
|
||||
// eliminate them in the order requested.
|
||||
return eliminated.second->eliminateSequential(*varsAsOrdering, function);
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
|
||||
return eliminated.second->eliminateSequential(boost::none, function);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the marginalized variables, so order them using constrained
|
||||
// COLAMD.
|
||||
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
|
||||
const std::vector<Key>* variablesOrOrdering =
|
||||
unmarginalizedAreOrdered ?
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
||||
// Split up ordering
|
||||
const size_t nVars = variablesOrOrdering->size();
|
||||
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
|
||||
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
|
||||
|
||||
// Call this function again with the computed orderings
|
||||
return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
|
||||
}
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex)
|
||||
{
|
||||
if(marginalizedVariableOrdering)
|
||||
{
|
||||
gttic(marginalMultifrontalBayesTree);
|
||||
// An ordering was provided for the marginalized variables, so we can first eliminate them
|
||||
// in the order requested.
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
|
||||
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
|
||||
|
||||
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
||||
{
|
||||
// An ordering was also provided for the unmarginalized variables, so we can also
|
||||
// eliminate them in the order requested.
|
||||
return eliminated.second->eliminateMultifrontal(*varsAsOrdering, function);
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
|
||||
return eliminated.second->eliminateMultifrontal(boost::none, function);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// No ordering was provided for the marginalized variables, so order them using constrained
|
||||
// COLAMD.
|
||||
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
|
||||
const std::vector<Key>* variablesOrOrdering =
|
||||
unmarginalizedAreOrdered ?
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
||||
// Split up ordering
|
||||
const size_t nVars = variablesOrOrdering->size();
|
||||
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
|
||||
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
|
||||
|
||||
// Call this function again with the computed orderings
|
||||
return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
|
||||
}
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<FACTORGRAPH>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginal(
|
||||
const std::vector<Key>& variables,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex)
|
||||
{
|
||||
// Compute a total ordering for all variables
|
||||
Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables);
|
||||
|
||||
// Split out the part for the marginalized variables
|
||||
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
|
||||
|
||||
// Eliminate and return the remaining factor graph
|
||||
return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
|
||||
}
|
||||
else
|
||||
{
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return marginal(variables, function, VariableIndex(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,236 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminateableFactorGraph.h
|
||||
* @brief Variable elimination algorithms for factor graphs
|
||||
* @author Richard Roberts
|
||||
* @date Apr 21, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/variant.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Traits class for eliminateable factor graphs, specifies the types that result from
|
||||
/// elimination, etc. This must be defined for each factor graph that inherits from
|
||||
/// EliminateableFactorGraph.
|
||||
template<class GRAPH>
|
||||
struct EliminationTraits
|
||||
{
|
||||
// Template for deriving:
|
||||
// typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. GaussianFactor)
|
||||
// typedef MyFactorGraphType FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
|
||||
// typedef MyConditional ConditionalType; ///< Type of conditionals from elimination (e.g. GaussianConditional)
|
||||
// typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet)
|
||||
// typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree)
|
||||
// typedef MyBayesTree BayesTreeType; ///< Type of Bayes tree (e.g. GaussianBayesTree)
|
||||
// typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree)
|
||||
// static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
|
||||
// DefaultEliminate(
|
||||
// const MyFactorGraph& factors, const Ordering& keys); ///< The default dense elimination function
|
||||
};
|
||||
|
||||
|
||||
/** EliminateableFactorGraph is a base class for factor graphs that contains elimination
|
||||
* algorithms. Any factor graph holding eliminateable factors can derive from this class to
|
||||
* expose functions for computing marginals, conditional marginals, doing multifrontal and
|
||||
* sequential elimination, etc. */
|
||||
template<class FACTORGRAPH>
|
||||
class EliminateableFactorGraph
|
||||
{
|
||||
private:
|
||||
typedef EliminateableFactorGraph<FACTORGRAPH> This; ///< Typedef to this class.
|
||||
typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type
|
||||
// Base factor type stored in this graph (private because derived classes will get this from
|
||||
// their FactorGraph base class)
|
||||
typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
|
||||
|
||||
public:
|
||||
/// Typedef to the specific EliminationTraits for this graph
|
||||
typedef EliminationTraits<FactorGraphType> EliminationTraitsType;
|
||||
|
||||
/// Conditional type stored in the Bayes net produced by elimination
|
||||
typedef typename EliminationTraitsType::ConditionalType ConditionalType;
|
||||
|
||||
/// Bayes net type produced by sequential elimination
|
||||
typedef typename EliminationTraitsType::BayesNetType BayesNetType;
|
||||
|
||||
/// Elimination tree type that can do sequential elimination of this graph
|
||||
typedef typename EliminationTraitsType::EliminationTreeType EliminationTreeType;
|
||||
|
||||
/// Bayes tree type produced by multifrontal elimination
|
||||
typedef typename EliminationTraitsType::BayesTreeType BayesTreeType;
|
||||
|
||||
/// Junction tree type that can do multifrontal elimination of this graph
|
||||
typedef typename EliminationTraitsType::JunctionTreeType JunctionTreeType;
|
||||
|
||||
/// The pair of conditional and remaining factor produced by a single dense elimination step on
|
||||
/// a subgraph.
|
||||
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
|
||||
|
||||
/// The function type that does a single dense elimination step on a subgraph.
|
||||
typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
|
||||
|
||||
/// Typedef for an optional ordering as an argument to elimination functions
|
||||
typedef boost::optional<const Ordering&> OptionalOrdering;
|
||||
|
||||
/// Typedef for an optional variable index as an argument to elimination functions
|
||||
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
|
||||
|
||||
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
|
||||
* provided, the ordering provided by COLAMD will be used.
|
||||
*
|
||||
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
|
||||
* \code
|
||||
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
|
||||
* \endcode
|
||||
*
|
||||
* <b> Example - Full QR elimination in specified order:
|
||||
* \code
|
||||
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, myOrdering);
|
||||
* \endcode
|
||||
*
|
||||
* <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
|
||||
* \code
|
||||
* VariableIndex varIndex(graph); // Build variable index
|
||||
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
|
||||
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex);
|
||||
* \endcode
|
||||
* */
|
||||
boost::shared_ptr<BayesNetType> eliminateSequential(
|
||||
OptionalOrdering ordering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
|
||||
* provided, the ordering provided by COLAMD will be used.
|
||||
*
|
||||
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
|
||||
* \code
|
||||
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateCholesky);
|
||||
* \endcode
|
||||
*
|
||||
* <b> Example - Full QR elimination in specified order:
|
||||
* \code
|
||||
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering);
|
||||
* \endcode
|
||||
*
|
||||
* <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
|
||||
* \code
|
||||
* VariableIndex varIndex(graph); // Build variable index
|
||||
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
|
||||
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex);
|
||||
* \endcode
|
||||
* */
|
||||
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
|
||||
OptionalOrdering ordering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
|
||||
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
|
||||
* where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$
|
||||
* B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialSequential(
|
||||
const Ordering& ordering,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to
|
||||
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
|
||||
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
|
||||
* factor graph, and \f$ B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialSequential(
|
||||
const std::vector<Key>& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes
|
||||
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
|
||||
* \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
|
||||
* \f$ B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialMultifrontal(
|
||||
const Ordering& ordering,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
|
||||
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
|
||||
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
|
||||
* factor graph, and \f$ B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialMultifrontal(
|
||||
const std::vector<Key>& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Compute the marginal of the requested variables and return the result as a Bayes net.
|
||||
* @param variables Determines the variables whose marginal to compute, if provided as an
|
||||
* Ordering they will be ordered in the returned BayesNet as specified, and if provided
|
||||
* as a vector<Key> they will be ordered using constrained COLAMD.
|
||||
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
|
||||
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
|
||||
* will be computed with COLAMD.
|
||||
* @param function Optional dense elimination function, if not provided the default will be
|
||||
* used.
|
||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||
* provided one will be computed. */
|
||||
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Compute the marginal of the requested variables and return the result as a Bayes tree.
|
||||
* @param variables Determines the variables whose marginal to compute, if provided as an
|
||||
* Ordering they will be ordered in the returned BayesNet as specified, and if provided
|
||||
* as a vector<Key> they will be ordered using constrained COLAMD.
|
||||
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
|
||||
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
|
||||
* will be computed with COLAMD.
|
||||
* @param function Optional dense elimination function, if not provided the default will be
|
||||
* used.
|
||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||
* provided one will be computed. */
|
||||
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Compute the marginal factor graph of the requested variables. */
|
||||
boost::shared_ptr<FactorGraphType> marginal(
|
||||
const std::vector<Key>& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
private:
|
||||
|
||||
// Access the derived factor graph class
|
||||
const FactorGraphType& asDerived() const { return static_cast<const FactorGraphType&>(*this); }
|
||||
|
||||
// Access the derived factor graph class
|
||||
FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,234 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminationTree-inl.h
|
||||
* @author Frank Dellaert
|
||||
* @date Oct 13, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/static_assert.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename EliminationTree<FACTOR>::sharedFactor EliminationTree<FACTOR>::eliminate_(
|
||||
Eliminate function, Conditionals& conditionals) const {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl;
|
||||
|
||||
if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable
|
||||
// Create the list of factors to be eliminated, initially empty, and reserve space
|
||||
FactorGraph<FACTOR> factors;
|
||||
factors.reserve(this->factors_.size() + this->subTrees_.size());
|
||||
|
||||
// Add all factors associated with the current node
|
||||
factors.push_back(this->factors_.begin(), this->factors_.end());
|
||||
|
||||
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_)
|
||||
factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread
|
||||
// TODO: wait for completion of all threads
|
||||
|
||||
// Combine all factors (from this node and from subtrees) into a joint factor
|
||||
typename FactorGraph<FACTOR>::EliminationResult
|
||||
eliminated(function(factors, 1));
|
||||
conditionals[this->key_] = eliminated.first;
|
||||
|
||||
if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n";
|
||||
if(debug) eliminated.first->print("Conditional: ");
|
||||
if(debug) eliminated.second->print("Factor: ");
|
||||
|
||||
return eliminated.second;
|
||||
} else {
|
||||
// Eliminate each child but discard the result.
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
|
||||
(void)child->eliminate_(function, conditionals);
|
||||
}
|
||||
return sharedFactor(); // Return a NULL factor
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
std::vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
|
||||
|
||||
// Number of factors and variables
|
||||
const size_t m = structure.nFactors();
|
||||
const size_t n = structure.size();
|
||||
|
||||
static const Index none = std::numeric_limits<Index>::max();
|
||||
|
||||
// Allocate result parent vector and vector of last factor columns
|
||||
std::vector<Index> parents(n, none);
|
||||
std::vector<Index> prevCol(m, none);
|
||||
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(const size_t i, structure[j]) {
|
||||
if (prevCol[i] != none) {
|
||||
Index k = prevCol[i];
|
||||
// find root r of the current tree that contains k
|
||||
Index r = k;
|
||||
while (parents[r] != none)
|
||||
r = parents[r];
|
||||
if (r != j) parents[r] = j;
|
||||
}
|
||||
prevCol[i] = j;
|
||||
}
|
||||
}
|
||||
|
||||
return parents;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class DERIVEDFACTOR>
|
||||
typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
|
||||
const FactorGraph<DERIVEDFACTOR>& factorGraph,
|
||||
const VariableIndex& structure) {
|
||||
|
||||
static const bool debug = false;
|
||||
gttic(ET_Create1);
|
||||
|
||||
gttic(ET_ComputeParents);
|
||||
// Compute the tree structure
|
||||
std::vector<Index> parents(ComputeParents(structure));
|
||||
gttoc(ET_ComputeParents);
|
||||
|
||||
// Number of variables
|
||||
const size_t n = structure.size();
|
||||
|
||||
static const Index none = std::numeric_limits<Index>::max();
|
||||
|
||||
// Create tree structure
|
||||
gttic(assemble_tree);
|
||||
std::vector<shared_ptr> trees(n);
|
||||
for (Index k = 1; k <= n; k++) {
|
||||
Index j = n - k; // Start at the last variable and loop down to 0
|
||||
trees[j].reset(new EliminationTree(j)); // Create a new node on this variable
|
||||
if (parents[j] != none) // If this node has a parent, add it to the parent's children
|
||||
trees[parents[j]]->add(trees[j]);
|
||||
else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest
|
||||
throw DisconnectedGraphException();
|
||||
}
|
||||
gttoc(assemble_tree);
|
||||
|
||||
// Hang factors in right places
|
||||
gttic(hang_factors);
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<DERIVEDFACTOR>& factor, factorGraph) {
|
||||
if(factor && factor->size() > 0) {
|
||||
Index j = *std::min_element(factor->begin(), factor->end());
|
||||
if(j < structure.size())
|
||||
trees[j]->add(factor);
|
||||
}
|
||||
}
|
||||
gttoc(hang_factors);
|
||||
|
||||
if(debug)
|
||||
trees.back()->print("ETree: ");
|
||||
|
||||
// Check that this is not null
|
||||
assert(trees.back().get());
|
||||
return trees.back();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class DERIVEDFACTOR>
|
||||
typename EliminationTree<FACTOR>::shared_ptr
|
||||
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
|
||||
|
||||
gttic(ET_Create2);
|
||||
// Build variable index
|
||||
const VariableIndex variableIndex(factorGraph);
|
||||
|
||||
// Build elimination tree
|
||||
return Create(factorGraph, variableIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
void EliminationTree<FACTORGRAPH>::print(const std::string& name,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << name << " (" << formatter(key_) << ")" << std::endl;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
factor->print(name + " ", formatter); }
|
||||
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
|
||||
child->print(name + " ", formatter); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& expected, double tol) const {
|
||||
if(this->key_ == expected.key_ && this->factors_ == expected.factors_
|
||||
&& this->subTrees_.size() == expected.subTrees_.size()) {
|
||||
typename SubTrees::const_iterator this_subtree = this->subTrees_.begin();
|
||||
typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin();
|
||||
while(this_subtree != this->subTrees_.end())
|
||||
if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol))
|
||||
return false;
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
|
||||
EliminationTree<FACTOR>::eliminatePartial(typename EliminationTree<FACTOR>::Eliminate function, size_t nrToEliminate) const {
|
||||
|
||||
// call recursive routine
|
||||
gttic(ET_recursive_eliminate);
|
||||
if(nrToEliminate > this->key_ + 1)
|
||||
throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist");
|
||||
Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers
|
||||
(void)eliminate_(function, conditionals); // modify in place
|
||||
gttoc(ET_recursive_eliminate);
|
||||
|
||||
// Add conditionals to BayesNet
|
||||
gttic(assemble_BayesNet);
|
||||
typename BayesNet::shared_ptr bayesNet(new BayesNet);
|
||||
BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) {
|
||||
if(conditional)
|
||||
bayesNet->push_back(conditional);
|
||||
}
|
||||
gttoc(assemble_BayesNet);
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
|
||||
EliminationTree<FACTOR>::eliminate(Eliminate function) const {
|
||||
size_t nrConditionals = this->key_ + 1; // root key has highest index
|
||||
return eliminatePartial(function, nrConditionals);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,290 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminationTree-inl.h
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @date Oct 13, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <stack>
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/inference-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
|
||||
EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
|
||||
const boost::shared_ptr<BayesNetType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
|
||||
{
|
||||
// This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
|
||||
|
||||
assert(childrenResults.size() == children.size());
|
||||
|
||||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(factors.size() + children.size());
|
||||
gatheredFactors.push_back(factors.begin(), factors.end());
|
||||
gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
|
||||
|
||||
// Do dense elimination step
|
||||
std::vector<Key> keyAsVector(1); keyAsVector[0] = key;
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
function(gatheredFactors, Ordering(keyAsVector));
|
||||
|
||||
// Add conditional to BayesNet
|
||||
output->push_back(eliminationResult.first);
|
||||
|
||||
// Return result
|
||||
return eliminationResult.second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
void EliminationTree<BAYESNET,GRAPH>::Node::print(
|
||||
const std::string& str, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
std::cout << str << "(" << keyFormatter(key) << ")\n";
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors) {
|
||||
if(factor)
|
||||
factor->print(str);
|
||||
else
|
||||
std::cout << str << "null factor\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
EliminationTree<BAYESNET,GRAPH>::EliminationTree(const FactorGraphType& graph,
|
||||
const VariableIndex& structure, const Ordering& order)
|
||||
{
|
||||
gttic(EliminationTree_Contructor);
|
||||
|
||||
// Number of factors and variables - NOTE in the case of partial elimination, n here may
|
||||
// be fewer variables than are actually present in the graph.
|
||||
const size_t m = graph.size();
|
||||
const size_t n = order.size();
|
||||
|
||||
static const size_t none = std::numeric_limits<size_t>::max();
|
||||
|
||||
// Allocate result parent vector and vector of last factor columns
|
||||
std::vector<sharedNode> nodes(n);
|
||||
std::vector<size_t> parents(n, none);
|
||||
std::vector<size_t> prevCol(m, none);
|
||||
std::vector<bool> factorUsed(m, false);
|
||||
|
||||
try {
|
||||
// for column j \in 1 to n do
|
||||
for (size_t j = 0; j < n; j++)
|
||||
{
|
||||
// Retrieve the factors involving this variable and create the current node
|
||||
const VariableIndex::Factors& factors = structure[order[j]];
|
||||
nodes[j] = boost::make_shared<Node>();
|
||||
nodes[j]->key = order[j];
|
||||
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(const size_t i, factors) {
|
||||
// If we already hit a variable in this factor, make the subtree containing the previous
|
||||
// variable in this factor a child of the current node. This means that the variables
|
||||
// eliminated earlier in the factor depend on the later variables in the factor. If we
|
||||
// haven't yet hit a variable in this factor, we add the factor to the current node.
|
||||
// TODO: Store root shortcuts instead of parents.
|
||||
if (prevCol[i] != none) {
|
||||
size_t k = prevCol[i];
|
||||
// Find root r of the current tree that contains k. Use raw pointers in computing the
|
||||
// parents to avoid changing the reference counts while traversing up the tree.
|
||||
size_t r = k;
|
||||
while (parents[r] != none)
|
||||
r = parents[r];
|
||||
// If the root of the subtree involving this node is actually the current node,
|
||||
// TODO: what does this mean? forest?
|
||||
if (r != j) {
|
||||
// Now that we found the root, hook up parent and child pointers in the nodes.
|
||||
parents[r] = j;
|
||||
nodes[j]->children.push_back(nodes[r]);
|
||||
}
|
||||
} else {
|
||||
// Add the current factor to the current node since we are at the first variable in this
|
||||
// factor.
|
||||
nodes[j]->factors.push_back(graph[i]);
|
||||
factorUsed[i] = true;
|
||||
}
|
||||
prevCol[i] = j;
|
||||
}
|
||||
}
|
||||
} catch(std::invalid_argument& e) {
|
||||
// If this is thrown from structure[order[j]] above, it means that it was requested to
|
||||
// eliminate a variable not present in the graph, so throw a more informative error message.
|
||||
(void)e; // Prevent unused variable warning
|
||||
throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
|
||||
} catch(...) {
|
||||
throw;
|
||||
}
|
||||
|
||||
// Find roots
|
||||
assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what
|
||||
for(size_t j = 0; j < n; ++j)
|
||||
if(parents[j] == none)
|
||||
roots_.push_back(nodes[j]);
|
||||
|
||||
// Gather remaining factors (exclude null factors)
|
||||
for(size_t i = 0; i < m; ++i)
|
||||
if(!factorUsed[i] && graph[i])
|
||||
remainingFactors_.push_back(graph[i]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
EliminationTree<BAYESNET,GRAPH>::EliminationTree(
|
||||
const FactorGraphType& factorGraph, const Ordering& order)
|
||||
{
|
||||
gttic(ET_Create2);
|
||||
// Build variable index first
|
||||
const VariableIndex variableIndex(factorGraph);
|
||||
This temp(factorGraph, variableIndex, order);
|
||||
this->swap(temp); // Swap in the tree, and temp will be deleted
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
EliminationTree<BAYESNET,GRAPH>&
|
||||
EliminationTree<BAYESNET,GRAPH>::operator=(const EliminationTree<BAYESNET,GRAPH>& other)
|
||||
{
|
||||
// Start by duplicating the tree.
|
||||
roots_ = treeTraversal::CloneForest(other);
|
||||
|
||||
// Assign the remaining factors - these are pointers to factors in the original factor graph and
|
||||
// we do not clone them.
|
||||
remainingFactors_ = other.remainingFactors_;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
|
||||
EliminationTree<BAYESNET,GRAPH>::eliminate(Eliminate function) const
|
||||
{
|
||||
gttic(EliminationTree_eliminate);
|
||||
// Allocate result
|
||||
boost::shared_ptr<BayesNetType> result = boost::make_shared<BayesNetType>();
|
||||
|
||||
// Run tree elimination algorithm
|
||||
std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
|
||||
|
||||
// Add remaining factors that were not involved with eliminated variables
|
||||
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
|
||||
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
|
||||
{
|
||||
treeTraversal::PrintForest(*this, name, formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
|
||||
{
|
||||
// Depth-first-traversal stacks
|
||||
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2;
|
||||
|
||||
// Add roots in sorted order
|
||||
{
|
||||
FastMap<Key,sharedNode> keys;
|
||||
BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); }
|
||||
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
|
||||
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
|
||||
}
|
||||
{
|
||||
FastMap<Key,sharedNode> keys;
|
||||
BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
|
||||
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
|
||||
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
|
||||
}
|
||||
|
||||
// Traverse, adding children in sorted order
|
||||
while(!stack1.empty() && !stack2.empty()) {
|
||||
// Pop nodes
|
||||
sharedNode node1 = stack1.top();
|
||||
stack1.pop();
|
||||
sharedNode node2 = stack2.top();
|
||||
stack2.pop();
|
||||
|
||||
// Compare nodes
|
||||
if(node1->key != node2->key)
|
||||
return false;
|
||||
if(node1->factors.size() != node2->factors.size()) {
|
||||
return false;
|
||||
} else {
|
||||
for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
|
||||
it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
|
||||
{
|
||||
if(*it1 && *it2) {
|
||||
if(!(*it1)->equals(**it2, tol))
|
||||
return false;
|
||||
} else if((*it1 && !*it2) || (*it2 && !*it1)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add children in sorted order
|
||||
{
|
||||
FastMap<Key,sharedNode> keys;
|
||||
BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); }
|
||||
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
|
||||
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
|
||||
}
|
||||
{
|
||||
FastMap<Key,sharedNode> keys;
|
||||
BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); }
|
||||
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
|
||||
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
|
||||
}
|
||||
}
|
||||
|
||||
// If either stack is not empty, the number of nodes differed
|
||||
if(!stack1.empty() || !stack2.empty())
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
void EliminationTree<BAYESNET,GRAPH>::swap(This& other) {
|
||||
roots_.swap(other.roots_);
|
||||
remainingFactors_.swap(other.remainingFactors_);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -1,187 +1,167 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminationTree.h
|
||||
* @author Frank Dellaert
|
||||
* @date Oct 13, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
class EliminationTreeTester; // for unit tests, see testEliminationTree
|
||||
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An elimination tree is a data structure used intermediately during
|
||||
* elimination. In future versions it will be used to save work between
|
||||
* multiple eliminations.
|
||||
*
|
||||
* When a variable is eliminated, a new factor is created by combining that
|
||||
* variable's neighboring factors. The new combined factor involves the combined
|
||||
* factors' involved variables. When the lowest-ordered one of those variables
|
||||
* is eliminated, it consumes that combined factor. In the elimination tree,
|
||||
* that lowest-ordered variable is the parent of the variable that was eliminated to
|
||||
* produce the combined factor. This yields a tree in general, and not a chain
|
||||
* because of the implicit sparse structure of the resulting Bayes net.
|
||||
*
|
||||
* This structure is examined even more closely in a JunctionTree, which
|
||||
* additionally identifies cliques in the chordal Bayes net.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FACTOR>
|
||||
class EliminationTree {
|
||||
|
||||
public:
|
||||
|
||||
typedef EliminationTree<FACTOR> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
|
||||
typedef FACTOR Factor;
|
||||
typedef typename FACTOR::KeyType KeyType;
|
||||
|
||||
/** Typedef for an eliminate subroutine */
|
||||
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
|
||||
|
||||
private:
|
||||
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
|
||||
typedef FastList<sharedFactor> Factors;
|
||||
typedef FastList<shared_ptr> SubTrees;
|
||||
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
|
||||
|
||||
Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor?
|
||||
Factors factors_; ///< factors associated with root
|
||||
SubTrees subTrees_; ///< sub-trees
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Named constructor to build the elimination tree of a factor graph using
|
||||
* pre-computed column structure.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
* @param structure The set of factors involving each variable. If this is not
|
||||
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
|
||||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
template<class DERIVEDFACTOR>
|
||||
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& structure);
|
||||
|
||||
/** Named constructor to build the elimination tree of a factor graph. Note
|
||||
* that this has to compute the column structure as a VariableIndex, so if you
|
||||
* already have this precomputed, use the Create(const FactorGraph<DERIVEDFACTOR>&, const VariableIndex&)
|
||||
* named constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
template<class DERIVEDFACTOR>
|
||||
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors to a Bayes Net
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The BayesNet resulting from elimination
|
||||
*/
|
||||
typename BayesNet::shared_ptr eliminate(Eliminate function) const;
|
||||
|
||||
/** Eliminate the factors to a Bayes Net and return the remaining factor
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The BayesNet resulting from elimination and the remaining factor
|
||||
*/
|
||||
typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print the tree to cout */
|
||||
void print(const std::string& name = "EliminationTree: ",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
|
||||
/** Test whether the tree is equal to another */
|
||||
bool equals(const EliminationTree& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
/** default constructor, private, as you should use Create below */
|
||||
EliminationTree(Index key = 0) : key_(key) {}
|
||||
|
||||
/**
|
||||
* Static internal function to build a vector of parent pointers using the
|
||||
* algorithm of Gilbert et al., 2001, BIT.
|
||||
*/
|
||||
static std::vector<Index> ComputeParents(const VariableIndex& structure);
|
||||
|
||||
/** add a factor, for Create use only */
|
||||
void add(const sharedFactor& factor) { factors_.push_back(factor); }
|
||||
|
||||
/** add a subtree, for Create use only */
|
||||
void add(const shared_ptr& child) { subTrees_.push_back(child); }
|
||||
|
||||
/**
|
||||
* Recursive routine that eliminates the factors arranged in an elimination tree
|
||||
* @param Conditionals is a vector of shared pointers that will be modified in place
|
||||
*/
|
||||
sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;
|
||||
|
||||
/// Allow access to constructor and add methods for testing purposes
|
||||
friend class ::EliminationTreeTester;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* An exception thrown when attempting to eliminate a disconnected factor
|
||||
* graph, which is not currently possible in gtsam. If you need to work with
|
||||
* disconnected graphs, a workaround is to create zero-information factors to
|
||||
* bridge the disconnects. To do this, create any factor type (e.g.
|
||||
* BetweenFactor or RangeFactor) with the noise model
|
||||
* <tt>sharedPrecision(dim, 0.0)</tt>, where \c dim is the appropriate
|
||||
* dimensionality for that factor.
|
||||
*/
|
||||
struct DisconnectedGraphException : public std::exception {
|
||||
DisconnectedGraphException() {}
|
||||
virtual ~DisconnectedGraphException() throw() {}
|
||||
|
||||
/// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."
|
||||
virtual const char* what() const throw() {
|
||||
return
|
||||
"Attempting to eliminate a disconnected graph - this is not currently possible in\n"
|
||||
"GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n"
|
||||
"will affect the symbolic structure and solving complexity of the graph but not\n"
|
||||
"the solution. To do this, create BetweenFactor's with zero-precision noise\n"
|
||||
"models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; }
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/inference/EliminationTree-inl.h>
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 EliminationTree.h
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @date Oct 13, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
class EliminationTreeTester; // for unit tests, see testEliminationTree
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class VariableIndex;
|
||||
class Ordering;
|
||||
|
||||
/**
|
||||
* An elimination tree is a data structure used intermediately during
|
||||
* elimination. In future versions it will be used to save work between
|
||||
* multiple eliminations.
|
||||
*
|
||||
* When a variable is eliminated, a new factor is created by combining that
|
||||
* variable's neighboring factors. The new combined factor involves the combined
|
||||
* factors' involved variables. When the lowest-ordered one of those variables
|
||||
* is eliminated, it consumes that combined factor. In the elimination tree,
|
||||
* that lowest-ordered variable is the parent of the variable that was eliminated to
|
||||
* produce the combined factor. This yields a tree in general, and not a chain
|
||||
* because of the implicit sparse structure of the resulting Bayes net.
|
||||
*
|
||||
* This structure is examined even more closely in a JunctionTree, which
|
||||
* additionally identifies cliques in the chordal Bayes net.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class BAYESNET, class GRAPH>
|
||||
class EliminationTree
|
||||
{
|
||||
protected:
|
||||
typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
public:
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
|
||||
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
|
||||
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
|
||||
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
typedef typename GRAPH::Eliminate Eliminate;
|
||||
|
||||
struct Node {
|
||||
typedef std::vector<sharedFactor> Factors;
|
||||
typedef std::vector<boost::shared_ptr<Node> > Children;
|
||||
|
||||
Key key; ///< key associated with root
|
||||
Factors factors; ///< factors associated with root
|
||||
Children children; ///< sub-trees
|
||||
|
||||
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
|
||||
|
||||
void print(const std::string& str, const KeyFormatter& keyFormatter) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
|
||||
|
||||
protected:
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
std::vector<sharedNode> roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Build the elimination tree of a factor graph using pre-computed column structure.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
* @param structure The set of factors involving each variable. If this is not
|
||||
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
|
||||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
EliminationTree(const FactorGraphType& factorGraph,
|
||||
const VariableIndex& structure, const Ordering& order);
|
||||
|
||||
/** Build the elimination tree of a factor graph. Note that this has to compute the column
|
||||
* structure as a VariableIndex, so if you already have this precomputed, use the other
|
||||
* constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
EliminationTree(const This& other) { *this = other; }
|
||||
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
|
||||
* are copied, factors are not cloned. */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors to a Bayes net and remaining factor graph
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The Bayes net and factor graph resulting from elimination
|
||||
*/
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminate(Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print the tree to cout */
|
||||
void print(const std::string& name = "EliminationTree: ",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
protected:
|
||||
/** Test whether the tree is equal to another */
|
||||
bool equals(const This& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const std::vector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
/** Swap the data of this tree with another one, this operation is very fast. */
|
||||
void swap(This& other);
|
||||
|
||||
protected:
|
||||
/// Protected default constructor
|
||||
EliminationTree() {}
|
||||
|
||||
private:
|
||||
/// Allow access to constructor and add methods for testing purposes
|
||||
friend class ::EliminationTreeTester;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,108 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Factor-inl.h
|
||||
* @author Richard Roberts
|
||||
* @date Sep 1, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Factor.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
Factor<KEY>::Factor(const Factor<KEY>& f) :
|
||||
keys_(f.keys_) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
Factor<KEY>::Factor(const ConditionalType& c) :
|
||||
keys_(c.keys()) {
|
||||
// assert(c.nrFrontals() == 1);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
void Factor<KEY>::assertInvariants() const {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
// Check that keys are all unique
|
||||
std::multiset<KeyType> nonunique(keys_.begin(), keys_.end());
|
||||
std::set<KeyType> unique(keys_.begin(), keys_.end());
|
||||
bool correct = (nonunique.size() == unique.size())
|
||||
&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
|
||||
if (!correct) throw std::logic_error(
|
||||
"Factor::assertInvariants: Factor contains duplicate keys");
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
void Factor<KEY>::print(const std::string& s,
|
||||
const IndexFormatter& formatter) const {
|
||||
printKeys(s,formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
void Factor<KEY>::printKeys(const std::string& s, const IndexFormatter& formatter) const {
|
||||
std::cout << s << " ";
|
||||
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
bool Factor<KEY>::equals(const This& other, double tol) const {
|
||||
return keys_ == other.keys_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef TRACK_ELIMINATE
|
||||
template<typename KEY>
|
||||
template<class COND>
|
||||
typename COND::shared_ptr Factor<KEY>::eliminateFirst() {
|
||||
assert(!keys_.empty());
|
||||
assertInvariants();
|
||||
KEY eliminated = keys_.front();
|
||||
keys_.erase(keys_.begin());
|
||||
assertInvariants();
|
||||
return typename COND::shared_ptr(new COND(eliminated, keys_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEY>
|
||||
template<class COND>
|
||||
typename BayesNet<COND>::shared_ptr Factor<KEY>::eliminate(size_t nrFrontals) {
|
||||
assert(keys_.size() >= nrFrontals);
|
||||
assertInvariants();
|
||||
typename BayesNet<COND>::shared_ptr fragment(new BayesNet<COND> ());
|
||||
const_iterator it = this->begin();
|
||||
for (KEY n = 0; n < nrFrontals; ++n, ++it)
|
||||
fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1));
|
||||
if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(),
|
||||
fragment->back()->endParents());
|
||||
assertInvariants();
|
||||
return fragment;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Factor.cpp
|
||||
* @brief The base class for all factors
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/Factor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Factor::print(const std::string& s, const KeyFormatter& formatter) const
|
||||
{
|
||||
return this->printKeys(s, formatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
|
||||
std::cout << s << " ";
|
||||
BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Factor::equals(const This& other, double tol) const {
|
||||
return keys_ == other.keys_;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -21,212 +21,151 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/function/function1.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class KEY> class Conditional;
|
||||
|
||||
/**
|
||||
* This is the base class for all factor types. It is templated on a KEY type,
|
||||
* which will be the type used to label variables. Key types currently in use
|
||||
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
|
||||
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
|
||||
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
|
||||
* though currently only IndexFactor and IndexConditional derive from this
|
||||
* class, using Index keys. This class does not store any data other than its
|
||||
* keys. Derived classes store data such as matrices and probability tables.
|
||||
*
|
||||
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
|
||||
* typedefs to refer to the associated conditional and shared_ptr types of the
|
||||
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
|
||||
*
|
||||
* This class is \b not virtual for performance reasons - derived symbolic classes,
|
||||
* IndexFactor and IndexConditional, need to be created and destroyed quickly
|
||||
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<typename KEY>
|
||||
class Factor {
|
||||
|
||||
public:
|
||||
|
||||
typedef KEY KeyType; ///< The KEY template parameter
|
||||
typedef Factor<KeyType> This; ///< This class
|
||||
|
||||
/**
|
||||
* Typedef to the conditional type obtained by eliminating this factor,
|
||||
* derived classes must redefine this.
|
||||
* This is the base class for all factor types. It is templated on a KEY type,
|
||||
* which will be the type used to label variables. Key types currently in use
|
||||
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
|
||||
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
|
||||
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
|
||||
* though currently only IndexFactor and IndexConditional derive from this
|
||||
* class, using Index keys. This class does not store any data other than its
|
||||
* keys. Derived classes store data such as matrices and probability tables.
|
||||
*
|
||||
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
|
||||
* typedefs to refer to the associated conditional and shared_ptr types of the
|
||||
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
|
||||
*
|
||||
* This class is \b not virtual for performance reasons - derived symbolic classes,
|
||||
* IndexFactor and IndexConditional, need to be created and destroyed quickly
|
||||
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
typedef Conditional<KeyType> ConditionalType;
|
||||
class GTSAM_EXPORT Factor
|
||||
{
|
||||
|
||||
/// A shared_ptr to this class, derived classes must redefine this.
|
||||
typedef boost::shared_ptr<Factor> shared_ptr;
|
||||
private:
|
||||
// These typedefs are private because they must be overridden in derived classes.
|
||||
typedef Factor This; ///< This class
|
||||
typedef boost::shared_ptr<Factor> shared_ptr; ///< A shared_ptr to this class.
|
||||
|
||||
/// Iterator over keys
|
||||
typedef typename std::vector<KeyType>::iterator iterator;
|
||||
public:
|
||||
/// Iterator over keys
|
||||
typedef std::vector<Key>::iterator iterator;
|
||||
|
||||
/// Const iterator over keys
|
||||
typedef typename std::vector<KeyType>::const_iterator const_iterator;
|
||||
/// Const iterator over keys
|
||||
typedef std::vector<Key>::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
protected:
|
||||
|
||||
/// The keys involved in this factor
|
||||
std::vector<KeyType> keys_;
|
||||
/// The keys involved in this factor
|
||||
std::vector<Key> keys_;
|
||||
|
||||
public:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
/** Default constructor for I/O */
|
||||
Factor() {}
|
||||
|
||||
/** Copy constructor */
|
||||
Factor(const This& f);
|
||||
/** Construct factor 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>
|
||||
Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
|
||||
|
||||
/** Construct from conditional, calls ConditionalType::toFactor() */
|
||||
Factor(const ConditionalType& c);
|
||||
/** Construct factor from iterator keys. This constructor may be used internally from derived
|
||||
* factor constructors, although our code currently does not use this. */
|
||||
template<typename ITERATOR>
|
||||
Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {}
|
||||
|
||||
/** Default constructor for I/O */
|
||||
Factor() {}
|
||||
/** Construct factor from container of keys. This is called internally from derived factor static
|
||||
* factor methods, as a workaround for not being able to call the protected constructors above. */
|
||||
template<typename CONTAINER>
|
||||
static Factor FromKeys(const CONTAINER& keys) {
|
||||
return Factor(keys.begin(), keys.end()); }
|
||||
|
||||
/** Construct unary factor */
|
||||
Factor(KeyType key) : keys_(1) {
|
||||
keys_[0] = key; assertInvariants(); }
|
||||
/** Construct factor from iterator keys. This is called internally from derived factor static
|
||||
* factor methods, as a workaround for not being able to call the protected constructors above. */
|
||||
template<typename ITERATOR>
|
||||
static Factor FromIterators(ITERATOR first, ITERATOR last) {
|
||||
return Factor(first, last); }
|
||||
|
||||
/** Construct binary factor */
|
||||
Factor(KeyType key1, KeyType key2) : keys_(2) {
|
||||
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
|
||||
/// @}
|
||||
|
||||
/** Construct ternary factor */
|
||||
Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Construct 4-way factor */
|
||||
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
|
||||
/// First key
|
||||
Key front() const { return keys_.front(); }
|
||||
|
||||
/** Construct 5-way factor */
|
||||
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); }
|
||||
/// Last key
|
||||
Key back() const { return keys_.back(); }
|
||||
|
||||
/** Construct 6-way factor */
|
||||
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); }
|
||||
/// find
|
||||
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
/// Access the factor's involved variable keys
|
||||
const std::vector<Key>& keys() const { return keys_; }
|
||||
|
||||
/** Construct n-way factor */
|
||||
Factor(const std::set<KeyType>& keys) {
|
||||
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
|
||||
assertInvariants();
|
||||
}
|
||||
/** Iterator at beginning of involved variable keys */
|
||||
const_iterator begin() const { return keys_.begin(); }
|
||||
|
||||
/** Construct n-way factor */
|
||||
Factor(const std::vector<KeyType>& keys) : keys_(keys) {
|
||||
assertInvariants();
|
||||
}
|
||||
/** Iterator at end of involved variable keys */
|
||||
const_iterator end() const { return keys_.end(); }
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
|
||||
keys_(beginKey, endKey) { assertInvariants(); }
|
||||
/**
|
||||
* @return the number of variables involved in this factor
|
||||
*/
|
||||
size_t size() const { return keys_.size(); }
|
||||
|
||||
/// @}
|
||||
/// @}
|
||||
|
||||
#ifdef TRACK_ELIMINATE
|
||||
/**
|
||||
* eliminate the first variable involved in this factor
|
||||
* @return a conditional on the eliminated variable
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
typename CONDITIONAL::shared_ptr eliminateFirst();
|
||||
|
||||
/**
|
||||
* eliminate the first nrFrontals frontal variables.
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
|
||||
#endif
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/// print
|
||||
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// First key
|
||||
KeyType front() const { return keys_.front(); }
|
||||
/// print only keys
|
||||
void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// Last key
|
||||
KeyType back() const { return keys_.back(); }
|
||||
protected:
|
||||
/// check equality
|
||||
bool equals(const This& other, double tol = 1e-9) const;
|
||||
|
||||
/// find
|
||||
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
|
||||
/// @}
|
||||
|
||||
/// Access the factor's involved variable keys
|
||||
const std::vector<KeyType>& keys() const { return keys_; }
|
||||
public:
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** iterators */
|
||||
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
|
||||
const_iterator end() const { return keys_.end(); } ///TODO: comment
|
||||
/** @return keys involved in this factor */
|
||||
std::vector<Key>& keys() { return keys_; }
|
||||
|
||||
/**
|
||||
* @return the number of variables involved in this factor
|
||||
*/
|
||||
size_t size() const { return keys_.size(); }
|
||||
/** Iterator at beginning of involved variable keys */
|
||||
iterator begin() { return keys_.begin(); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/** Iterator at end of involved variable keys */
|
||||
iterator end() { return keys_.end(); }
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "Factor",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(keys_);
|
||||
}
|
||||
|
||||
/// print only keys
|
||||
void printKeys(const std::string& s = "Factor",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
/// @}
|
||||
|
||||
/// check equality
|
||||
bool equals(const This& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* @return keys involved in this factor
|
||||
*/
|
||||
std::vector<KeyType>& keys() { return keys_; }
|
||||
|
||||
/** mutable iterators */
|
||||
iterator begin() { return keys_.begin(); } ///TODO: comment
|
||||
iterator end() { return keys_.end(); } ///TODO: comment
|
||||
|
||||
protected:
|
||||
friend class JacobianFactor;
|
||||
friend class HessianFactor;
|
||||
|
||||
/// Internal consistency check that is run frequently when in debug mode.
|
||||
/// If NDEBUG is defined, this is empty and optimized out.
|
||||
void assertInvariants() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(keys_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/inference/Factor-inl.h>
|
||||
|
|
|
|||
|
|
@ -1,288 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 FactorGraph-inl.h
|
||||
* This is a template definition file, include it where needed (only!)
|
||||
* so that the appropriate code is generated and link errors avoided.
|
||||
* @brief Factor Graph Base Class
|
||||
* @author Carlos Nieto
|
||||
* @author Frank Dellaert
|
||||
* @author Alireza Fathi
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/iterator/transform_iterator.hpp>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <list>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
factors_.reserve(bayesNet.size());
|
||||
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
|
||||
this->push_back(cond->toFactor());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraph<FACTOR>::print(const std::string& s,
|
||||
const IndexFormatter& formatter) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
std::stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
|
||||
/** check whether the two factor graphs have the same number of factors_ */
|
||||
if (factors_.size() != fg.size()) return false;
|
||||
|
||||
/** check whether the factors_ are the same */
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
// TODO: Doesn't this force order of factor insertion?
|
||||
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
if (f1 == NULL && f2 == NULL) continue;
|
||||
if (f1 == NULL || f2 == NULL) return false;
|
||||
if (!f1->equals(*f2, tol)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
size_t FactorGraph<FACTOR>::nrFactors() const {
|
||||
size_t size_ = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor) size_++;
|
||||
return size_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
std::set<typename FACTOR::KeyType> FactorGraph<FACTOR>::keys() const {
|
||||
std::set<KeyType> allKeys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor) {
|
||||
BOOST_FOREACH(Index j, factor->keys())
|
||||
allKeys.insert(j);
|
||||
}
|
||||
return allKeys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> >
|
||||
FactorGraph<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
|
||||
{
|
||||
// Build variable index
|
||||
VariableIndex variableIndex(*this);
|
||||
|
||||
// Find first variable
|
||||
Index firstIndex = 0;
|
||||
while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty())
|
||||
++ firstIndex;
|
||||
|
||||
// Check that number of variables is in bounds
|
||||
if(firstIndex + nFrontals > variableIndex.size())
|
||||
throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph.");
|
||||
|
||||
// Get set of involved factors
|
||||
FastSet<size_t> involvedFactorIs;
|
||||
for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) {
|
||||
BOOST_FOREACH(size_t i, variableIndex[j]) {
|
||||
involvedFactorIs.insert(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Separate factors into involved and remaining
|
||||
FactorGraph<FactorType> involvedFactors;
|
||||
FactorGraph<FactorType> remainingFactors;
|
||||
FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin();
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) {
|
||||
// If the current factor is involved, add it to involved and increment involved iterator
|
||||
involvedFactors.push_back((*this)[i]);
|
||||
++ involvedFactorIsIt;
|
||||
} else {
|
||||
// If not involved, add to remaining
|
||||
remainingFactors.push_back((*this)[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Do dense elimination on the involved factors
|
||||
typename FactorGraph<FactorType>::EliminationResult eliminationResult =
|
||||
eliminate(involvedFactors, nFrontals);
|
||||
|
||||
// Add the remaining factor back into the factor graph
|
||||
remainingFactors.push_back(eliminationResult.second);
|
||||
|
||||
// Return the eliminated factor and remaining factor graph
|
||||
return std::make_pair(eliminationResult.first, remainingFactors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
std::pair<typename FactorGraph<FACTOR>::sharedConditional, FactorGraph<FACTOR> >
|
||||
FactorGraph<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
|
||||
boost::optional<const VariableIndex&> variableIndex_) const
|
||||
{
|
||||
const VariableIndex& variableIndex =
|
||||
variableIndex_ ? *variableIndex_ : VariableIndex(*this);
|
||||
|
||||
// First find the involved factors
|
||||
FactorGraph<FACTOR> involvedFactors;
|
||||
Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors
|
||||
|
||||
// First get the indices of the involved factors, but uniquely in a set
|
||||
FastSet<size_t> involvedFactorIndices;
|
||||
BOOST_FOREACH(Index variable, variables) {
|
||||
involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); }
|
||||
|
||||
// Add the factors themselves to involvedFactors and update largest index
|
||||
involvedFactors.reserve(involvedFactorIndices.size());
|
||||
BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) {
|
||||
const sharedFactor factor = this->at(factorIndex);
|
||||
involvedFactors.push_back(factor); // Add involved factor
|
||||
highestInvolvedVariable = std::max( // Updated largest index
|
||||
highestInvolvedVariable,
|
||||
*std::max_element(factor->begin(), factor->end()));
|
||||
}
|
||||
|
||||
sharedConditional conditional;
|
||||
sharedFactor remainingFactor;
|
||||
if(involvedFactors.size() > 0) {
|
||||
// Now permute the variables to be eliminated to the front of the ordering
|
||||
Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
|
||||
Permutation toFrontInverse = *toFront.inverse();
|
||||
BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
|
||||
factor->permuteWithInverse(toFrontInverse); }
|
||||
|
||||
// Eliminate into conditional and remaining factor
|
||||
EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
|
||||
conditional = eliminated.first;
|
||||
remainingFactor = eliminated.second;
|
||||
|
||||
// Undo the permutation
|
||||
BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
|
||||
factor->permuteWithInverse(toFront); }
|
||||
conditional->permuteWithInverse(toFront);
|
||||
remainingFactor->permuteWithInverse(toFront);
|
||||
} else {
|
||||
// Eliminate 0 variables
|
||||
EliminationResult eliminated = eliminateFcn(involvedFactors, 0);
|
||||
conditional = eliminated.first;
|
||||
remainingFactor = eliminated.second;
|
||||
}
|
||||
|
||||
// Build the remaining graph, without the removed factors
|
||||
FactorGraph<FACTOR> remainingGraph;
|
||||
remainingGraph.reserve(this->size() - involvedFactors.size() + 1);
|
||||
FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i)
|
||||
++ involvedFactorIndexIt;
|
||||
else
|
||||
remainingGraph.push_back(this->at(i));
|
||||
}
|
||||
|
||||
// Add the remaining factor if it is not empty.
|
||||
if(remainingFactor->size() != 0)
|
||||
remainingGraph.push_back(remainingFactor);
|
||||
|
||||
return std::make_pair(conditional, remainingGraph);
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraph<FACTOR>::replace(size_t index, sharedFactor factor) {
|
||||
if (index >= factors_.size()) throw std::invalid_argument(boost::str(
|
||||
boost::format("Factor graph does not contain a factor with index %d.")
|
||||
% index));
|
||||
// Replace the factor
|
||||
factors_[index] = factor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTORGRAPH>
|
||||
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) {
|
||||
// create new linear factor graph equal to the first one
|
||||
FACTORGRAPH fg = fg1;
|
||||
|
||||
// add the second factors_ in the graph
|
||||
fg.push_back(fg2);
|
||||
|
||||
return fg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVEDFACTOR, class KEY>
|
||||
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph<DERIVEDFACTOR>& factors,
|
||||
const FastMap<KEY, std::vector<KEY> >& variableSlots) {
|
||||
|
||||
typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair;
|
||||
// Local functional for getting keys out of key-value pairs
|
||||
struct Local { static KEY FirstOf(const KeySlotPair& pr) { return pr.first; } };
|
||||
|
||||
return typename DERIVEDFACTOR::shared_ptr(new DERIVEDFACTOR(
|
||||
boost::make_transform_iterator(variableSlots.begin(), &Local::FirstOf),
|
||||
boost::make_transform_iterator(variableSlots.end(), &Local::FirstOf)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Recursive function to add factors in cliques to vector of factors_io
|
||||
template<class FACTOR, class CONDITIONAL, class CLIQUE>
|
||||
void _FactorGraph_BayesTree_adder(
|
||||
std::vector<typename boost::shared_ptr<FACTOR> >& factors_io,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add factor from this clique
|
||||
factors_io.push_back((*clique)->toFactor());
|
||||
|
||||
// Traverse children
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children())
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesTree<CONDITIONAL,CLIQUE>& bayesTree) {
|
||||
factors_.reserve(bayesTree.size());
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 FactorGraph-inl.h
|
||||
* @brief Factor Graph Base Class
|
||||
* @author Carlos Nieto
|
||||
* @author Frank Dellaert
|
||||
* @author Alireza Fathi
|
||||
* @author Michael Kaess
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <sstream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
std::stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i])
|
||||
factors_[i]->print(ss.str(), formatter);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
|
||||
/** check whether the two factor graphs have the same number of factors_ */
|
||||
if (factors_.size() != fg.size()) return false;
|
||||
|
||||
/** check whether the factors_ are the same */
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
// TODO: Doesn't this force order of factor insertion?
|
||||
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
if (f1 == NULL && f2 == NULL) continue;
|
||||
if (f1 == NULL || f2 == NULL) return false;
|
||||
if (!f1->equals(*f2, tol)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
size_t FactorGraph<FACTOR>::nrFactors() const {
|
||||
size_t size_ = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor) size_++;
|
||||
return size_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
FastSet<Key> FactorGraph<FACTOR>::keys() const {
|
||||
FastSet<Key> allKeys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor)
|
||||
allKeys.insert(factor->begin(), factor->end());
|
||||
return allKeys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -15,25 +15,59 @@
|
|||
* @author Carlos Nieto
|
||||
* @author Christian Potthast
|
||||
* @author Michael Kaess
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <set>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <type_traits>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
||||
class VariableIndex;
|
||||
// Forward declarations
|
||||
template<class CLIQUE> class BayesTree;
|
||||
|
||||
/** Helper */
|
||||
template<class C>
|
||||
class CRefCallPushBack
|
||||
{
|
||||
C& obj;
|
||||
public:
|
||||
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:
|
||||
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:
|
||||
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.
|
||||
|
|
@ -44,62 +78,64 @@ class VariableIndex;
|
|||
class FactorGraph {
|
||||
|
||||
public:
|
||||
|
||||
typedef FACTOR FactorType; ///< factor type
|
||||
typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
|
||||
typedef sharedFactor value_type;
|
||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
/** typedef for elimination result */
|
||||
typedef std::pair<sharedConditional, sharedFactor> EliminationResult;
|
||||
|
||||
/** typedef for an eliminate subroutine */
|
||||
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
|
||||
private:
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
|
||||
|
||||
protected:
|
||||
|
||||
/** concept check, makes sure FACTOR defines print and equals */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
|
||||
/** Collection of factors */
|
||||
std::vector<sharedFactor> factors_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructor
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
FactorGraph() {}
|
||||
|
||||
/** Constructor from iterator over factors (shared_ptr or plain objects) */
|
||||
template<typename ITERATOR>
|
||||
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
|
||||
|
||||
/** Construct from container of factors (shared_ptr or plain objects) */
|
||||
template<class CONTAINER>
|
||||
explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
// TODO: are these needed?
|
||||
|
||||
/**
|
||||
* @brief Constructor from a Bayes net
|
||||
* @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
|
||||
* @return a factor graph with all the conditionals, as factors
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
///**
|
||||
// * @brief Constructor from a Bayes net
|
||||
// * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
|
||||
// * @return a factor graph with all the conditionals, as factors
|
||||
// */
|
||||
//template<class CONDITIONAL>
|
||||
//FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
/** convert from Bayes tree */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
|
||||
///** convert from Bayes tree */
|
||||
//template<class CONDITIONAL, class CLIQUE>
|
||||
//FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
|
||||
|
||||
/** convert from a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
|
||||
factors_.assign(factors.begin(), factors.end());
|
||||
}
|
||||
///** convert from a derived type */
|
||||
//template<class DERIVEDFACTOR>
|
||||
//FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
|
||||
// factors_.assign(factors.begin(), factors.end());
|
||||
//}
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Adding Factors
|
||||
/// @{
|
||||
|
||||
|
|
@ -109,69 +145,165 @@ class VariableIndex;
|
|||
*/
|
||||
void reserve(size_t size) { factors_.reserve(size); }
|
||||
|
||||
/** Add a factor */
|
||||
// TODO: are these needed?
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
template<class DERIVEDFACTOR>
|
||||
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
|
||||
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
|
||||
}
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
push_back(boost::shared_ptr<DERIVEDFACTOR>& factor) {
|
||||
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
|
||||
|
||||
/** push back many factors */
|
||||
void push_back(const This& factors) {
|
||||
factors_.insert(end(), factors.begin(), factors.end());
|
||||
}
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
void push_back(const sharedFactor& factor) {
|
||||
factors_.push_back(factor); }
|
||||
|
||||
/** push back many factors with an iterator */
|
||||
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
|
||||
template<typename ITERATOR>
|
||||
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
factors_.insert(end(), firstFactor, lastFactor);
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
|
||||
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
factors_.insert(end(), firstFactor, lastFactor); }
|
||||
|
||||
/** push back many factors as shared_ptr's in a container (factors are not copied) */
|
||||
template<typename CONTAINER>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
|
||||
push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Add a vector of derived factors
|
||||
* @param factors to add
|
||||
*/
|
||||
template<typename DERIVEDFACTOR>
|
||||
void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
|
||||
factors_.insert(end(), factors.begin(), factors.end());
|
||||
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
|
||||
* classes in favor of a type-specialized version that calls this templated function. */
|
||||
template<class CLIQUE>
|
||||
typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
|
||||
push_back(const BayesTree<CLIQUE>& bayesTree) {
|
||||
bayesTree.addFactorsToGraph(*this);
|
||||
}
|
||||
|
||||
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
|
||||
//template<class T>
|
||||
//boost::assign::list_inserter<boost::function<void(const T&)> >
|
||||
// operator+=(const T& factorOrContainer)
|
||||
//{
|
||||
// return boost::assign::make_list_inserter(
|
||||
// boost::bind(&This::push_back<T>, this, _1));
|
||||
//}
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
|
||||
boost::assign::list_inserter<RefCallPushBack<This> > >::type
|
||||
operator+=(boost::shared_ptr<DERIVEDFACTOR>& factor) {
|
||||
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
|
||||
}
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
operator+=(const sharedFactor& factor) {
|
||||
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factor);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
///** Add a factor directly using a shared_ptr */
|
||||
//boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
// operator+=(const sharedFactor& factor) {
|
||||
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
|
||||
//}
|
||||
|
||||
///** push back many factors as shared_ptr's in a container (factors are not copied) */
|
||||
//template<typename CONTAINER>
|
||||
//typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value,
|
||||
// boost::assign::list_inserter<CRefCallPushBack<This> > >::type
|
||||
// operator+=(const CONTAINER& container) {
|
||||
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
|
||||
//}
|
||||
|
||||
///** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
|
||||
// * classes in favor of a type-specialized version that calls this templated function. */
|
||||
//template<class CLIQUE>
|
||||
//boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
// operator+=(const BayesTree<CLIQUE>& bayesTree) {
|
||||
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
|
||||
//}
|
||||
|
||||
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
|
||||
* the copy). */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
push_back(const DERIVEDFACTOR& factor) {
|
||||
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
|
||||
|
||||
/** push back many factors with an iterator over plain factors (factors are copied) */
|
||||
template<typename ITERATOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
|
||||
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
for(ITERATOR f = firstFactor; f != lastFactor; ++f)
|
||||
push_back(*f);
|
||||
}
|
||||
|
||||
/** push back many factors as non-pointer objects in a container (factors are copied) */
|
||||
template<typename CONTAINER>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
|
||||
push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
|
||||
//template<class FACTOR_OR_CONTAINER>
|
||||
//boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
// operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) {
|
||||
// return boost::assign::make_list_inserter(CRefCallAddCopy<This>(*this));
|
||||
//}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print out graph */
|
||||
void print(const std::string& s = "FactorGraph",
|
||||
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
protected:
|
||||
/** Check equality */
|
||||
bool equals(const This& fg, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return the number of factors and NULLS */
|
||||
size_t size() const { return factors_.size();}
|
||||
/** return the number of factors (including any null factors set by remove() ). */
|
||||
size_t size() const { return factors_.size(); }
|
||||
|
||||
/** Simple check for an empty graph - faster than comparing size() to zero */
|
||||
/** Check if the graph is empty (null factors set by remove() will cause this to return false). */
|
||||
bool empty() const { return factors_.empty(); }
|
||||
|
||||
/** const cast to the underlying vector of factors */
|
||||
operator const std::vector<sharedFactor>&() const { return factors_; }
|
||||
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
|
||||
* opposed to operator[] which does not).
|
||||
*/
|
||||
const sharedFactor at(size_t i) const { return factors_.at(i); }
|
||||
|
||||
/** Get a specific factor by index */
|
||||
const sharedFactor at(size_t i) const { assert(i<factors_.size()); return factors_[i]; }
|
||||
sharedFactor& at(size_t i) { assert(i<factors_.size()); return factors_[i]; }
|
||||
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
|
||||
* opposed to operator[] which does not).
|
||||
*/
|
||||
sharedFactor& at(size_t i) { return factors_.at(i); }
|
||||
|
||||
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
|
||||
* does).
|
||||
*/
|
||||
const sharedFactor operator[](size_t i) const { return at(i); }
|
||||
|
||||
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
|
||||
* does).
|
||||
*/
|
||||
sharedFactor& operator[](size_t i) { return at(i); }
|
||||
|
||||
/** Checks whether a valid factor exists at the given index */
|
||||
inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
|
||||
|
||||
/** STL begin, so we can use BOOST_FOREACH */
|
||||
/** Iterator to beginning of factors. */
|
||||
const_iterator begin() const { return factors_.begin();}
|
||||
|
||||
/** STL end, so we can use BOOST_FOREACH */
|
||||
/** Iterator to end of factors. */
|
||||
const_iterator end() const { return factors_.end(); }
|
||||
|
||||
/** Get the first factor */
|
||||
|
|
@ -180,38 +312,6 @@ class VariableIndex;
|
|||
/** Get the last factor */
|
||||
sharedFactor back() const { return factors_.back(); }
|
||||
|
||||
/** Eliminate the first \c n frontal variables, returning the resulting
|
||||
* conditional and remaining factor graph - this is very inefficient for
|
||||
* eliminating all variables, to do that use EliminationTree or
|
||||
* JunctionTree.
|
||||
*/
|
||||
std::pair<sharedConditional, FactorGraph<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
|
||||
|
||||
/** Factor the factor graph into a conditional and a remaining factor graph.
|
||||
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
|
||||
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
|
||||
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
|
||||
* a probability density or likelihood, the factorization produces a
|
||||
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
|
||||
*
|
||||
* For efficiency, this function treats the variables to eliminate
|
||||
* \c variables as fully-connected, so produces a dense (fully-connected)
|
||||
* conditional on all of the variables in \c variables, instead of a sparse
|
||||
* BayesNet. If the variables are not fully-connected, it is more efficient
|
||||
* to sequentially factorize multiple times.
|
||||
*/
|
||||
std::pair<sharedConditional, FactorGraph<FactorType> > eliminate(
|
||||
const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
|
||||
boost::optional<const VariableIndex&> variableIndex = boost::none) const;
|
||||
|
||||
/** Eliminate a single variable, by calling FactorGraph::eliminate. */
|
||||
std::pair<sharedConditional, FactorGraph<FactorType> > eliminateOne(
|
||||
KeyType variable, const Eliminate& eliminateFcn,
|
||||
boost::optional<const VariableIndex&> variableIndex = boost::none) const {
|
||||
std::vector<size_t> variables(1, variable);
|
||||
return eliminate(variables, eliminateFcn, variableIndex);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Modifying Factor Graphs (imperative, discouraged)
|
||||
/// @{
|
||||
|
|
@ -222,24 +322,33 @@ class VariableIndex;
|
|||
/** non-const STL-style end() */
|
||||
iterator end() { return factors_.end(); }
|
||||
|
||||
/** resize the factor graph. TODO: effects? */
|
||||
/** Directly resize the number of factors in the graph. If the new size is less than the
|
||||
* original, factors at the end will be removed. If the new size is larger than the original,
|
||||
* null factors will be appended.
|
||||
*/
|
||||
void resize(size_t size) { factors_.resize(size); }
|
||||
|
||||
/** delete factor without re-arranging indexes by inserting a NULL pointer */
|
||||
inline void remove(size_t i) { factors_[i].reset();}
|
||||
void remove(size_t i) { factors_[i].reset();}
|
||||
|
||||
/** replace a factor by index */
|
||||
void replace(size_t index, sharedFactor factor);
|
||||
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
|
||||
|
||||
/** Erase factor and rearrange other factors to take up the empty space */
|
||||
void erase(iterator item) { factors_.erase(item); }
|
||||
|
||||
/** Erase factors and rearrange other factors to take up the empty space */
|
||||
void erase(iterator first, iterator last) { factors_.erase(first, last); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** return the number valid factors */
|
||||
/** return the number of non-null factors */
|
||||
size_t nrFactors() const;
|
||||
|
||||
/** Potentially very slow function to return all keys involved */
|
||||
std::set<KeyType> keys() const;
|
||||
FastSet<Key> keys() const;
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -254,20 +363,4 @@ class VariableIndex;
|
|||
|
||||
}; // FactorGraph
|
||||
|
||||
/** Create a combined joint factor (new style for EliminationTree). */
|
||||
template<class DERIVEDFACTOR, class KEY>
|
||||
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraph<DERIVEDFACTOR>& factors,
|
||||
const FastMap<KEY, std::vector<KEY> >& variableSlots);
|
||||
|
||||
/**
|
||||
* static function that combines two factor graphs
|
||||
* @param fg1 Linear factor graph
|
||||
* @param fg2 Linear factor graph
|
||||
* @return a new combined factor graph
|
||||
*/
|
||||
template<class FACTORGRAPH>
|
||||
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
|
|
|
|||
|
|
@ -1,107 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 GenericMultifrontalSolver-inl.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Factor-inl.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
|
||||
const FactorGraph<F>& graph) :
|
||||
structure_(new VariableIndex(graph)), junctionTree_(
|
||||
new JT(graph, *structure_)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
|
||||
const sharedGraph& graph,
|
||||
const VariableIndex::shared_ptr& variableIndex) :
|
||||
structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
void GenericMultifrontalSolver<F, JT>::print(const std::string& s) const {
|
||||
this->structure_->print(s + " structure:\n");
|
||||
this->junctionTree_->print(s + " jtree:");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
bool GenericMultifrontalSolver<F, JT>::equals(
|
||||
const GenericMultifrontalSolver& expected, double tol) const {
|
||||
if (!this->structure_->equals(*expected.structure_, tol)) return false;
|
||||
if (!this->junctionTree_->equals(*expected.junctionTree_, tol)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
void GenericMultifrontalSolver<F, JT>::replaceFactors(const sharedGraph& graph) {
|
||||
junctionTree_.reset(new JT(*graph, *structure_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR, class JUNCTIONTREE>
|
||||
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
|
||||
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
|
||||
|
||||
// eliminate junction tree, returns pointer to root
|
||||
typename BayesTree<typename FACTOR::ConditionalType>::sharedClique
|
||||
root = junctionTree_->eliminate(function);
|
||||
|
||||
// create an empty Bayes tree and insert root clique
|
||||
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
|
||||
bayesTree(new BayesTree<typename FACTOR::ConditionalType>);
|
||||
bayesTree->insert(root);
|
||||
|
||||
// return the Bayes tree
|
||||
return bayesTree;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
typename FactorGraph<F>::shared_ptr GenericMultifrontalSolver<F, JT>::jointFactorGraph(
|
||||
const std::vector<Index>& js, Eliminate function) const {
|
||||
|
||||
// FIXME: joint for arbitrary sets of variables not present
|
||||
// TODO: develop and implement theory for shortcuts of more than two variables
|
||||
|
||||
if (js.size() != 2) throw std::domain_error(
|
||||
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"
|
||||
"for exactly two variables. You can call marginal to compute the\n"
|
||||
"marginal for one variable. *SequentialSolver::joint(js) can compute the\n"
|
||||
"joint marginal over any number of variables, so use that if necessary.\n");
|
||||
|
||||
return eliminate(function)->joint(js[0], js[1], function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
typename boost::shared_ptr<F> GenericMultifrontalSolver<F, JT>::marginalFactor(
|
||||
Index j, Eliminate function) const {
|
||||
return eliminate(function)->marginalFactor(j, function);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,124 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 GenericMultifrontalSolver.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Generic Multifrontal Solver class
|
||||
*
|
||||
* A solver is given a factor graph at construction, and implements
|
||||
* a strategy to solve it (in this case, eliminate into a Bayes tree).
|
||||
* This generic one will create a Bayes tree when eliminate() is called.
|
||||
*
|
||||
* Takes two template arguments:
|
||||
* FACTOR the factor type, e.g., GaussianFactor, DiscreteFactor
|
||||
* JUNCTIONTREE annoyingly, you also have to supply a compatible JT type
|
||||
* i.e., one templated on a factor graph with the same factors
|
||||
* TODO: figure why this is so and possibly fix it
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FACTOR, class JUNCTIONTREE>
|
||||
class GenericMultifrontalSolver {
|
||||
|
||||
protected:
|
||||
|
||||
/// Column structure of the factor graph
|
||||
VariableIndex::shared_ptr structure_;
|
||||
|
||||
/// Junction tree that performs elimination.
|
||||
typename JUNCTIONTREE::shared_ptr junctionTree_;
|
||||
|
||||
public:
|
||||
|
||||
typedef typename FactorGraph<FACTOR>::shared_ptr sharedGraph;
|
||||
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Construct the solver for a factor graph. This builds the junction
|
||||
* tree, which does the symbolic elimination, identifies the cliques,
|
||||
* and distributes all the factors to the right cliques.
|
||||
*/
|
||||
GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph);
|
||||
|
||||
/**
|
||||
* Construct the solver with a shared pointer to a factor graph and to a
|
||||
* VariableIndex. The solver will store these pointers, so this constructor
|
||||
* is the fastest.
|
||||
*/
|
||||
GenericMultifrontalSolver(const sharedGraph& factorGraph,
|
||||
const VariableIndex::shared_ptr& variableIndex);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print to cout */
|
||||
void print(const std::string& name = "GenericMultifrontalSolver: ") const;
|
||||
|
||||
/** Test whether is equal to another */
|
||||
bool equals(const GenericMultifrontalSolver& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Replace the factor graph with a new one having the same structure. The
|
||||
* This function can be used if the numerical part of the factors changes,
|
||||
* such as during relinearization or adjusting of noise models.
|
||||
*/
|
||||
void replaceFactors(const sharedGraph& factorGraph);
|
||||
|
||||
/**
|
||||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr
|
||||
eliminate(Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor
|
||||
* graph.
|
||||
*/
|
||||
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(
|
||||
const std::vector<Index>& js, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute the marginal density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
typename boost::shared_ptr<FACTOR> marginalFactor(Index j,
|
||||
Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
||||
#include <gtsam/inference/GenericMultifrontalSolver-inl.h>
|
||||
|
|
@ -1,203 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 GenericSequentialSolver-inl.h
|
||||
* @brief Implementation for generic sequential solver
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Factor.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) {
|
||||
gttic(GenericSequentialSolver_constructor1);
|
||||
assert(factorGraph.size());
|
||||
factors_.reset(new FactorGraph<FACTOR>(factorGraph));
|
||||
structure_.reset(new VariableIndex(factorGraph));
|
||||
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
|
||||
const sharedFactorGraph& factorGraph,
|
||||
const boost::shared_ptr<VariableIndex>& variableIndex)
|
||||
{
|
||||
gttic(GenericSequentialSolver_constructor2);
|
||||
factors_ = factorGraph;
|
||||
structure_ = variableIndex;
|
||||
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void GenericSequentialSolver<FACTOR>::print(const std::string& s) const {
|
||||
this->factors_->print(s + " factors:");
|
||||
this->structure_->print(s + " structure:\n");
|
||||
this->eliminationTree_->print(s + " etree:");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
bool GenericSequentialSolver<FACTOR>::equals(
|
||||
const GenericSequentialSolver& expected, double tol) const {
|
||||
if (!this->factors_->equals(*expected.factors_, tol))
|
||||
return false;
|
||||
if (!this->structure_->equals(*expected.structure_, tol))
|
||||
return false;
|
||||
if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void GenericSequentialSolver<FACTOR>::replaceFactors(
|
||||
const sharedFactorGraph& factorGraph) {
|
||||
// Reset this shared pointer first to deallocate if possible - for big
|
||||
// problems there may not be enough memory to store two copies.
|
||||
eliminationTree_.reset();
|
||||
factors_ = factorGraph;
|
||||
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
|
||||
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
|
||||
return eliminationTree_->eliminate(function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
|
||||
GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
|
||||
Eliminate function, boost::optional<size_t> nrToEliminate) const
|
||||
{
|
||||
gttic(GenericSequentialSolver_eliminate);
|
||||
// Create inverse permutation
|
||||
Permutation::shared_ptr permutationInverse(permutation.inverse());
|
||||
|
||||
// Permute the factors - NOTE that this permutes the original factors, not
|
||||
// copies. Other parts of the code may hold shared_ptr's to these factors so
|
||||
// we must undo the permutation before returning.
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
|
||||
if (factor)
|
||||
factor->permuteWithInverse(*permutationInverse);
|
||||
|
||||
// Eliminate using elimination tree provided
|
||||
typename EliminationTree<FACTOR>::shared_ptr etree = EliminationTree<FACTOR>::Create(*factors_);
|
||||
sharedBayesNet bayesNet;
|
||||
if(nrToEliminate)
|
||||
bayesNet = etree->eliminatePartial(function, *nrToEliminate);
|
||||
else
|
||||
bayesNet = etree->eliminate(function);
|
||||
|
||||
// Undo the permutation on the original factors and on the structure.
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
|
||||
if (factor)
|
||||
factor->permuteWithInverse(permutation);
|
||||
|
||||
// Undo the permutation on the conditionals
|
||||
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
|
||||
c->permuteWithInverse(permutation);
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
|
||||
GenericSequentialSolver<FACTOR>::conditionalBayesNet(
|
||||
const std::vector<Index>& js, size_t nrFrontals,
|
||||
Eliminate function) const
|
||||
{
|
||||
gttic(GenericSequentialSolver_conditionalBayesNet);
|
||||
// Compute a COLAMD permutation with the marginal variables constrained to the end.
|
||||
// TODO in case of nrFrontals, the order of js has to be respected here !
|
||||
Permutation::shared_ptr permutation(
|
||||
inference::PermutationCOLAMD(*structure_, js, true));
|
||||
|
||||
// Eliminate only variables J \cup F from P(J,F,S) to get P(F|S)
|
||||
size_t nrVariables = structure_->size();
|
||||
size_t nrMarginalized = nrVariables - js.size();
|
||||
size_t nrToEliminate = nrMarginalized + nrFrontals;
|
||||
sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate);
|
||||
// Get rid of conditionals on variables that we want to marginalize out
|
||||
for (size_t i = 0; i < nrMarginalized; i++)
|
||||
bayesNet->pop_front();
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
|
||||
GenericSequentialSolver<FACTOR>::jointBayesNet(const std::vector<Index>& js,
|
||||
Eliminate function) const
|
||||
{
|
||||
gttic(GenericSequentialSolver_jointBayesNet);
|
||||
// Compute a COLAMD permutation with the marginal variables constrained to the end.
|
||||
Permutation::shared_ptr permutation(
|
||||
inference::PermutationCOLAMD(*structure_, js));
|
||||
|
||||
// Eliminate all variables
|
||||
sharedBayesNet bayesNet = eliminate(*permutation, function);
|
||||
|
||||
// Get rid of conditionals on variables that we want to marginalize out
|
||||
size_t nrMarginalized = bayesNet->size() - js.size();
|
||||
for (size_t i = 0; i < nrMarginalized; i++)
|
||||
bayesNet->pop_front();
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename FactorGraph<FACTOR>::shared_ptr //
|
||||
GenericSequentialSolver<FACTOR>::jointFactorGraph(
|
||||
const std::vector<Index>& js, Eliminate function) const
|
||||
{
|
||||
gttic(GenericSequentialSolver_jointFactorGraph);
|
||||
// Eliminate all variables
|
||||
typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js, function);
|
||||
|
||||
return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename boost::shared_ptr<FACTOR> //
|
||||
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
|
||||
gttic(GenericSequentialSolver_marginalFactor);
|
||||
// Create a container for the one variable index
|
||||
std::vector<Index> js(1);
|
||||
js[0] = j;
|
||||
|
||||
// Call joint and return the only factor in the factor graph it returns
|
||||
// TODO: just call jointBayesNet and grab last conditional, then toFactor....
|
||||
return (*this->jointFactorGraph(js, function))[0];
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,179 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 GenericSequentialSolver.h
|
||||
* @brief generic sequential elimination
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
class VariableIndex;
|
||||
class Permutation;
|
||||
}
|
||||
namespace gtsam {
|
||||
template<class FACTOR> class EliminationTree;
|
||||
}
|
||||
namespace gtsam {
|
||||
template<class FACTOR> class FactorGraph;
|
||||
}
|
||||
namespace gtsam {
|
||||
template<class CONDITIONAL> class BayesNet;
|
||||
}
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This solver implements sequential variable elimination for factor graphs.
|
||||
* Underlying this is a column elimination tree, see Gilbert 2001 BIT.
|
||||
*
|
||||
* The elimination ordering is "baked in" to the variable indices at this
|
||||
* stage, i.e. elimination proceeds in order from '0'.
|
||||
*
|
||||
* This is not the most efficient algorithm we provide, most efficient is the
|
||||
* MultifrontalSolver, which examines and uses the clique structure.
|
||||
* However, sequential variable elimination is easier to understand so this is a good
|
||||
* starting point to learn about these algorithms and our implementation.
|
||||
* Additionally, the first step of MFQR is symbolic sequential elimination.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FACTOR>
|
||||
class GenericSequentialSolver {
|
||||
|
||||
protected:
|
||||
|
||||
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
|
||||
typedef typename FACTOR::ConditionalType Conditional;
|
||||
typedef typename boost::shared_ptr<Conditional> sharedConditional;
|
||||
typedef typename boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet;
|
||||
typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
|
||||
typedef boost::function<
|
||||
EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
|
||||
|
||||
/** Store the original factors for computing marginals
|
||||
* TODO Frank says: really? Marginals should be computed from result.
|
||||
*/
|
||||
sharedFactorGraph factors_;
|
||||
|
||||
/** Store column structure of the factor graph. Why? */
|
||||
boost::shared_ptr<VariableIndex> structure_;
|
||||
|
||||
/** Elimination tree that performs elimination */
|
||||
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
|
||||
|
||||
/** concept checks */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
|
||||
|
||||
/**
|
||||
* Eliminate in a different order, given a permutation
|
||||
*/
|
||||
sharedBayesNet eliminate(const Permutation& permutation, Eliminate function,
|
||||
boost::optional<size_t> nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many
|
||||
) const;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Construct the solver for a factor graph. This builds the elimination
|
||||
* tree, which already does some of the work of elimination.
|
||||
*/
|
||||
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
|
||||
|
||||
/**
|
||||
* Construct the solver with a shared pointer to a factor graph and to a
|
||||
* VariableIndex. The solver will store these pointers, so this constructor
|
||||
* is the fastest.
|
||||
*/
|
||||
GenericSequentialSolver(const sharedFactorGraph& factorGraph,
|
||||
const boost::shared_ptr<VariableIndex>& variableIndex);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print to cout */
|
||||
void print(const std::string& name = "GenericSequentialSolver: ") const;
|
||||
|
||||
/** Test whether is equal to another */
|
||||
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Replace the factor graph with a new one having the same structure. The
|
||||
* This function can be used if the numerical part of the factors changes,
|
||||
* such as during relinearization or adjusting of noise models.
|
||||
*/
|
||||
void replaceFactors(const sharedFactorGraph& factorGraph);
|
||||
|
||||
/**
|
||||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
sharedBayesNet eliminate(Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute a conditional density P(F|S) while marginalizing out variables J
|
||||
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
|
||||
* Returns the result as a Bayes net.
|
||||
*/
|
||||
sharedBayesNet
|
||||
conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals,
|
||||
Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. Returns the result as a Bayes net
|
||||
*/
|
||||
sharedBayesNet
|
||||
jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. Returns the result as a factor graph.
|
||||
*/
|
||||
typename FactorGraph<FACTOR>::shared_ptr
|
||||
jointFactorGraph(const std::vector<Index>& js, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Compute the marginal Gaussian density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
typename boost::shared_ptr<FACTOR>
|
||||
marginalFactor(Index j, Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
|
||||
}
|
||||
;
|
||||
// GenericSequentialSolver
|
||||
|
||||
}// namespace gtsam
|
||||
|
||||
#include <gtsam/inference/GenericSequentialSolver-inl.h>
|
||||
|
|
@ -1,74 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ISAM-inl.h
|
||||
* @brief Incremental update functionality (iSAM) for BayesTree.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/GenericMultifrontalSolver.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
template<class FG> void ISAM<CONDITIONAL>::update_internal(
|
||||
const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) {
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
// Throw exception if disconnected
|
||||
BayesNet<CONDITIONAL> bn;
|
||||
if (!this->empty()) {
|
||||
this->removeTop(newFactors.keys(), bn, orphans);
|
||||
if (bn.empty())
|
||||
throw std::runtime_error(
|
||||
"ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
|
||||
}
|
||||
FG factors(bn);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
||||
// eliminate into a Bayes net
|
||||
GenericMultifrontalSolver<typename CONDITIONAL::FactorType, JunctionTree<FG> > solver(factors);
|
||||
boost::shared_ptr<BayesTree<CONDITIONAL> > bayesTree;
|
||||
bayesTree = solver.eliminate(function);
|
||||
this->root_ = bayesTree->root();
|
||||
this->nodes_ = bayesTree->nodes();
|
||||
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans)
|
||||
this->insert(orphan);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
template<class FG>
|
||||
void ISAM<CONDITIONAL>::update(const FG& newFactors,
|
||||
typename FG::Eliminate function) {
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans, function);
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ISAM-inl.h
|
||||
* @brief Incremental update functionality (iSAM) for BayesTree.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/ISAM.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function)
|
||||
{
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
// Throw exception if disconnected
|
||||
BayesNetType bn;
|
||||
if (!this->empty()) {
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
|
||||
}
|
||||
|
||||
// Add the removed top and the new factors
|
||||
FactorGraphType factors;
|
||||
factors += bn;
|
||||
factors += newFactors;
|
||||
|
||||
// Add the orphaned subtrees
|
||||
BOOST_FOREACH(const sharedClique& orphan, orphans)
|
||||
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
|
||||
|
||||
// eliminate into a Bayes net
|
||||
const VariableIndex varIndex(factors);
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
const Ordering constrainedOrdering =
|
||||
Ordering::COLAMDConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
|
||||
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
|
||||
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
|
||||
{
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans, function);
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -18,8 +18,6 @@
|
|||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
@ -27,12 +25,19 @@ namespace gtsam {
|
|||
* Given a set of new factors, it re-eliminates the invalidated part of the tree.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
class ISAM: public BayesTree<CONDITIONAL> {
|
||||
|
||||
template<class BAYESTREE>
|
||||
class ISAM: public BAYESTREE
|
||||
{
|
||||
private:
|
||||
|
||||
typedef BayesTree<CONDITIONAL> Base;
|
||||
typedef BAYESTREE Base;
|
||||
typedef typename Base::BayesNetType BayesNetType;
|
||||
typedef typename Base::FactorGraphType FactorGraphType;
|
||||
typedef typename Base::Clique Clique;
|
||||
typedef typename Base::sharedClique sharedClique;
|
||||
typedef typename Base::Cliques Cliques;
|
||||
typedef typename Base::Eliminate Eliminate;
|
||||
typedef typename Base::EliminationTraitsType EliminationTraitsType;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -40,12 +45,10 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM();
|
||||
ISAM() {}
|
||||
|
||||
/** Copy constructor */
|
||||
ISAM(const Base& bayesTree) :
|
||||
Base(bayesTree) {
|
||||
}
|
||||
ISAM(const Base& bayesTree) : Base(bayesTree) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface Interface
|
||||
|
|
@ -56,21 +59,14 @@ namespace gtsam {
|
|||
* @param newFactors is a factor graph that contains the new factors
|
||||
* @param function an elimination routine
|
||||
*/
|
||||
template<class FG>
|
||||
void update(const FG& newFactors, typename FG::Eliminate function);
|
||||
|
||||
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
|
||||
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
|
||||
void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate);
|
||||
|
||||
/** update_internal provides access to list of orphans for drawing purposes */
|
||||
template<class FG>
|
||||
void update_internal(const FG& newFactors, Cliques& orphans,
|
||||
typename FG::Eliminate function);
|
||||
void update_internal(const FactorGraphType& newFactors, Cliques& orphans,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate);
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}/// namespace gtsam
|
||||
|
||||
#include <gtsam/inference/ISAM-inl.h>
|
||||
|
|
|
|||
|
|
@ -1,65 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IndexConditional.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::lambda;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void IndexConditional::assertInvariants() const {
|
||||
// Checks for uniqueness of keys
|
||||
Base::assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
|
||||
#endif
|
||||
bool parentChanged = false;
|
||||
BOOST_FOREACH(KeyType& parent, parents()) {
|
||||
internal::Reduction::const_iterator it = inverseReduction.find(parent);
|
||||
if(it != inverseReduction.end()) {
|
||||
parentChanged = true;
|
||||
parent = it->second;
|
||||
}
|
||||
}
|
||||
assertInvariants();
|
||||
return parentChanged;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(Index& key, keys())
|
||||
key = inversePermutation[key];
|
||||
assertInvariants();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -1,135 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IndexConditional.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* IndexConditional serves two purposes. It is the base class for
|
||||
* GaussianConditional, and also functions as a symbolic conditional with
|
||||
* Index keys, produced by symbolic elimination of IndexFactor.
|
||||
*
|
||||
* It derives from Conditional with a key type of Index, which is an
|
||||
* unsigned integer.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class IndexConditional : public Conditional<Index> {
|
||||
|
||||
protected:
|
||||
|
||||
// Checks that frontal indices are sorted and lower than parent indices
|
||||
GTSAM_EXPORT void assertInvariants() const;
|
||||
|
||||
public:
|
||||
|
||||
typedef IndexConditional This;
|
||||
typedef Conditional<Index> Base;
|
||||
typedef IndexFactor FactorType;
|
||||
typedef boost::shared_ptr<IndexConditional> shared_ptr;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Empty Constructor to make serialization possible */
|
||||
IndexConditional() { assertInvariants(); }
|
||||
|
||||
/** No parents */
|
||||
IndexConditional(Index j) : Base(j) { assertInvariants(); }
|
||||
|
||||
/** Single parent */
|
||||
IndexConditional(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
|
||||
|
||||
/** Two parents */
|
||||
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); }
|
||||
|
||||
/** Three parents */
|
||||
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** Constructor from a frontal variable and a vector of parents */
|
||||
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from keys and nr of frontal variables */
|
||||
IndexConditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
||||
Base(keys, nrFrontals) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from keys and nr of frontal variables */
|
||||
IndexConditional(const std::list<Index>& keys, size_t nrFrontals) :
|
||||
Base(keys, nrFrontals) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Named constructor directly returning a shared pointer */
|
||||
template<class KEYS>
|
||||
static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) {
|
||||
shared_ptr conditional(new IndexConditional());
|
||||
conditional->keys_.assign(keys.begin(), keys.end());
|
||||
conditional->nrFrontals_ = nrFrontals;
|
||||
return conditional;
|
||||
}
|
||||
|
||||
/** Convert to a factor */
|
||||
IndexFactor::shared_ptr toFactor() const {
|
||||
return IndexFactor::shared_ptr(new IndexFactor(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Permute the variables when only separator variables need to be permuted.
|
||||
* Returns true if any reordered variables appeared in the separator and
|
||||
* false if not.
|
||||
*/
|
||||
GTSAM_EXPORT bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
|
||||
|
||||
/**
|
||||
* Permutes the Conditional, but for efficiency requires the permutation
|
||||
* to already be inverted.
|
||||
*/
|
||||
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
private:
|
||||
/** 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);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IndexFactor.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/Factor.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void IndexFactor::assertInvariants() const {
|
||||
Base::assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
IndexFactor::IndexFactor(const IndexConditional& c) :
|
||||
Base(c) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef TRACK_ELIMINATE
|
||||
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
|
||||
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst<
|
||||
IndexConditional>());
|
||||
assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(
|
||||
size_t nrFrontals) {
|
||||
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate<
|
||||
IndexConditional>(nrFrontals));
|
||||
assertInvariants();
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(Index& key, keys())
|
||||
key = inversePermutation[key];
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) {
|
||||
BOOST_FOREACH(Index& key, keys())
|
||||
key = inverseReduction[key];
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // gtsam
|
||||
|
|
@ -1,179 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IndexFactor.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Factor.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration of IndexConditional
|
||||
class IndexConditional;
|
||||
|
||||
/**
|
||||
* IndexFactor serves two purposes. It is the base class for all linear
|
||||
* factors (GaussianFactor, JacobianFactor, HessianFactor), and also
|
||||
* functions as a symbolic factor with Index keys, used to do symbolic
|
||||
* elimination by JunctionTree.
|
||||
*
|
||||
* It derives from Factor with a key type of Index, an unsigned integer.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class IndexFactor: public Factor<Index> {
|
||||
|
||||
protected:
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/// Internal function for checking class invariants (unique keys for this factor)
|
||||
GTSAM_EXPORT void assertInvariants() const;
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
|
||||
typedef IndexFactor This;
|
||||
typedef Factor<Index> Base;
|
||||
|
||||
/** Elimination produces an IndexConditional */
|
||||
typedef IndexConditional ConditionalType;
|
||||
|
||||
/** Overriding the shared_ptr typedef */
|
||||
typedef boost::shared_ptr<IndexFactor> shared_ptr;
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Copy constructor */
|
||||
IndexFactor(const This& f) :
|
||||
Base(f) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct from derived type */
|
||||
GTSAM_EXPORT IndexFactor(const IndexConditional& c);
|
||||
|
||||
/** Default constructor for I/O */
|
||||
IndexFactor() {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct unary factor */
|
||||
IndexFactor(Index j) :
|
||||
Base(j) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct binary factor */
|
||||
IndexFactor(Index j1, Index j2) :
|
||||
Base(j1, j2) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct ternary factor */
|
||||
IndexFactor(Index j1, Index j2, Index j3) :
|
||||
Base(j1, j2, j3) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct 4-way factor */
|
||||
IndexFactor(Index j1, Index j2, Index j3, Index j4) :
|
||||
Base(j1, j2, j3, j4) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct 5-way factor */
|
||||
IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5) :
|
||||
Base(j1, j2, j3, j4, j5) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct 6-way factor */
|
||||
IndexFactor(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) :
|
||||
Base(j1, j2, j3, j4, j5, j6) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct n-way factor */
|
||||
IndexFactor(const std::set<Index>& js) :
|
||||
Base(js) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Construct n-way factor */
|
||||
IndexFactor(const std::vector<Index>& js) :
|
||||
Base(js) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
|
||||
KeyIterator endKey) :
|
||||
Base(beginKey, endKey) {
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
#ifdef TRACK_ELIMINATE
|
||||
/**
|
||||
* eliminate the first variable involved in this factor
|
||||
* @return a conditional on the eliminated variable
|
||||
*/
|
||||
GTSAM_EXPORT boost::shared_ptr<ConditionalType> eliminateFirst();
|
||||
|
||||
/** eliminate the first nrFrontals frontal variables.*/
|
||||
GTSAM_EXPORT boost::shared_ptr<BayesNet<ConditionalType> > eliminate(size_t nrFrontals =
|
||||
1);
|
||||
#endif
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Permutes the factor, but for efficiency requires the permutation
|
||||
* to already be inverted.
|
||||
*/
|
||||
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/**
|
||||
* Apply a reduction, which is a remapping of variable indices.
|
||||
*/
|
||||
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
|
||||
|
||||
virtual ~IndexFactor() {
|
||||
}
|
||||
|
||||
private:
|
||||
/** 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);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // IndexFactor
|
||||
|
||||
}
|
||||
|
|
@ -1,205 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 JunctionTree-inl.h
|
||||
* @date Feb 4, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @brief The junction tree, template bodies
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
|
||||
gttic(JT_symbolic_ET);
|
||||
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
|
||||
EliminationTree<IndexFactor>::Create(fg, variableIndex);
|
||||
assert(symETree.get());
|
||||
gttoc(JT_symbolic_ET);
|
||||
gttic(JT_symbolic_eliminate);
|
||||
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
|
||||
assert(sbn.get());
|
||||
gttoc(JT_symbolic_eliminate);
|
||||
gttic(symbolic_BayesTree);
|
||||
SymbolicBayesTree sbt(*sbn);
|
||||
gttoc(symbolic_BayesTree);
|
||||
|
||||
// distribute factors
|
||||
gttic(distributeFactors);
|
||||
this->root_ = distributeFactors(fg, sbt.root());
|
||||
gttoc(distributeFactors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
|
||||
gttic(VariableIndex);
|
||||
VariableIndex varIndex(fg);
|
||||
gttoc(VariableIndex);
|
||||
construct(fg, varIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
|
||||
construct(fg, variableIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(
|
||||
const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
|
||||
// Build "target" index. This is an index for each variable of the factors
|
||||
// that involve this variable as their *lowest-ordered* variable. For each
|
||||
// factor, it is the lowest-ordered variable of that factor that pulls the
|
||||
// factor into elimination, after which all of the information in the
|
||||
// factor is contained in the eliminated factors that are passed up the
|
||||
// tree as elimination continues.
|
||||
|
||||
// Two stages - first build an array of the lowest-ordered variable in each
|
||||
// factor and find the last variable to be eliminated.
|
||||
std::vector<Index> lowestOrdered(fg.size(), std::numeric_limits<Index>::max());
|
||||
Index maxVar = 0;
|
||||
for(size_t i=0; i<fg.size(); ++i)
|
||||
if(fg[i]) {
|
||||
typename FG::FactorType::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
|
||||
if(min != fg[i]->end()) {
|
||||
lowestOrdered[i] = *min;
|
||||
maxVar = std::max(maxVar, *min);
|
||||
}
|
||||
}
|
||||
|
||||
// Now add each factor to the list corresponding to its lowest-ordered
|
||||
// variable.
|
||||
std::vector<FastList<size_t> > targets(maxVar+1);
|
||||
for(size_t i=0; i<lowestOrdered.size(); ++i)
|
||||
if(lowestOrdered[i] != std::numeric_limits<Index>::max())
|
||||
targets[lowestOrdered[i]].push_back(i);
|
||||
|
||||
// Now call the recursive distributeFactors
|
||||
return distributeFactors(fg, targets, bayesClique);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
|
||||
const std::vector<FastList<size_t> >& targets,
|
||||
const SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
|
||||
if(bayesClique) {
|
||||
// create a new clique in the junction tree
|
||||
sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(),
|
||||
(*bayesClique)->beginParents(), (*bayesClique)->endParents()));
|
||||
|
||||
// count the factors for this cluster to pre-allocate space
|
||||
{
|
||||
size_t nFactors = 0;
|
||||
BOOST_FOREACH(const Index frontal, clique->frontal) {
|
||||
// There may be less variables in "targets" than there really are if
|
||||
// some of the highest-numbered variables do not pull in any factors.
|
||||
if(frontal < targets.size())
|
||||
nFactors += targets[frontal].size(); }
|
||||
clique->reserve(nFactors);
|
||||
}
|
||||
// add the factors to this cluster
|
||||
BOOST_FOREACH(const Index frontal, clique->frontal) {
|
||||
if(frontal < targets.size()) {
|
||||
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
|
||||
clique->push_back(fg[factorI]); } } }
|
||||
|
||||
// recursively call the children
|
||||
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
|
||||
sharedClique child = distributeFactors(fg, targets, bayesChild);
|
||||
clique->addChild(child);
|
||||
child->parent() = clique;
|
||||
}
|
||||
return clique;
|
||||
} else
|
||||
return sharedClique();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
std::pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
|
||||
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
|
||||
typename FG::Eliminate function,
|
||||
const boost::shared_ptr<const Clique>& current) const {
|
||||
|
||||
FG fg; // factor graph will be assembled from local factors and marginalized children
|
||||
fg.reserve(current->size() + current->children().size());
|
||||
fg.push_back(*current); // add the local factors
|
||||
|
||||
// receive the factors from the child and its clique point
|
||||
std::list<typename BTClique::shared_ptr> children;
|
||||
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
|
||||
eliminateOneClique(function, child));
|
||||
children.push_back(tree_factor.first);
|
||||
fg.push_back(tree_factor.second);
|
||||
}
|
||||
|
||||
// eliminate the combined factors
|
||||
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
||||
|
||||
// Now that we know which factors and variables, and where variables
|
||||
// come from and go to, create and eliminate the new joint factor.
|
||||
gttic(CombineAndEliminate);
|
||||
typename FG::EliminationResult eliminated(function(fg,
|
||||
current->frontal.size()));
|
||||
gttoc(CombineAndEliminate);
|
||||
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
|
||||
#endif
|
||||
|
||||
gttic(Update_tree);
|
||||
// create a new clique corresponding the combined factors
|
||||
typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated));
|
||||
new_clique->children_ = children;
|
||||
|
||||
BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
|
||||
childRoot->parent_ = new_clique;
|
||||
}
|
||||
gttoc(Update_tree);
|
||||
|
||||
return std::make_pair(new_clique, eliminated.second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate(
|
||||
typename FG::Eliminate function) const {
|
||||
if (this->root()) {
|
||||
gttic(JT_eliminate);
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
|
||||
this->eliminateOneClique(function, this->root());
|
||||
if (ret.second->size() != 0) throw std::runtime_error(
|
||||
"JuntionTree::eliminate: elimination failed because of factors left over!");
|
||||
gttoc(JT_eliminate);
|
||||
return ret.first;
|
||||
} else
|
||||
return typename BTClique::shared_ptr();
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
@ -0,0 +1,313 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 JunctionTree-inl.h
|
||||
* @date Feb 4, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @brief The junction tree, template bodies
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/symbolic/SymbolicConditional.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace {
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
struct ConstructorTraversalData {
|
||||
ConstructorTraversalData* const parentData;
|
||||
typename JunctionTree<BAYESTREE,GRAPH>::sharedNode myJTNode;
|
||||
std::vector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
|
||||
std::vector<SymbolicFactor::shared_ptr> childSymbolicFactors;
|
||||
ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Pre-order visitor function
|
||||
template<class BAYESTREE, class GRAPH, class ETREE_NODE>
|
||||
ConstructorTraversalData<BAYESTREE,GRAPH> ConstructorTraversalVisitorPre(
|
||||
const boost::shared_ptr<ETREE_NODE>& node,
|
||||
ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
|
||||
{
|
||||
// On the pre-order pass, before children have been visited, we just set up a traversal data
|
||||
// structure with its own JT node, and create a child pointer in its parent.
|
||||
ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
|
||||
myData.myJTNode = boost::make_shared<typename JunctionTree<BAYESTREE,GRAPH>::Node>();
|
||||
myData.myJTNode->keys.push_back(node->key);
|
||||
myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
|
||||
parentData.myJTNode->children.push_back(myData.myJTNode);
|
||||
return myData;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Post-order visitor function
|
||||
template<class BAYESTREE, class GRAPH, class ETREE_NODE>
|
||||
void ConstructorTraversalVisitorPost(
|
||||
const boost::shared_ptr<ETREE_NODE>& ETreeNode,
|
||||
const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
|
||||
{
|
||||
// In this post-order visitor, we combine the symbolic elimination results from the
|
||||
// elimination tree children and symbolically eliminate the current elimination tree node. We
|
||||
// then check whether each of our elimination tree child nodes should be merged with us. The
|
||||
// check for this is that our number of symbolic elimination parents is exactly 1 less than
|
||||
// our child's symbolic elimination parents - this condition indicates that eliminating the
|
||||
// current node did not introduce any parents beyond those already in the child.
|
||||
|
||||
// Do symbolic elimination for this node
|
||||
SymbolicFactorGraph symbolicFactors;
|
||||
symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
|
||||
// Add symbolic versions of the ETree node factors
|
||||
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
|
||||
symbolicFactors.push_back(boost::make_shared<SymbolicFactor>(
|
||||
SymbolicFactor::FromKeys(*factor))); }
|
||||
// Add symbolic factors passed up from children
|
||||
symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
|
||||
Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
|
||||
std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr> symbolicElimResult =
|
||||
EliminateSymbolic(symbolicFactors, keyAsOrdering);
|
||||
|
||||
// Store symbolic elimination results in the parent
|
||||
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
|
||||
myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
|
||||
|
||||
// Merge our children if they are in our clique - if our conditional has exactly one fewer
|
||||
// parent than our child's conditional.
|
||||
const size_t myNrParents = symbolicElimResult.first->nrParents();
|
||||
size_t nrMergedChildren = 0;
|
||||
assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size());
|
||||
// Loop over children
|
||||
int combinedProblemSize = (int)symbolicElimResult.first->size();
|
||||
for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) {
|
||||
// Check if we should merge the child
|
||||
if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
|
||||
// Get a reference to the child, adjusting the index to account for children previously
|
||||
// merged and removed from the child list.
|
||||
const typename JunctionTree<BAYESTREE,GRAPH>::Node& childToMerge =
|
||||
*myData.myJTNode->children[child - nrMergedChildren];
|
||||
// Merge keys, factors, and children.
|
||||
myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
|
||||
myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end());
|
||||
myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
|
||||
// Remove child from list.
|
||||
myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren));
|
||||
// Increment number of merged children
|
||||
++ nrMergedChildren;
|
||||
// Increment problem size
|
||||
combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
|
||||
}
|
||||
}
|
||||
myData.myJTNode->problemSize_ = combinedProblemSize;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination traversal data - stores a pointer to the parent data and collects the factors
|
||||
// resulting from elimination of the children. Also sets up BayesTree cliques with parent and
|
||||
// child pointers.
|
||||
template<class JUNCTIONTREE>
|
||||
struct EliminationData {
|
||||
EliminationData* const parentData;
|
||||
size_t myIndexInParent;
|
||||
std::vector<typename JUNCTIONTREE::sharedFactor> childFactors;
|
||||
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode;
|
||||
EliminationData(EliminationData* _parentData, size_t nChildren) :
|
||||
parentData(_parentData),
|
||||
bayesTreeNode(boost::make_shared<typename JUNCTIONTREE::BayesTreeType::Node>())
|
||||
{
|
||||
if(parentData) {
|
||||
myIndexInParent = parentData->childFactors.size();
|
||||
parentData->childFactors.push_back(typename JUNCTIONTREE::sharedFactor());
|
||||
} else {
|
||||
myIndexInParent = 0;
|
||||
}
|
||||
// Set up BayesTree parent and child pointers
|
||||
if(parentData) {
|
||||
if(parentData->parentData) // If our parent is not the dummy node
|
||||
bayesTreeNode->parent_ = parentData->bayesTreeNode;
|
||||
parentData->bayesTreeNode->children.push_back(bayesTreeNode);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination pre-order visitor - just creates the EliminationData structure for the visited
|
||||
// node.
|
||||
template<class JUNCTIONTREE>
|
||||
EliminationData<JUNCTIONTREE> eliminationPreOrderVisitor(
|
||||
const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& parentData)
|
||||
{
|
||||
return EliminationData<JUNCTIONTREE>(&parentData, node->children.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination post-order visitor - combine the child factors with our own factors, add the
|
||||
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
|
||||
template<class JUNCTIONTREE>
|
||||
struct EliminationPostOrderVisitor
|
||||
{
|
||||
const typename JUNCTIONTREE::Eliminate& eliminationFunction;
|
||||
typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex;
|
||||
EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction,
|
||||
typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex) :
|
||||
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {}
|
||||
void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData)
|
||||
{
|
||||
// Typedefs
|
||||
typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
|
||||
typedef typename JUNCTIONTREE::FactorType FactorType;
|
||||
typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType;
|
||||
typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
|
||||
typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
|
||||
|
||||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(node->factors.size() + node->children.size());
|
||||
gatheredFactors += node->factors;
|
||||
gatheredFactors += myData.childFactors;
|
||||
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
BOOST_FOREACH(const sharedFactor& f, node->factors)
|
||||
{
|
||||
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
|
||||
{
|
||||
myData.bayesTreeNode->children.push_back(asSubtree->clique);
|
||||
asSubtree->clique->parent_ = myData.bayesTreeNode;
|
||||
}
|
||||
}
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, Ordering(node->keys));
|
||||
|
||||
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
|
||||
// Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
|
||||
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2
|
||||
// object they're added to.
|
||||
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
|
||||
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)).second;
|
||||
|
||||
// Store remaining factor in parent's gathered factors
|
||||
if(!eliminationResult.second->empty())
|
||||
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void JunctionTree<BAYESTREE,GRAPH>::Node::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
std::cout << s;
|
||||
BOOST_FOREACH(Key j, keys)
|
||||
std::cout << j << " ";
|
||||
std::cout << "problemSize = " << problemSize_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void JunctionTree<BAYESTREE,GRAPH>::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
treeTraversal::PrintForest(*this, s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
template<class ETREE>
|
||||
JunctionTree<BAYESTREE,GRAPH>
|
||||
JunctionTree<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
|
||||
{
|
||||
gttic(JunctionTree_FromEliminationTree);
|
||||
// Here we rely on the BayesNet having been produced by this elimination tree, such that the
|
||||
// conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect
|
||||
// the symbolic conditional corresponding to each node. The elimination tree node is added to
|
||||
// the same clique with its parent if it has exactly one more Bayes net conditional parent than
|
||||
// does its elimination tree parent.
|
||||
|
||||
// Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather
|
||||
// the created junction tree roots in a dummy Node.
|
||||
typedef typename ETREE::Node ETreeNode;
|
||||
ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
|
||||
rootData.myJTNode = boost::make_shared<Node>(); // Make a dummy node to gather the junction tree roots
|
||||
treeTraversal::DepthFirstForest(eliminationTree, rootData,
|
||||
ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
|
||||
|
||||
// Assign roots from the dummy node
|
||||
This result;
|
||||
result.roots_ = rootData.myJTNode->children;
|
||||
|
||||
// Transfer remaining factors from elimination tree
|
||||
result.remainingFactors_ = eliminationTree.remainingFactors();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
JunctionTree<BAYESTREE,GRAPH>& JunctionTree<BAYESTREE,GRAPH>::operator=(const This& other)
|
||||
{
|
||||
// Start by duplicating the tree.
|
||||
roots_ = treeTraversal::CloneForest(other);
|
||||
|
||||
// Assign the remaining factors - these are pointers to factors in the original factor graph and
|
||||
// we do not clone them.
|
||||
remainingFactors_ = other.remainingFactors_;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
|
||||
JunctionTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
|
||||
{
|
||||
gttic(JunctionTree_eliminate);
|
||||
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
|
||||
// that contains all of the roots as its children. rootsContainer also stores the remaining
|
||||
// uneliminated factors passed up from the roots.
|
||||
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
|
||||
EliminationData<This> rootsContainer(0, roots_.size());
|
||||
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
|
||||
//tbb::task_scheduler_init init(1);
|
||||
treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer,
|
||||
eliminationPreOrderVisitor<This>, visitorPost/*, 10*/);
|
||||
|
||||
// Create BayesTree from roots stored in the dummy BayesTree node.
|
||||
result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
|
||||
|
||||
// Add remaining factors that were not involved with eliminated variables
|
||||
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
|
||||
allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
|
||||
if(factor)
|
||||
allRemainingFactors->push_back(factor);
|
||||
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
@ -14,18 +14,14 @@
|
|||
* @date Feb 4, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @brief: The junction tree
|
||||
* @author Richard Roberts
|
||||
* @brief The junction tree
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -46,90 +42,103 @@ namespace gtsam {
|
|||
* The tree structure and elimination method are exactly analagous to the EliminationTree,
|
||||
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
|
||||
*
|
||||
*
|
||||
* \addtogroup Multifrontal
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
|
||||
class JunctionTree: public ClusterTree<FG> {
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
class JunctionTree {
|
||||
|
||||
public:
|
||||
|
||||
/// In a junction tree each cluster is associated with a clique
|
||||
typedef typename ClusterTree<FG>::Cluster Clique;
|
||||
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
|
||||
typedef JunctionTree<BAYESTREE, GRAPH> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
typedef boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination
|
||||
typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
|
||||
typedef boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
|
||||
|
||||
/// The BayesTree type produced by elimination
|
||||
typedef BTCLIQUE BTClique;
|
||||
struct Node {
|
||||
typedef std::vector<Key> Keys;
|
||||
typedef std::vector<sharedFactor> Factors;
|
||||
typedef std::vector<boost::shared_ptr<Node> > Children;
|
||||
|
||||
/// Shared pointer to this class
|
||||
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
|
||||
Keys keys; ///< Frontal keys of this node
|
||||
Factors factors; ///< Factors associated with this node
|
||||
Children children; ///< sub-trees
|
||||
int problemSize_;
|
||||
|
||||
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
|
||||
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
|
||||
int problemSize() const { return problemSize_; }
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
|
||||
|
||||
private:
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
/// distribute the factors along the cluster tree
|
||||
sharedClique distributeFactors(const FG& fg,
|
||||
const SymbolicBayesTree::sharedClique& clique);
|
||||
std::vector<sharedNode> roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
/// distribute the factors along the cluster tree
|
||||
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
|
||||
const SymbolicBayesTree::sharedClique& clique);
|
||||
|
||||
/// recursive elimination function
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
|
||||
eliminateOneClique(typename FG::Eliminate function,
|
||||
const boost::shared_ptr<const Clique>& clique) const;
|
||||
|
||||
/// internal constructor
|
||||
void construct(const FG& fg, const VariableIndex& variableIndex);
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
protected:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
JunctionTree() {}
|
||||
/** Build the junction tree from an elimination tree. */
|
||||
template<class ETREE>
|
||||
static This FromEliminationTree(const ETREE& eliminationTree);
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
JunctionTree(const This& other) { *this = other; }
|
||||
|
||||
/** Named constructor to build the junction tree of a factor graph. Note
|
||||
* that this has to compute the column structure as a VariableIndex, so if you
|
||||
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
|
||||
* constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
JunctionTree(const FG& factorGraph);
|
||||
|
||||
/** Construct from a factor graph and pre-computed variable index.
|
||||
* @param fg The factor graph for which to build the junction tree
|
||||
* @param structure The set of factors involving each variable. If this is not
|
||||
* precomputed, you can call the JunctionTree(const FG&)
|
||||
* constructor instead.
|
||||
*/
|
||||
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
|
||||
* are copied, factors are not cloned. */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors in the subgraphs to produce a BayesTree.
|
||||
* @param function The function used to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The BayesTree resulting from elimination
|
||||
*/
|
||||
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
|
||||
/** Eliminate the factors to a Bayes net and remaining factor graph
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The Bayes net and factor graph resulting from elimination
|
||||
*/
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminate(const Eliminate& function) const;
|
||||
|
||||
/** Print the junction tree */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
|
||||
}; // JunctionTree
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const std::vector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
} // namespace gtsam
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
#include <gtsam/inference/JunctionTree-inl.h>
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
// Private default constructor (used in static construction methods)
|
||||
JunctionTree() {}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -22,19 +22,11 @@
|
|||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/nonlinear/LabeledSymbol.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string _defaultKeyFormatter(Key key) {
|
||||
const Symbol asSymbol(key);
|
||||
if(asSymbol.chr() > 0)
|
||||
return (std::string)asSymbol;
|
||||
else
|
||||
return boost::lexical_cast<std::string>(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string _multirobotKeyFormatter(gtsam::Key key) {
|
||||
|
|
@ -27,19 +27,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/// Integer nonlinear key type
|
||||
typedef size_t Key;
|
||||
|
||||
/// Typedef for a function to format a key, i.e. to convert it to a string
|
||||
typedef boost::function<std::string(Key)> KeyFormatter;
|
||||
|
||||
// Helper function for DefaultKeyFormatter
|
||||
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
|
||||
|
||||
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
|
||||
/// a nonlinear 'print' function. Automatically detects plain integer keys
|
||||
/// and Symbol keys.
|
||||
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
|
||||
|
||||
// Helper function for Multi-robot Key Formatter
|
||||
GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
|
||||
|
|
@ -0,0 +1,232 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Ordering.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Sep 2, 2010
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastMap<Key, size_t> Ordering::invert() const
|
||||
{
|
||||
FastMap<Key, size_t> inverted;
|
||||
for(size_t pos = 0; pos < this->size(); ++pos)
|
||||
inverted.insert(make_pair((*this)[pos], pos));
|
||||
return inverted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMD(const VariableIndex& variableIndex)
|
||||
{
|
||||
// Call constrained version with all groups set to zero
|
||||
vector<int> dummy_groups(variableIndex.size(), 0);
|
||||
return Ordering::COLAMDConstrained(variableIndex, dummy_groups);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrained(
|
||||
const VariableIndex& variableIndex, std::vector<int>& cmember)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrained);
|
||||
|
||||
gttic(Prepare);
|
||||
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
|
||||
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
|
||||
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
|
||||
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
|
||||
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
|
||||
|
||||
// Fill in input data for COLAMD
|
||||
p[0] = 0;
|
||||
int count = 0;
|
||||
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
|
||||
size_t index = 0;
|
||||
BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) {
|
||||
// Arrange factor indices into COLAMD format
|
||||
const VariableIndex::Factors& column = key_factors.second;
|
||||
size_t lastFactorId = numeric_limits<size_t>::max();
|
||||
BOOST_FOREACH(size_t factorIndex, column) {
|
||||
if(lastFactorId != numeric_limits<size_t>::max())
|
||||
assert(factorIndex > lastFactorId);
|
||||
A[count++] = (int)factorIndex; // copy sparse column
|
||||
}
|
||||
p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
|
||||
// Store key in array and increment index
|
||||
keys[index] = key_factors.first;
|
||||
++ index;
|
||||
}
|
||||
|
||||
assert((size_t)count == variableIndex.nEntries());
|
||||
|
||||
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
|
||||
double knobs[CCOLAMD_KNOBS];
|
||||
ccolamd_set_defaults(knobs);
|
||||
knobs[CCOLAMD_DENSE_ROW]=-1;
|
||||
knobs[CCOLAMD_DENSE_COL]=-1;
|
||||
|
||||
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
|
||||
|
||||
gttoc(Prepare);
|
||||
|
||||
// call colamd, result will be in p
|
||||
/* returns (1) if successful, (0) otherwise*/
|
||||
if(nVars > 0) {
|
||||
gttic(ccolamd);
|
||||
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
|
||||
if(rv != 1)
|
||||
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
|
||||
}
|
||||
|
||||
// ccolamd_report(stats);
|
||||
|
||||
gttic(Fill_Ordering);
|
||||
// Convert elimination ordering in p to an ordering
|
||||
Ordering result;
|
||||
result.resize(nVars);
|
||||
for(size_t j = 0; j < nVars; ++j)
|
||||
result[j] = keys[p[j]];
|
||||
gttoc(Fill_Ordering);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrainedLast(
|
||||
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrainedLast);
|
||||
|
||||
size_t n = variableIndex.size();
|
||||
std::vector<int> cmember(n, 0);
|
||||
|
||||
// Build a mapping to look up sorted Key indices by Key
|
||||
FastMap<Key, size_t> keyIndices;
|
||||
size_t j = 0;
|
||||
BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex)
|
||||
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
|
||||
|
||||
// If at least some variables are not constrained to be last, constrain the
|
||||
// ones that should be constrained.
|
||||
int group = (constrainLast.size() != n ? 1 : 0);
|
||||
BOOST_FOREACH(Key key, constrainLast) {
|
||||
cmember[keyIndices.at(key)] = group;
|
||||
if(forceOrder)
|
||||
++ group;
|
||||
}
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrainedFirst(
|
||||
const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrainedFirst);
|
||||
|
||||
const int none = -1;
|
||||
size_t n = variableIndex.size();
|
||||
std::vector<int> cmember(n, none);
|
||||
|
||||
// Build a mapping to look up sorted Key indices by Key
|
||||
FastMap<Key, size_t> keyIndices;
|
||||
size_t j = 0;
|
||||
BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex)
|
||||
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
|
||||
|
||||
// If at least some variables are not constrained to be last, constrain the
|
||||
// ones that should be constrained.
|
||||
int group = 0;
|
||||
BOOST_FOREACH(Key key, constrainFirst) {
|
||||
cmember[keyIndices.at(key)] = group;
|
||||
if(forceOrder)
|
||||
++ group;
|
||||
}
|
||||
|
||||
if(!forceOrder)
|
||||
++ group;
|
||||
BOOST_FOREACH(int& c, cmember)
|
||||
if(c == none)
|
||||
c = group;
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex,
|
||||
const FastMap<Key, int>& groups)
|
||||
{
|
||||
gttic(Ordering_COLAMDConstrained);
|
||||
size_t n = variableIndex.size();
|
||||
std::vector<int> cmember(n, 0);
|
||||
|
||||
// Build a mapping to look up sorted Key indices by Key
|
||||
FastMap<Key, size_t> keyIndices;
|
||||
size_t j = 0;
|
||||
BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex)
|
||||
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
|
||||
|
||||
// Assign groups
|
||||
typedef FastMap<Key, int>::value_type key_group;
|
||||
BOOST_FOREACH(const key_group& p, groups) {
|
||||
// FIXME: check that no groups are skipped
|
||||
cmember[keyIndices.at(p.first)] = p.second;
|
||||
}
|
||||
|
||||
return Ordering::COLAMDConstrained(variableIndex, cmember);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
cout << str;
|
||||
// Print ordering in index order
|
||||
// Print the ordering with varsPerLine ordering entries printed on each line,
|
||||
// for compactness.
|
||||
static const size_t varsPerLine = 10;
|
||||
bool endedOnNewline = false;
|
||||
for(size_t i = 0; i < size(); ++i) {
|
||||
if(i % varsPerLine == 0)
|
||||
cout << "Position " << i << ": ";
|
||||
if(i % varsPerLine != 0)
|
||||
cout << ", ";
|
||||
cout << keyFormatter(at(i));
|
||||
if(i % varsPerLine == varsPerLine - 1) {
|
||||
cout << "\n";
|
||||
endedOnNewline = true;
|
||||
} else {
|
||||
endedOnNewline = false;
|
||||
}
|
||||
}
|
||||
if(!endedOnNewline)
|
||||
cout << "\n";
|
||||
cout.flush();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Ordering::equals(const Ordering& other, double tol) const
|
||||
{
|
||||
return (*this) == other;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Ordering.h
|
||||
* @author Richard Roberts
|
||||
* @date Sep 2, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
class Ordering : public std::vector<Key> {
|
||||
protected:
|
||||
typedef std::vector<Key> Base;
|
||||
|
||||
public:
|
||||
typedef Ordering This; ///< Typedef to this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
|
||||
/// Create an empty ordering
|
||||
GTSAM_EXPORT Ordering() {}
|
||||
|
||||
/// Create from a container
|
||||
template<typename KEYS>
|
||||
explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {}
|
||||
|
||||
/// Create an ordering using iterators over keys
|
||||
template<typename ITERATOR>
|
||||
Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}
|
||||
|
||||
/// 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);
|
||||
}
|
||||
|
||||
/// Invert (not reverse) the ordering - returns a map from key to order position
|
||||
FastMap<Key, size_t> invert() const;
|
||||
|
||||
/// @name Fill-reducing Orderings @{
|
||||
|
||||
/// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on
|
||||
/// performance). This internally builds a VariableIndex so if you already have a VariableIndex,
|
||||
/// it is faster to use COLAMD(const VariableIndex&)
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMD(const FactorGraph<FACTOR>& graph) {
|
||||
return COLAMD(VariableIndex(graph)); }
|
||||
|
||||
/// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
|
||||
static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
/// for note on performance). This internally builds a VariableIndex so if you already have a
|
||||
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
|
||||
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
|
||||
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
|
||||
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
|
||||
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& graph,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder = false) {
|
||||
return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
|
||||
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders
|
||||
/// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the
|
||||
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
|
||||
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
/// for note on performance). This internally builds a VariableIndex so if you already have a
|
||||
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
|
||||
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
|
||||
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
|
||||
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
|
||||
/// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrainedFirst(const FactorGraph<FACTOR>& graph,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
|
||||
return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
|
||||
/// function constrains the variables in \c constrainFirst to the front of the ordering, and
|
||||
/// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the
|
||||
/// variables in \c constrainFirst will be ordered in the same order specified in the
|
||||
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
|
||||
/// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
|
||||
/// reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
/// for note on performance). This internally builds a VariableIndex so if you already have a
|
||||
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group
|
||||
/// for each variable should be specified in \c groups, and each group of variables will appear
|
||||
/// in the ordering in group index order. \c groups should be a map from Key to group index.
|
||||
/// The group indices used should be consecutive starting at 0, but may appear in \c groups in
|
||||
/// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This
|
||||
/// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the
|
||||
/// CCOLAMD documentation for more information.
|
||||
template<class FACTOR>
|
||||
static Ordering COLAMDConstrained(const FactorGraph<FACTOR>& graph,
|
||||
const FastMap<Key, int>& groups) {
|
||||
return COLAMDConstrained(VariableIndex(graph), groups); }
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this
|
||||
/// function, a group for each variable should be specified in \c groups, and each group of
|
||||
/// variables will appear in the ordering in group index order. \c groups should be a map from
|
||||
/// Key to group index. The group indices used should be consecutive starting at 0, but may
|
||||
/// appear in \c groups in arbitrary order. Any variables not present in \c groups will be
|
||||
/// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the
|
||||
/// supplied indices, see the CCOLAMD documentation for more information.
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
|
||||
const FastMap<Key, int>& groups);
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Testable @{
|
||||
|
||||
GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/// Internal COLAMD function
|
||||
static GTSAM_EXPORT Ordering COLAMDConstrained(
|
||||
const VariableIndex& variableIndex, std::vector<int>& cmember);
|
||||
|
||||
/** 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);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -1,226 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Permutation.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 9, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Permutation::Identity(Index nVars) {
|
||||
Permutation ret(nVars);
|
||||
for(Index i=0; i<nVars; ++i)
|
||||
ret[i] = i;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size, bool filterDuplicates) {
|
||||
|
||||
Permutation ret(size);
|
||||
|
||||
// Mask of which variables have been pulled, used to reorder
|
||||
vector<bool> pulled(size, false);
|
||||
|
||||
// Put the pulled variables at the front of the permutation and set up the
|
||||
// pulled flags.
|
||||
size_t toFrontUniqueSize = 0;
|
||||
for(Index j=0; j<toFront.size(); ++j) {
|
||||
if(!pulled[toFront[j]]) {
|
||||
ret[j] = toFront[j];
|
||||
pulled[toFront[j]] = true;
|
||||
++ toFrontUniqueSize;
|
||||
} else if(!filterDuplicates) {
|
||||
stringstream ss;
|
||||
ss << "Duplicate variable given as input to Permutation::PullToFront:\n";
|
||||
ss << " toFront:";
|
||||
BOOST_FOREACH(Index i, toFront) { ss << " " << i; }
|
||||
ss << ", size = " << size << endl;
|
||||
throw invalid_argument(ss.str());
|
||||
}
|
||||
}
|
||||
|
||||
// Fill in the rest of the variables
|
||||
Index nextVar = toFrontUniqueSize;
|
||||
for(Index j=0; j<size; ++j)
|
||||
if(!pulled[j])
|
||||
ret[nextVar++] = j;
|
||||
assert(nextVar == size);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates) {
|
||||
|
||||
Permutation ret(size);
|
||||
|
||||
// Mask of which variables have been pushed, used to reorder
|
||||
vector<bool> pushed(size, false);
|
||||
|
||||
// Put the pushed variables at the back of the permutation and set up the
|
||||
// pushed flags;
|
||||
Index nextVar = size;
|
||||
size_t toBackUniqueSize = 0;
|
||||
if(toBack.size() > 0) {
|
||||
Index j = toBack.size();
|
||||
do {
|
||||
-- j;
|
||||
if(!pushed[toBack[j]]) {
|
||||
ret[--nextVar] = toBack[j];
|
||||
pushed[toBack[j]] = true;
|
||||
++ toBackUniqueSize;
|
||||
} else if(!filterDuplicates) {
|
||||
stringstream ss;
|
||||
ss << "Duplicate variable given as input to Permutation::PushToBack:\n";
|
||||
ss << " toBack:";
|
||||
BOOST_FOREACH(Index i, toBack) { ss << " " << i; }
|
||||
ss << ", size = " << size << endl;
|
||||
throw invalid_argument(ss.str());
|
||||
}
|
||||
} while(j > 0);
|
||||
}
|
||||
assert(nextVar == size - toBackUniqueSize);
|
||||
|
||||
// Fill in the rest of the variables
|
||||
nextVar = 0;
|
||||
for(Index j=0; j<size; ++j)
|
||||
if(!pushed[j])
|
||||
ret[nextVar++] = j;
|
||||
assert(nextVar == size - toBackUniqueSize);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation::shared_ptr Permutation::permute(const Permutation& permutation) const {
|
||||
const size_t nVars = permutation.size();
|
||||
Permutation::shared_ptr result(new Permutation(nVars));
|
||||
for(size_t j=0; j<nVars; ++j) {
|
||||
assert(permutation[j] < rangeIndices_.size());
|
||||
(*result)[j] = operator[](permutation[j]);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation::shared_ptr Permutation::inverse() const {
|
||||
Permutation::shared_ptr result(new Permutation(this->size()));
|
||||
for(Index i=0; i<this->size(); ++i) {
|
||||
assert((*this)[i] < this->size());
|
||||
(*result)[(*this)[i]] = i;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Permutation::print(const std::string& str) const {
|
||||
std::cout << str << " ";
|
||||
BOOST_FOREACH(Index s, rangeIndices_) { std::cout << s << " "; }
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
namespace internal {
|
||||
/* ************************************************************************* */
|
||||
Reduction Reduction::CreateAsInverse(const Permutation& p) {
|
||||
Reduction result;
|
||||
for(Index j = 0; j < p.size(); ++j)
|
||||
result.insert(std::make_pair(p[j], j));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
|
||||
if(selector.size() != p.size())
|
||||
throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
|
||||
Reduction result;
|
||||
for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
|
||||
result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Reduction::applyInverse(std::vector<Index>& js) const {
|
||||
BOOST_FOREACH(Index& j, js) {
|
||||
j = this->find(j)->second;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Reduction::inverse() const {
|
||||
Index maxIndex = 0;
|
||||
BOOST_FOREACH(const value_type& target_source, *this) {
|
||||
if(target_source.second > maxIndex)
|
||||
maxIndex = target_source.second;
|
||||
}
|
||||
Permutation result(maxIndex + 1);
|
||||
BOOST_FOREACH(const value_type& target_source, *this) {
|
||||
result[target_source.second] = target_source.first;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Index& Reduction::operator[](const Index& j) {
|
||||
iterator it = this->find(j);
|
||||
if(it == this->end())
|
||||
return j;
|
||||
else
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Index& Reduction::operator[](const Index& j) const {
|
||||
const_iterator it = this->find(j);
|
||||
if(it == this->end())
|
||||
return j;
|
||||
else
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Reduction::print(const std::string& s) const {
|
||||
cout << s << " reduction:" << endl;
|
||||
BOOST_FOREACH(const value_type& p, *this)
|
||||
cout << " " << p.first << " : " << p.second << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Reduction::equals(const Reduction& other, double tol) const {
|
||||
return (const Base&)(*this) == (const Base&)other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation createReducingPermutation(const std::set<Index>& indices) {
|
||||
Permutation p(indices.size());
|
||||
Index newJ = 0;
|
||||
BOOST_FOREACH(Index j, indices) {
|
||||
p[newJ] = j;
|
||||
++ newJ;
|
||||
}
|
||||
return p;
|
||||
}
|
||||
} // \namespace internal
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace gtsam
|
||||
|
|
@ -1,215 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Permutation.h
|
||||
* @author Richard Roberts
|
||||
* @date Sep 12, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <set>
|
||||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A permutation reorders variables, for example to reduce fill-in during
|
||||
* elimination. To save computation, the permutation can be applied to
|
||||
* the necessary data structures only once, then multiple computations
|
||||
* performed on the permuted problem. For example, in an iterative
|
||||
* non-linear setting, a permutation can be created from the symbolic graph
|
||||
* structure and applied to the ordering of nonlinear variables once, so
|
||||
* that linearized factor graphs are already correctly ordered and need
|
||||
* not be permuted.
|
||||
*
|
||||
* Consistent with their mathematical definition, permutations are functions
|
||||
* that accept the destination index and return the source index. For example,
|
||||
* to permute data structure A into a new data structure B using permutation P,
|
||||
* the mapping is B[i] = A[P[i]]. This is the behavior implemented by
|
||||
* B = A.permute(P).
|
||||
*
|
||||
* For some data structures in GTSAM, for efficiency it is necessary to have
|
||||
* the inverse of the desired permutation. In this case, the data structure
|
||||
* will implement permuteWithInverse(P) instead of permute(P). You may already
|
||||
* have the inverse permutation by construction, or may instead compute it with
|
||||
* Pinv = P.inverse().
|
||||
*
|
||||
* Permutations can be composed and inverted
|
||||
* in order to create new indexing for a structure.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Permutation {
|
||||
protected:
|
||||
std::vector<Index> rangeIndices_;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<Permutation> shared_ptr;
|
||||
typedef std::vector<Index>::const_iterator const_iterator;
|
||||
typedef std::vector<Index>::iterator iterator;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Create an empty permutation. This cannot do anything, but you can later
|
||||
* assign to it.
|
||||
*/
|
||||
Permutation() {}
|
||||
|
||||
/**
|
||||
* Create an uninitialized permutation. You must assign all values using the
|
||||
* square bracket [] operator or they will be undefined!
|
||||
*/
|
||||
Permutation(Index nVars) : rangeIndices_(nVars) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& str = "Permutation: ") const;
|
||||
|
||||
/** Check equality */
|
||||
bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; }
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Return the source index of the supplied destination index.
|
||||
*/
|
||||
Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; }
|
||||
|
||||
/**
|
||||
* Return the source index of the supplied destination index. This version allows modification.
|
||||
*/
|
||||
Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; }
|
||||
|
||||
/**
|
||||
* Return the source index of the supplied destination index. Synonym for operator[](Index).
|
||||
*/
|
||||
Index at(Index variable) const { return operator[](variable); }
|
||||
|
||||
/**
|
||||
* Return the source index of the supplied destination index. This version allows modification. Synonym for operator[](Index).
|
||||
*/
|
||||
Index& at(Index variable) { return operator[](variable); }
|
||||
|
||||
/**
|
||||
* The number of variables in the range of this permutation, i.e. the
|
||||
* destination space.
|
||||
*/
|
||||
Index size() const { return rangeIndices_.size(); }
|
||||
|
||||
/** Whether the permutation contains any entries */
|
||||
bool empty() const { return rangeIndices_.empty(); }
|
||||
|
||||
/**
|
||||
* Resize the permutation. You must fill in the new entries if new new size
|
||||
* is larger than the old one. If the new size is smaller, entries from the
|
||||
* end are discarded.
|
||||
*/
|
||||
void resize(size_t newSize) { rangeIndices_.resize(newSize); }
|
||||
|
||||
/**
|
||||
* Return an identity permutation.
|
||||
*/
|
||||
static Permutation Identity(Index nVars);
|
||||
|
||||
/**
|
||||
* Create a permutation that pulls the given variables to the front while
|
||||
* pushing the rest to the back.
|
||||
*/
|
||||
static Permutation PullToFront(const std::vector<Index>& toFront, size_t size, bool filterDuplicates = false);
|
||||
|
||||
/**
|
||||
* Create a permutation that pushes the given variables to the back while
|
||||
* pulling the rest to the front.
|
||||
*/
|
||||
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates = false);
|
||||
|
||||
|
||||
/**
|
||||
* Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]].
|
||||
*/
|
||||
Permutation::shared_ptr permute(const Permutation& permutation) const;
|
||||
|
||||
/**
|
||||
* Return the inverse permutation. This is only possible if this is a non-
|
||||
* reducing permutation, that is, (*this)[i] < this->size() for all
|
||||
* i < size(). If NDEBUG is not defined, this conditional will be checked.
|
||||
*/
|
||||
Permutation::shared_ptr inverse() const;
|
||||
|
||||
const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices
|
||||
const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices
|
||||
|
||||
/** Apply the permutation to a collection, which must have operator[] defined.
|
||||
* Note that permutable gtsam data structures typically have their own
|
||||
* permute function to apply a permutation. Permutation::applyToCollection is
|
||||
* a generic function, e.g. for STL classes.
|
||||
* @param input The input collection.
|
||||
* @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]]
|
||||
*/
|
||||
template<typename INPUT_COLLECTION, typename OUTPUT_COLLECTION>
|
||||
void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const {
|
||||
for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; }
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
|
||||
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices
|
||||
|
||||
protected:
|
||||
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
||||
namespace internal {
|
||||
/**
|
||||
* An internal class used for storing and applying a permutation from a map
|
||||
*/
|
||||
class Reduction : public gtsam::FastMap<Index,Index> {
|
||||
public:
|
||||
typedef gtsam::FastMap<Index,Index> Base;
|
||||
|
||||
GTSAM_EXPORT static Reduction CreateAsInverse(const Permutation& p);
|
||||
GTSAM_EXPORT static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
|
||||
GTSAM_EXPORT void applyInverse(std::vector<Index>& js) const;
|
||||
GTSAM_EXPORT Permutation inverse() const;
|
||||
GTSAM_EXPORT const Index& operator[](const Index& j);
|
||||
GTSAM_EXPORT const Index& operator[](const Index& j) const;
|
||||
|
||||
GTSAM_EXPORT void print(const std::string& s="") const;
|
||||
GTSAM_EXPORT bool equals(const Reduction& other, double tol = 1e-9) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* Reduce the variable indices so that those in the set are mapped to start at zero
|
||||
*/
|
||||
GTSAM_EXPORT Permutation createReducingPermutation(const std::set<Index>& indices);
|
||||
} // \namespace internal
|
||||
} // \namespace gtsam
|
||||
|
|
@ -1,132 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactorGraph.cpp
|
||||
* @date Oct 29, 2009
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesNet& bayesNet) :
|
||||
FactorGraph<IndexFactor>(bayesNet) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicFactorGraph::SymbolicFactorGraph(const SymbolicBayesTree& bayesTree) :
|
||||
FactorGraph<IndexFactor>(bayesTree) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraph::push_factor(Index key) {
|
||||
push_back(boost::make_shared<IndexFactor>(key));
|
||||
}
|
||||
|
||||
/** Push back binary factor */
|
||||
void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
|
||||
push_back(boost::make_shared<IndexFactor>(key1,key2));
|
||||
}
|
||||
|
||||
/** Push back ternary factor */
|
||||
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
|
||||
push_back(boost::make_shared<IndexFactor>(key1,key2,key3));
|
||||
}
|
||||
|
||||
/** Push back 4-way factor */
|
||||
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) {
|
||||
push_back(boost::make_shared<IndexFactor>(key1,key2,key3,key4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index>
|
||||
SymbolicFactorGraph::keys() const {
|
||||
FastSet<Index> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
if(factor) keys.insert(factor->begin(), factor->end()); }
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const
|
||||
{
|
||||
return FactorGraph<IndexFactor>::eliminateFrontals(nFrontals, EliminateSymbolic);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
SymbolicFactorGraph::eliminate(const std::vector<Index>& variables) const
|
||||
{
|
||||
return FactorGraph<IndexFactor>::eliminate(variables, EliminateSymbolic);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
SymbolicFactorGraph::eliminateOne(Index variable) const
|
||||
{
|
||||
return FactorGraph<IndexFactor>::eliminateOne(variable, EliminateSymbolic);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraph::permuteWithInverse(
|
||||
const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
if(factor)
|
||||
factor->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraph::reduceWithInverse(
|
||||
const internal::Reduction& inverseReduction) {
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
if(factor)
|
||||
factor->reduceWithInverse(inverseReduction);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
IndexFactor::shared_ptr CombineSymbolic(
|
||||
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
|
||||
vector<Index> >& variableSlots) {
|
||||
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots));
|
||||
// combined->assertInvariants();
|
||||
return combined;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> //
|
||||
EliminateSymbolic(const FactorGraph<IndexFactor>& factors, size_t nrFrontals) {
|
||||
|
||||
FastSet<Index> keys;
|
||||
BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors)
|
||||
BOOST_FOREACH(Index var, *factor)
|
||||
keys.insert(var);
|
||||
|
||||
if (keys.size() < nrFrontals) throw invalid_argument(
|
||||
"EliminateSymbolic requested to eliminate more variables than exist in graph.");
|
||||
|
||||
vector<Index> newKeys(keys.begin(), keys.end());
|
||||
return make_pair(boost::make_shared<IndexConditional>(newKeys, nrFrontals),
|
||||
boost::make_shared<IndexFactor>(newKeys.begin() + nrFrontals, newKeys.end()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
}
|
||||
|
|
@ -1,151 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactorGraph.h
|
||||
* @date Oct 29, 2009
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
|
||||
namespace gtsam { template<class FACTOR> class EliminationTree; }
|
||||
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
|
||||
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTree; }
|
||||
namespace gtsam { class IndexConditional; }
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
|
||||
typedef BayesNet<IndexConditional> SymbolicBayesNet;
|
||||
typedef BayesTree<IndexConditional> SymbolicBayesTree;
|
||||
|
||||
/** Symbolic IndexFactor Graph
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct empty factor graph */
|
||||
SymbolicFactorGraph() {
|
||||
}
|
||||
|
||||
/** Construct from a BayesNet */
|
||||
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesNet& bayesNet);
|
||||
|
||||
/** Construct from a BayesTree */
|
||||
GTSAM_EXPORT SymbolicFactorGraph(const SymbolicBayesTree& bayesTree);
|
||||
|
||||
/**
|
||||
* Construct from a factor graph of any type
|
||||
*/
|
||||
template<class FACTOR>
|
||||
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
|
||||
|
||||
/** Eliminate the first \c n frontal variables, returning the resulting
|
||||
* conditional and remaining factor graph - this is very inefficient for
|
||||
* eliminating all variables, to do that use EliminationTree or
|
||||
* JunctionTree. Note that this version simply calls
|
||||
* FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
|
||||
* as the eliminate function argument.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
|
||||
|
||||
/** Factor the factor graph into a conditional and a remaining factor graph.
|
||||
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
|
||||
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
|
||||
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
|
||||
* a probability density or likelihood, the factorization produces a
|
||||
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
|
||||
*
|
||||
* For efficiency, this function treats the variables to eliminate
|
||||
* \c variables as fully-connected, so produces a dense (fully-connected)
|
||||
* conditional on all of the variables in \c variables, instead of a sparse
|
||||
* BayesNet. If the variables are not fully-connected, it is more efficient
|
||||
* to sequentially factorize multiple times.
|
||||
* Note that this version simply calls
|
||||
* FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
|
||||
* function argument.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminate(const std::vector<Index>& variables) const;
|
||||
|
||||
/** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
|
||||
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateOne(Index variable) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Return the set of variables involved in the factors (computes a set
|
||||
* union).
|
||||
*/
|
||||
GTSAM_EXPORT FastSet<Index> keys() const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Push back unary factor */
|
||||
GTSAM_EXPORT void push_factor(Index key);
|
||||
|
||||
/** Push back binary factor */
|
||||
GTSAM_EXPORT void push_factor(Index key1, Index key2);
|
||||
|
||||
/** Push back ternary factor */
|
||||
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3);
|
||||
|
||||
/** Push back 4-way factor */
|
||||
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4);
|
||||
|
||||
/** Permute the variables in the factors */
|
||||
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/** Apply a reduction, which is a remapping of variable indices. */
|
||||
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
|
||||
|
||||
};
|
||||
|
||||
/** Create a combined joint factor (new style for EliminationTree). */
|
||||
GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph<IndexFactor>& factors,
|
||||
const FastMap<Index, std::vector<Index> >& variableSlots);
|
||||
|
||||
/**
|
||||
* CombineAndEliminate provides symbolic elimination.
|
||||
* Combine and eliminate can also be called separately, but for this and
|
||||
* derived classes calling them separately generally does extra work.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
|
||||
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
|
||||
|
||||
/// @}
|
||||
|
||||
/** Template function implementation */
|
||||
template<class FACTOR>
|
||||
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
|
||||
for (size_t i = 0; i < fg.size(); i++) {
|
||||
if (fg[i]) {
|
||||
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
|
||||
push_back(factor);
|
||||
} else
|
||||
push_back(IndexFactor::shared_ptr());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicMultifrontalSolver.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 22, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//template class GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > >;
|
||||
|
||||
}
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicMultifrontalSolver.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 22, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/GenericMultifrontalSolver.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > {
|
||||
|
||||
protected:
|
||||
typedef GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > Base;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Construct the solver for a factor graph. This builds the junction
|
||||
* tree, which does the symbolic elimination, identifies the cliques,
|
||||
* and distributes all the factors to the right cliques.
|
||||
*/
|
||||
SymbolicMultifrontalSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {};
|
||||
|
||||
/**
|
||||
* Construct the solver with a shared pointer to a factor graph and to a
|
||||
* VariableIndex. The solver will store these pointers, so this constructor
|
||||
* is the fastest.
|
||||
*/
|
||||
SymbolicMultifrontalSolver(const SymbolicFactorGraph::shared_ptr& factorGraph,
|
||||
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
|
||||
|
||||
/** Print to cout */
|
||||
void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); };
|
||||
|
||||
/** Test whether is equal to another */
|
||||
bool equals(const SymbolicMultifrontalSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); };
|
||||
|
||||
/**
|
||||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
SymbolicBayesTree::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. Returns the result as a factor graph.
|
||||
*/
|
||||
SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal Gaussian density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicSequentialSolver.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
||||
#include <gtsam/inference/GenericSequentialSolver-inl.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// An explicit instantiation to be compiled into the library
|
||||
//template class GenericSequentialSolver<IndexFactor>;
|
||||
|
||||
}
|
||||
|
|
@ -1,88 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicSequentialSolver.h
|
||||
* @author Richard Roberts
|
||||
* @date Oct 21, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/GenericSequentialSolver.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
|
||||
|
||||
protected:
|
||||
typedef GenericSequentialSolver<IndexFactor> Base;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Construct the solver for a factor graph. This builds the junction
|
||||
* tree, which does the symbolic elimination, identifies the cliques,
|
||||
* and distributes all the factors to the right cliques.
|
||||
*/
|
||||
SymbolicSequentialSolver(const SymbolicFactorGraph& factorGraph) : Base(factorGraph) {};
|
||||
|
||||
/**
|
||||
* Construct the solver with a shared pointer to a factor graph and to a
|
||||
* VariableIndex. The solver will store these pointers, so this constructor
|
||||
* is the fastest.
|
||||
*/
|
||||
SymbolicSequentialSolver(const SymbolicFactorGraph::shared_ptr& factorGraph,
|
||||
const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
|
||||
|
||||
/** Print to cout */
|
||||
void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); };
|
||||
|
||||
/** Test whether is equal to another */
|
||||
bool equals(const SymbolicSequentialSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); };
|
||||
|
||||
/**
|
||||
* Eliminate the factor graph sequentially. Uses a column elimination tree
|
||||
* to recursively eliminate.
|
||||
*/
|
||||
SymbolicBayesNet::shared_ptr eliminate() const
|
||||
{ return Base::eliminate(&EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute a conditional density P(F|S) while marginalizing out variables J
|
||||
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
|
||||
* Returns the result as a Bayes net.
|
||||
*/
|
||||
SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const
|
||||
{ return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. Returns the result as a Bayes net.
|
||||
*/
|
||||
SymbolicBayesNet::shared_ptr jointBayesNet(const std::vector<Index>& js) const
|
||||
{ return Base::jointBayesNet(js, &EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal joint over a set of variables, by integrating out
|
||||
* all of the other variables. Returns the result as a factor graph.
|
||||
*/
|
||||
SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const
|
||||
{ return Base::jointFactorGraph(js, &EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal Gaussian density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
IndexFactor::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VariableIndex-inl.h
|
||||
* @author Richard Roberts
|
||||
* @date March 26, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void VariableIndex::augment(const FG& factors)
|
||||
{
|
||||
gttic(VariableIndex_augment);
|
||||
|
||||
// Save original number of factors for keeping track of indices
|
||||
const size_t originalNFactors = nFactors_;
|
||||
|
||||
// Augment index for each factor
|
||||
for(size_t i = 0; i < factors.size(); ++i) {
|
||||
if(factors[i]) {
|
||||
const size_t globalI = originalNFactors + i;
|
||||
BOOST_FOREACH(const Key key, *factors[i]) {
|
||||
index_[key].push_back(globalI);
|
||||
++ nEntries_;
|
||||
}
|
||||
}
|
||||
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename ITERATOR, class FG>
|
||||
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors)
|
||||
{
|
||||
gttic(VariableIndex_remove);
|
||||
|
||||
// NOTE: We intentionally do not decrement nFactors_ because the factor
|
||||
// indices need to remain consistent. Removing factors from a factor graph
|
||||
// does not shift the indices of other factors. Also, we keep nFactors_
|
||||
// one greater than the highest-numbered factor referenced in a VariableIndex.
|
||||
ITERATOR factorIndex = firstFactor;
|
||||
size_t i = 0;
|
||||
for( ; factorIndex != lastFactor; ++factorIndex, ++i) {
|
||||
if(i >= factors.size())
|
||||
throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
|
||||
if(factors[i]) {
|
||||
BOOST_FOREACH(Key j, *factors[i]) {
|
||||
Factors& factorEntries = internalAt(j);
|
||||
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex);
|
||||
if(entry == factorEntries.end())
|
||||
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
|
||||
factorEntries.erase(entry);
|
||||
-- nEntries_;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename ITERATOR>
|
||||
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
|
||||
for(ITERATOR key = firstKey; key != lastKey; ++key) {
|
||||
assert(internalAt(*key).empty());
|
||||
index_.erase(*key);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,104 +1,58 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VariableIndex.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 22, 2010
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
||||
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
|
||||
for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var)
|
||||
if(var >= this->index_.size() || var >= other.index_.size()) {
|
||||
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
|
||||
(var >= other.index_.size() && this->index_[var].empty())))
|
||||
return false;
|
||||
} else if(this->index_[var] != other.index_[var])
|
||||
return false;
|
||||
} else
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::print(const string& str) const {
|
||||
cout << str;
|
||||
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
||||
for(Index var = 0; var < size(); ++var) {
|
||||
cout << "var " << var << ":";
|
||||
BOOST_FOREACH(const size_t factor, index_[var])
|
||||
cout << " " << factor;
|
||||
cout << "\n";
|
||||
}
|
||||
cout << flush;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::outputMetisFormat(ostream& os) const {
|
||||
os << size() << " " << nFactors() << "\n";
|
||||
// run over variables, which will be hyper-edges.
|
||||
BOOST_FOREACH(const Factors& variable, index_) {
|
||||
// every variable is a hyper-edge covering its factors
|
||||
BOOST_FOREACH(const size_t factor, variable)
|
||||
os << (factor+1) << " "; // base 1
|
||||
os << "\n";
|
||||
}
|
||||
os << flush;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::permuteInPlace(const Permutation& permutation) {
|
||||
// Create new index and move references to data into it in permuted order
|
||||
vector<VariableIndex::Factors> newIndex(this->size());
|
||||
for(Index i = 0; i < newIndex.size(); ++i)
|
||||
newIndex[i].swap(this->index_[permutation[i]]);
|
||||
|
||||
// Move reference to entire index into the VariableIndex
|
||||
index_.swap(newIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
|
||||
if(selector.size() != permutation.size())
|
||||
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
|
||||
// Create new index the size of the permuted entries
|
||||
vector<VariableIndex::Factors> newIndex(selector.size());
|
||||
// Permute the affected entries into the new index
|
||||
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
|
||||
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
|
||||
// Put the affected entries back in the new order
|
||||
for(size_t slot = 0; slot < selector.size(); ++slot)
|
||||
this->index_[selector[slot]].swap(newIndex[slot]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
|
||||
if(!(*this)[i].empty())
|
||||
throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
|
||||
#endif
|
||||
|
||||
index_.resize(this->size() - nToRemove);
|
||||
}
|
||||
|
||||
}
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VariableIndex.cpp
|
||||
* @author Richard Roberts
|
||||
* @date March 26, 2013
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
||||
return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_
|
||||
&& this->index_ == other.index_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::print(const string& str) const {
|
||||
cout << str;
|
||||
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
||||
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
|
||||
cout << "var " << key_factors.first << ":";
|
||||
BOOST_FOREACH(const size_t factor, key_factors.second)
|
||||
cout << " " << factor;
|
||||
cout << "\n";
|
||||
}
|
||||
cout.flush();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VariableIndex::outputMetisFormat(ostream& os) const {
|
||||
os << size() << " " << nFactors() << "\n";
|
||||
// run over variables, which will be hyper-edges.
|
||||
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
|
||||
// every variable is a hyper-edge covering its factors
|
||||
BOOST_FOREACH(const size_t factor, key_factors.second)
|
||||
os << (factor+1) << " "; // base 1
|
||||
os << "\n";
|
||||
}
|
||||
os << flush;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,292 +1,179 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VariableIndex.h
|
||||
* @author Richard Roberts
|
||||
* @date Sep 12, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <stdexcept>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Permutation;
|
||||
|
||||
/**
|
||||
* The VariableIndex class computes and stores the block column structure of a
|
||||
* factor graph. The factor graph stores a collection of factors, each of
|
||||
* which involves a set of variables. In contrast, the VariableIndex is built
|
||||
* from a factor graph prior to elimination, and stores the list of factors
|
||||
* that involve each variable. This information is stored as a deque of
|
||||
* lists of factor indices.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT VariableIndex {
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<VariableIndex> shared_ptr;
|
||||
typedef FastList<size_t> Factors;
|
||||
typedef Factors::iterator Factor_iterator;
|
||||
typedef Factors::const_iterator Factor_const_iterator;
|
||||
|
||||
protected:
|
||||
std::vector<Factors> index_;
|
||||
size_t nFactors_; // Number of factors in the original factor graph.
|
||||
size_t nEntries_; // Sum of involved variable counts of each factor.
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor, creates an empty VariableIndex */
|
||||
VariableIndex() : nFactors_(0), nEntries_(0) {}
|
||||
|
||||
/**
|
||||
* Create a VariableIndex that computes and stores the block column structure
|
||||
* of a factor graph. This constructor is used when the number of variables
|
||||
* is known beforehand.
|
||||
*/
|
||||
template<class FG> VariableIndex(const FG& factorGraph, Index nVariables);
|
||||
|
||||
/**
|
||||
* Create a VariableIndex that computes and stores the block column structure
|
||||
* of a factor graph.
|
||||
*/
|
||||
template<class FG> VariableIndex(const FG& factorGraph);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* The number of variable entries. This is one greater than the variable
|
||||
* with the highest index.
|
||||
*/
|
||||
Index size() const { return index_.size(); }
|
||||
|
||||
/** The number of factors in the original factor graph */
|
||||
size_t nFactors() const { return nFactors_; }
|
||||
|
||||
/** The number of nonzero blocks, i.e. the number of variable-factor entries */
|
||||
size_t nEntries() const { return nEntries_; }
|
||||
|
||||
/** Access a list of factors by variable */
|
||||
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Test for equality (for unit tests and debug assertions). */
|
||||
bool equals(const VariableIndex& other, double tol=0.0) const;
|
||||
|
||||
/** Print the variable index (for unit tests and debugging). */
|
||||
void print(const std::string& str = "VariableIndex: ") const;
|
||||
|
||||
/**
|
||||
* Output dual hypergraph to Metis file format for use with hmetis
|
||||
* In the dual graph, variables are hyperedges, factors are nodes.
|
||||
*/
|
||||
void outputMetisFormat(std::ostream& os) const;
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Augment the variable index with new factors. This can be used when
|
||||
* solving problems incrementally.
|
||||
*/
|
||||
template<class FG> void augment(const FG& factors);
|
||||
|
||||
/**
|
||||
* Remove entries corresponding to the specified factors.
|
||||
* NOTE: We intentionally do not decrement nFactors_ because the factor
|
||||
* indices need to remain consistent. Removing factors from a factor graph
|
||||
* does not shift the indices of other factors. Also, we keep nFactors_
|
||||
* one greater than the highest-numbered factor referenced in a VariableIndex.
|
||||
*
|
||||
* @param indices The indices of the factors to remove, which must match \c factors
|
||||
* @param factors The factors being removed, which must symbolically correspond
|
||||
* exactly to the factors with the specified \c indices that were added.
|
||||
*/
|
||||
template<typename CONTAINER, class FG> void remove(const CONTAINER& indices, const FG& factors);
|
||||
|
||||
/// Permute the variables in the VariableIndex according to the given permutation
|
||||
void permuteInPlace(const Permutation& permutation);
|
||||
|
||||
/// Permute the variables in the VariableIndex according to the given partial permutation
|
||||
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
|
||||
|
||||
/** Remove unused empty variables at the end of the ordering (in debug mode
|
||||
* verifies they are empty).
|
||||
* @param nToRemove The number of unused variables at the end to remove
|
||||
*/
|
||||
void removeUnusedAtEnd(size_t nToRemove);
|
||||
|
||||
protected:
|
||||
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
|
||||
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
|
||||
|
||||
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
|
||||
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
|
||||
|
||||
/// Internal constructor to allocate a VariableIndex of the requested size
|
||||
VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
|
||||
|
||||
/// Internal check of the validity of a variable
|
||||
void checkVar(Index variable) const { assert(variable < index_.size()); }
|
||||
|
||||
/// Internal function to populate the variable index from a factor graph
|
||||
template<class FG> void fill(const FG& factorGraph);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(index_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nFactors_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nEntries_);
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void VariableIndex::fill(const FG& factorGraph) {
|
||||
gttic(VariableIndex_fill);
|
||||
// Build index mapping from variable id to factor index
|
||||
for(size_t fi=0; fi<factorGraph.size(); ++fi) {
|
||||
if(factorGraph[fi]) {
|
||||
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
|
||||
if(key < index_.size()) {
|
||||
index_[key].push_back(fi);
|
||||
++ nEntries_;
|
||||
}
|
||||
}
|
||||
}
|
||||
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
VariableIndex::VariableIndex(const FG& factorGraph) :
|
||||
nFactors_(0), nEntries_(0)
|
||||
{
|
||||
gttic(VariableIndex_constructor);
|
||||
// If the factor graph is empty, return an empty index because inside this
|
||||
// if block we assume at least one factor.
|
||||
if(factorGraph.size() > 0) {
|
||||
gttic(VariableIndex_constructor_find_highest);
|
||||
// Find highest-numbered variable
|
||||
Index maxVar = 0;
|
||||
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
|
||||
if(factor) {
|
||||
BOOST_FOREACH(const Index key, factor->keys()) {
|
||||
if(key > maxVar)
|
||||
maxVar = key;
|
||||
}
|
||||
}
|
||||
}
|
||||
gttoc(VariableIndex_constructor_find_highest);
|
||||
|
||||
// Allocate array
|
||||
gttic(VariableIndex_constructor_allocate);
|
||||
index_.resize(maxVar+1);
|
||||
gttoc(VariableIndex_constructor_allocate);
|
||||
|
||||
fill(factorGraph);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
VariableIndex::VariableIndex(const FG& factorGraph, Index nVariables) :
|
||||
nFactors_(0), nEntries_(0)
|
||||
{
|
||||
gttic(VariableIndex_constructor_allocate);
|
||||
index_.resize(nVariables);
|
||||
gttoc(VariableIndex_constructor_allocate);
|
||||
fill(factorGraph);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
void VariableIndex::augment(const FG& factors) {
|
||||
gttic(VariableIndex_augment);
|
||||
// If the factor graph is empty, return an empty index because inside this
|
||||
// if block we assume at least one factor.
|
||||
if(factors.size() > 0) {
|
||||
// Find highest-numbered variable
|
||||
Index maxVar = 0;
|
||||
BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) {
|
||||
if(factor) {
|
||||
BOOST_FOREACH(const Index key, factor->keys()) {
|
||||
if(key > maxVar)
|
||||
maxVar = key;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Allocate index
|
||||
index_.resize(std::max(index_.size(), maxVar+1));
|
||||
|
||||
// Augment index mapping from variable id to factor index
|
||||
size_t orignFactors = nFactors_;
|
||||
for(size_t fi=0; fi<factors.size(); ++fi) {
|
||||
if(factors[fi]) {
|
||||
BOOST_FOREACH(const Index key, factors[fi]->keys()) {
|
||||
index_[key].push_back(orignFactors + fi);
|
||||
++ nEntries_;
|
||||
}
|
||||
}
|
||||
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename CONTAINER, class FG>
|
||||
void VariableIndex::remove(const CONTAINER& indices, const FG& factors) {
|
||||
gttic(VariableIndex_remove);
|
||||
// NOTE: We intentionally do not decrement nFactors_ because the factor
|
||||
// indices need to remain consistent. Removing factors from a factor graph
|
||||
// does not shift the indices of other factors. Also, we keep nFactors_
|
||||
// one greater than the highest-numbered factor referenced in a VariableIndex.
|
||||
for(size_t fi=0; fi<factors.size(); ++fi)
|
||||
if(factors[fi]) {
|
||||
for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {
|
||||
Factors& factorEntries = index_[factors[fi]->keys()[ji]];
|
||||
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]);
|
||||
if(entry == factorEntries.end())
|
||||
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
|
||||
factorEntries.erase(entry);
|
||||
-- nEntries_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 VariableIndex.h
|
||||
* @author Richard Roberts
|
||||
* @date March 26, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <stdexcept>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* The VariableIndex class computes and stores the block column structure of a
|
||||
* factor graph. The factor graph stores a collection of factors, each of
|
||||
* which involves a set of variables. In contrast, the VariableIndex is built
|
||||
* from a factor graph prior to elimination, and stores the list of factors
|
||||
* that involve each variable. This information is stored as a deque of
|
||||
* lists of factor indices.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT VariableIndex {
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<VariableIndex> shared_ptr;
|
||||
typedef FastList<size_t> Factors;
|
||||
typedef Factors::iterator Factor_iterator;
|
||||
typedef Factors::const_iterator Factor_const_iterator;
|
||||
|
||||
protected:
|
||||
typedef FastMap<Key,Factors> KeyMap;
|
||||
KeyMap index_;
|
||||
size_t nFactors_; // Number of factors in the original factor graph.
|
||||
size_t nEntries_; // Sum of involved variable counts of each factor.
|
||||
|
||||
public:
|
||||
typedef KeyMap::const_iterator const_iterator;
|
||||
typedef KeyMap::const_iterator iterator;
|
||||
typedef KeyMap::value_type value_type;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor, creates an empty VariableIndex */
|
||||
VariableIndex() : nFactors_(0), nEntries_(0) {}
|
||||
|
||||
/**
|
||||
* Create a VariableIndex that computes and stores the block column structure
|
||||
* of a factor graph.
|
||||
*/
|
||||
template<class FG>
|
||||
VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* The number of variable entries. This is one greater than the variable
|
||||
* with the highest index.
|
||||
*/
|
||||
Index size() const { return index_.size(); }
|
||||
|
||||
/** The number of factors in the original factor graph */
|
||||
size_t nFactors() const { return nFactors_; }
|
||||
|
||||
/** The number of nonzero blocks, i.e. the number of variable-factor entries */
|
||||
size_t nEntries() const { return nEntries_; }
|
||||
|
||||
/** Access a list of factors by variable */
|
||||
const Factors& operator[](Key variable) const {
|
||||
KeyMap::const_iterator item = index_.find(variable);
|
||||
if(item == index_.end())
|
||||
throw std::invalid_argument("Requested non-existent variable from VariableIndex");
|
||||
else
|
||||
return item->second;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Test for equality (for unit tests and debug assertions). */
|
||||
bool equals(const VariableIndex& other, double tol=0.0) const;
|
||||
|
||||
/** Print the variable index (for unit tests and debugging). */
|
||||
void print(const std::string& str = "VariableIndex: ") const;
|
||||
|
||||
/**
|
||||
* Output dual hypergraph to Metis file format for use with hmetis
|
||||
* In the dual graph, variables are hyperedges, factors are nodes.
|
||||
*/
|
||||
void outputMetisFormat(std::ostream& os) const;
|
||||
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Augment the variable index with new factors. This can be used when
|
||||
* solving problems incrementally.
|
||||
*/
|
||||
template<class FG>
|
||||
void augment(const FG& factors);
|
||||
|
||||
/**
|
||||
* Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
|
||||
* nFactors_ because the factor indices need to remain consistent. Removing factors from a factor
|
||||
* graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than
|
||||
* the highest-numbered factor referenced in a VariableIndex.
|
||||
*
|
||||
* @param indices The indices of the factors to remove, which must match \c factors
|
||||
* @param factors The factors being removed, which must symbolically correspond exactly to the
|
||||
* factors with the specified \c indices that were added.
|
||||
*/
|
||||
template<typename ITERATOR, class FG>
|
||||
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
|
||||
|
||||
/** Remove unused empty variables (in debug mode verifies they are empty). */
|
||||
template<typename ITERATOR>
|
||||
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
|
||||
|
||||
/** Iterator to the first variable entry */
|
||||
const_iterator begin() const { return index_.begin(); }
|
||||
|
||||
/** Iterator to the first variable entry */
|
||||
const_iterator end() const { return index_.end(); }
|
||||
|
||||
/** Find the iterator for the requested variable entry */
|
||||
const_iterator find(Key key) const { return index_.find(key); }
|
||||
|
||||
protected:
|
||||
Factor_iterator factorsBegin(Index variable) { return internalAt(variable).begin(); }
|
||||
Factor_iterator factorsEnd(Index variable) { return internalAt(variable).end(); }
|
||||
|
||||
Factor_const_iterator factorsBegin(Index variable) const { return internalAt(variable).begin(); }
|
||||
Factor_const_iterator factorsEnd(Index variable) const { return internalAt(variable).end(); }
|
||||
|
||||
/// Internal version of 'at' that asserts existence
|
||||
const Factors& internalAt(Key variable) const {
|
||||
const KeyMap::const_iterator item = index_.find(variable);
|
||||
assert(item != index_.end());
|
||||
return item->second; }
|
||||
|
||||
/// Internal version of 'at' that asserts existence
|
||||
Factors& internalAt(Key variable) {
|
||||
const KeyMap::iterator item = index_.find(variable);
|
||||
assert(item != index_.end());
|
||||
return item->second; }
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/inference/VariableIndex-inl.h>
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ void VariableSlots::print(const std::string& str) const {
|
|||
for(size_t i=0; i<this->begin()->second.size(); ++i) {
|
||||
cout << " \t";
|
||||
BOOST_FOREACH(const value_type& slot, *this) {
|
||||
if(slot.second[i] == numeric_limits<Index>::max())
|
||||
if(slot.second[i] == numeric_limits<size_t>::max())
|
||||
cout << "x" << "\t";
|
||||
else
|
||||
cout << slot.second[i] << "\t";
|
||||
|
|
|
|||
|
|
@ -11,15 +11,16 @@
|
|||
|
||||
/**
|
||||
* @file VariableSlots.h
|
||||
* @brief VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors.
|
||||
* @brief VariableSlots describes the structure of a combined factor in terms of where each block
|
||||
* comes from in the source factors.
|
||||
* @author Richard Roberts
|
||||
* @date Oct 4, 2010
|
||||
*/
|
||||
* @date Oct 4, 2010 */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -31,31 +32,29 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A combined factor is assembled as one block of rows for each component
|
||||
* factor. In each row-block (factor), some of the column-blocks (variables)
|
||||
* may be empty since factors involving different sets of variables are
|
||||
* interleaved.
|
||||
*
|
||||
* VariableSlots describes the 2D block structure of the combined factor. It
|
||||
* is essentially a map<Index, vector<size_t> >. The Index is the real
|
||||
* variable index of the combined factor slot. The vector<size_t> tells, for
|
||||
* each row-block (factor), which column-block (variable slot) from the
|
||||
* component factor appears in this block of the combined factor.
|
||||
*
|
||||
* As an example, if the combined factor contains variables 1, 3, and 5, then
|
||||
* "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to
|
||||
* variable index 3), row-block 2 (also meaning component factor 2), comes from
|
||||
* column-block 0 of component factor 2.
|
||||
*
|
||||
* \nosubgrouping
|
||||
*/
|
||||
/** A combined factor is assembled as one block of rows for each component
|
||||
* factor. In each row-block (factor), some of the column-blocks (variables)
|
||||
* may be empty since factors involving different sets of variables are
|
||||
* interleaved.
|
||||
*
|
||||
* VariableSlots describes the 2D block structure of the combined factor. It
|
||||
* is a map<Index, vector<size_t> >. The Index is the real
|
||||
* variable index of the combined factor slot. The vector<size_t> tells, for
|
||||
* each row-block (factor), which column-block (variable slot) from the
|
||||
* component factor appears in this block of the combined factor.
|
||||
*
|
||||
* As an example, if the combined factor contains variables 1, 3, and 5, then
|
||||
* "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to
|
||||
* variable index 3), row-block 2 (also meaning component factor 2), comes from
|
||||
* column-block 0 of component factor 2.
|
||||
*
|
||||
* \nosubgrouping */
|
||||
|
||||
class VariableSlots : public FastMap<Index, std::vector<Index> > {
|
||||
class VariableSlots : public FastMap<Index, std::vector<size_t> > {
|
||||
|
||||
public:
|
||||
|
||||
typedef FastMap<Index, std::vector<Index> > Base;
|
||||
typedef FastMap<Index, std::vector<size_t> > Base;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -69,6 +68,7 @@ public:
|
|||
VariableSlots(const FG& factorGraph);
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -79,12 +79,13 @@ public:
|
|||
GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
VariableSlots::VariableSlots(const FG& factorGraph) {
|
||||
VariableSlots::VariableSlots(const FG& factorGraph)
|
||||
{
|
||||
gttic(VariableSlots_constructor);
|
||||
static const bool debug = false;
|
||||
|
||||
// Compute a mapping (called variableSlots) *from* each involved
|
||||
|
|
@ -96,7 +97,7 @@ VariableSlots::VariableSlots(const FG& factorGraph) {
|
|||
size_t jointFactorPos = 0;
|
||||
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
|
||||
assert(factor);
|
||||
Index factorVarSlot = 0;
|
||||
size_t factorVarSlot = 0;
|
||||
BOOST_FOREACH(const Index involvedVariable, *factor) {
|
||||
// Set the slot in this factor for this variable. If the
|
||||
// variable was not already discovered, create an array for it
|
||||
|
|
@ -105,9 +106,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) {
|
|||
// the array entry for each factor that will indicate the factor
|
||||
// does not involve the variable.
|
||||
iterator thisVarSlots; bool inserted;
|
||||
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<Index>()));
|
||||
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<size_t>()));
|
||||
if(inserted)
|
||||
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<Index>::max());
|
||||
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max());
|
||||
thisVarSlots->second[jointFactorPos] = factorVarSlot;
|
||||
if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
|
||||
++ factorVarSlot;
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue