Loading...
Searching...
No Matches
FactorGraph.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/inference/DotWriter.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Testable.h>
#include <Eigen/Core> // for Eigen::aligned_allocator
#include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <string>
#include <type_traits>
#include <utility>
#include <iosfwd>
namespace gtsam {
typedef FastVector<FactorIndex> FactorIndices;
// Forward declarations
template <class CLIQUE>
class BayesTree;
class HybridValues;
template <class C>
class CRefCallPushBack {
C& obj;
public:
explicit CRefCallPushBack(C& obj) : obj(obj) {}
template <typename A>
void operator()(const A& a) {
obj.push_back(a);
}
};
template <class C>
class RefCallPushBack {
C& obj;
public:
explicit RefCallPushBack(C& obj) : obj(obj) {}
template <typename A>
void operator()(A& a) {
obj.push_back(a);
}
};
template <class C>
class CRefCallAddCopy {
C& obj;
public:
explicit CRefCallAddCopy(C& obj) : obj(obj) {}
template <typename A>
void operator()(const A& a) {
obj.addCopy(a);
}
};
template <class FACTOR>
class FactorGraph {
public:
typedef FACTOR FactorType;
typedef boost::shared_ptr<FACTOR>
sharedFactor;
typedef sharedFactor value_type;
typedef typename FastVector<sharedFactor>::iterator iterator;
typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
private:
typedef FactorGraph<FACTOR> This;
typedef boost::shared_ptr<This>
shared_ptr;
template <typename DERIVEDFACTOR>
using IsDerived = typename std::enable_if<
std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
template <typename T>
using HasDerivedValueType = typename std::enable_if<
std::is_base_of<FactorType, typename T::value_type>::value>::type;
template <typename T>
using HasDerivedElementType = typename std::enable_if<std::is_base_of<
FactorType, typename T::value_type::element_type>::value>::type;
protected:
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
FastVector<sharedFactor> factors_;
bool isEqual(const FactorGraph& other) const {
return factors_ == other.factors_;
}
FactorGraph() {}
template <typename ITERATOR>
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
push_back(firstFactor, lastFactor);
}
template <class CONTAINER>
explicit FactorGraph(const CONTAINER& factors) {
push_back(factors);
}
public:
virtual ~FactorGraph() = default;
template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
FactorGraph(std::initializer_list<boost::shared_ptr<DERIVEDFACTOR>> sharedFactors)
: factors_(sharedFactors) {}
void reserve(size_t size) { factors_.reserve(size); }
template <class DERIVEDFACTOR>
IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
template <class DERIVEDFACTOR, class... Args>
IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
Eigen::aligned_allocator<DERIVEDFACTOR>(),
std::forward<Args>(args)...));
}
template <class DERIVEDFACTOR>
IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
}
template <class DERIVEDFACTOR>
IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
push_back(factor);
}
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);
}
template <typename ITERATOR>
HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
}
template <typename ITERATOR>
HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
ITERATOR lastFactor) {
for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
}
template <typename CONTAINER>
HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
template <typename CONTAINER>
HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
template <class FACTOR_OR_CONTAINER>
void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
push_back(factorOrContainer);
}
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);
}
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);
}
template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
FactorIndices add_factors(const CONTAINER& factors,
bool useEmptySlots = false);
virtual void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
bool equals(const This& fg, double tol = 1e-9) const;
public:
size_t size() const { return factors_.size(); }
bool empty() const { return factors_.empty(); }
const sharedFactor at(size_t i) const { return factors_.at(i); }
sharedFactor& at(size_t i) { return factors_.at(i); }
const sharedFactor operator[](size_t i) const { return at(i); }
sharedFactor& operator[](size_t i) { return at(i); }
const_iterator begin() const { return factors_.begin(); }
const_iterator end() const { return factors_.end(); }
sharedFactor front() const { return factors_.front(); }
sharedFactor back() const { return factors_.back(); }
double error(const HybridValues &values) const;
iterator begin() { return factors_.begin(); }
iterator end() { return factors_.end(); }
virtual void resize(size_t size) { factors_.resize(size); }
void remove(size_t i) { factors_.at(i).reset(); }
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
iterator erase(iterator item) { return factors_.erase(item); }
iterator erase(iterator first, iterator last) {
return factors_.erase(first, last);
}
void dot(std::ostream& os,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
void saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
size_t nrFactors() const;
KeySet keys() const;
KeyVector keyVector() const;
inline bool exists(size_t idx) const { return idx < size() && at(idx); }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(factors_);
}
}; // FactorGraph
} // namespace gtsam
#include <gtsam/inference/FactorGraph-inst.h>
Testable.h
Concept check for values that can be used in unit tests.
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
Key.h
DotWriter.h
Graphviz formatter.
FactorGraph-inst.h
Factor Graph Base Class.
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition FactorGraph-inst.h:37
gtsam::FactorGraph< DiscreteFactor >::isEqual
bool isEqual(const FactorGraph &other) const
Definition FactorGraph.h:134
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
gtsam::FactorGraph::empty
bool empty() const
Check if the graph is empty (null factors set by remove() will cause this to return false).
Definition FactorGraph.h:330
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
gtsam::FactorGraph::erase
iterator erase(iterator item)
Erase factor and rearrange other factors to take up the empty space.
Definition FactorGraph.h:391
gtsam::FactorGraph::add
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:269
gtsam::FactorGraph::erase
iterator erase(iterator first, iterator last)
Erase factors and rearrange other factors to take up the empty space.
Definition FactorGraph.h:394
gtsam::FactorGraph::back
sharedFactor back() const
Get the last factor.
Definition FactorGraph.h:362
gtsam::FactorGraph::FactorGraph
FactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects).
Definition FactorGraph.h:152
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
gtsam::FactorGraph::push_back
HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
Push back non-pointer objects in a container (factors are copied).
Definition FactorGraph.h:260
gtsam::FactorGraph::FactorGraph
FactorGraph()
Default constructor.
Definition FactorGraph.h:142
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition FactorGraph.h:192
gtsam::FactorGraph::operator[]
sharedFactor & operator[](size_t i)
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:350
gtsam::FactorGraph::at
sharedFactor & at(size_t i)
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:340
gtsam::FactorGraph::push_back
std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Push back a BayesTree as a collection of factors.
Definition FactorGraph.h:296
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
gtsam::FactorGraph::FactorGraph
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Constructor from iterator over factors (shared_ptr or plain objects).
Definition FactorGraph.h:146
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
gtsam::FactorGraph< DiscreteFactor >::size
size_t size() const
Definition FactorGraph.h:326
gtsam::FactorGraph::push_back
HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator (factors are copied).
Definition FactorGraph.h:240
gtsam::FactorGraph::front
sharedFactor front() const
Get the first factor.
Definition FactorGraph.h:359
gtsam::FactorGraph::end
const_iterator end() const
Iterator to end of factors.
Definition FactorGraph.h:356
gtsam::FactorGraph::push_back
HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container)
Push back many factors as shared_ptr's in a container (factors are not copied).
Definition FactorGraph.h:254
gtsam::FactorGraph< DiscreteFactor >::FactorType
DiscreteFactor FactorType
Definition FactorGraph.h:99
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
replace a factor by index
Definition FactorGraph.h:388
gtsam::FactorGraph::operator+=
boost::assign::list_inserter< CRefCallPushBack< This > > operator+=(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:278
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition FactorGraph-inst.h:50
gtsam::FactorGraph::operator+=
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type operator+=(boost::shared_ptr< DERIVEDFACTOR > factor)
+= works well with boost::assign list inserter.
Definition FactorGraph.h:219
gtsam::FactorGraph::begin
iterator begin()
non-const STL-style begin()
Definition FactorGraph.h:372
gtsam::FactorGraph::begin
const_iterator begin() const
Iterator to beginning of factors.
Definition FactorGraph.h:353
gtsam::FactorGraph::push_back
HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator over shared_ptr (factors are not copied).
Definition FactorGraph.h:233
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
gtsam::FactorGraph< DiscreteFactor >::sharedFactor
boost::shared_ptr< DiscreteFactor > sharedFactor
Definition FactorGraph.h:101
gtsam::FactorGraph::exists
bool exists(size_t idx) const
MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer...
Definition FactorGraph.h:434
gtsam::FactorGraph::operator[]
const sharedFactor operator[](size_t i) const
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:345
gtsam::FactorGraph::dot
std::string dot(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format string.
Definition FactorGraph-inst.h:168
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor)
Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
Definition FactorGraph.h:203
gtsam::FactorGraph::FactorGraph
FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition FactorGraph.h:171
gtsam::FactorGraph::end
iterator end()
non-const STL-style end()
Definition FactorGraph.h:375
gtsam::FactorGraph< DiscreteFactor >::factors_
FastVector< sharedFactor > factors_
Definition FactorGraph.h:131
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
gtsam::FactorGraph::~FactorGraph
virtual ~FactorGraph()=default
Default destructor Public and virtual so boost serialization can call it.
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition FactorGraph.h:182
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
gtsam::CRefCallPushBack
Helper.
Definition FactorGraph.h:54
gtsam::RefCallPushBack
Helper.
Definition FactorGraph.h:67