32#include <boost/assign/list_inserter.hpp>
33#include <boost/make_shared.hpp>
34#include <boost/serialization/nvp.hpp>
35#include <boost/serialization/vector.hpp>
47template <
class CLIQUE>
60 void operator()(
const A& a) {
73 void operator()(A& a) {
86 void operator()(
const A& a) {
96template <
class FACTOR>
100 typedef boost::shared_ptr<FACTOR>
108 typedef boost::shared_ptr<This>
112 template <
typename DERIVEDFACTOR>
113 using IsDerived =
typename std::enable_if<
114 std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
117 template <
typename T>
118 using HasDerivedValueType =
typename std::enable_if<
119 std::is_base_of<FactorType, typename T::value_type>::value>::type;
122 template <
typename T>
123 using HasDerivedElementType =
typename std::enable_if<std::is_base_of<
124 FactorType,
typename T::value_type::element_type>::value>::type;
128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
145 template <
typename ITERATOR>
151 template <
class CONTAINER>
170 template <
class DERIVEDFACTOR,
typename = IsDerived<DERIVEDFACTOR>>
171 FactorGraph(std::initializer_list<boost::shared_ptr<DERIVEDFACTOR>> sharedFactors)
185 template <
class DERIVEDFACTOR>
186 IsDerived<DERIVEDFACTOR>
push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
187 factors_.push_back(boost::shared_ptr<FACTOR>(factor));
191 template <
class DERIVEDFACTOR,
class... Args>
193 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
194 Eigen::aligned_allocator<DERIVEDFACTOR>(),
195 std::forward<Args>(args)...));
202 template <
class DERIVEDFACTOR>
203 IsDerived<DERIVEDFACTOR>
push_back(
const DERIVEDFACTOR& factor) {
204 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
205 Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
209 template <
class DERIVEDFACTOR>
210 IsDerived<DERIVEDFACTOR>
add(boost::shared_ptr<DERIVEDFACTOR> factor) {
215 template <
class DERIVEDFACTOR>
216 typename std::enable_if<
217 std::is_base_of<FactorType, DERIVEDFACTOR>::value,
218 boost::assign::list_inserter<RefCallPushBack<This>>>::type
232 template <
typename ITERATOR>
233 HasDerivedElementType<ITERATOR>
push_back(ITERATOR firstFactor,
234 ITERATOR lastFactor) {
239 template <
typename ITERATOR>
240 HasDerivedValueType<ITERATOR>
push_back(ITERATOR firstFactor,
241 ITERATOR lastFactor) {
242 for (ITERATOR f = firstFactor; f != lastFactor; ++f)
push_back(*f);
253 template <
typename CONTAINER>
254 HasDerivedElementType<CONTAINER>
push_back(
const CONTAINER& container) {
255 push_back(container.begin(), container.end());
259 template <
typename CONTAINER>
260 HasDerivedValueType<CONTAINER>
push_back(
const CONTAINER& container) {
261 push_back(container.begin(), container.end());
268 template <
class FACTOR_OR_CONTAINER>
269 void add(
const FACTOR_OR_CONTAINER& factorOrContainer) {
277 template <
class FACTOR_OR_CONTAINER>
278 boost::assign::list_inserter<CRefCallPushBack<This>>
operator+=(
279 const FACTOR_OR_CONTAINER& factorOrContainer) {
293 template <
class CLIQUE>
294 typename std::enable_if<
295 std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
304 template <
typename CONTAINER,
typename = HasDerivedElementType<CONTAINER>>
306 bool useEmptySlots =
false);
313 virtual void print(
const std::string& s =
"FactorGraph",
314 const KeyFormatter& formatter = DefaultKeyFormatter)
const;
317 bool equals(
const This& fg,
double tol = 1e-9)
const;
394 iterator
erase(iterator first, iterator last) {
403 void dot(std::ostream& os,
408 std::string
dot(
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
412 void saveGraph(
const std::string& filename,
434 inline bool exists(
size_t idx)
const {
return idx <
size() &&
at(idx); }
439 template <
class ARCHIVE>
440 void serialize(ARCHIVE& ar,
const unsigned int ) {
441 ar& BOOST_SERIALIZATION_NVP(
factors_);
Concept check for values that can be used in unit tests.
A thin wrapper around std::vector that uses a custom allocator.
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:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
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
Template to create a binary predicate.
Definition Testable.h:111
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
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
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
bool empty() const
Check if the graph is empty (null factors set by remove() will cause this to return false).
Definition FactorGraph.h:330
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
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
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
iterator erase(iterator item)
Erase factor and rearrange other factors to take up the empty space.
Definition FactorGraph.h:391
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:269
iterator erase(iterator first, iterator last)
Erase factors and rearrange other factors to take up the empty space.
Definition FactorGraph.h:394
sharedFactor back() const
Get the last factor.
Definition FactorGraph.h:362
FactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition FactorGraph.h:152
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
Push back non-pointer objects in a container (factors are copied).
Definition FactorGraph.h:260
FactorGraph()
Default constructor.
Definition FactorGraph.h:142
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition FactorGraph.h:192
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
sharedFactor & at(size_t i)
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:340
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
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Constructor from iterator over factors (shared_ptr or plain objects)
Definition FactorGraph.h:146
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator (factors are copied)
Definition FactorGraph.h:240
sharedFactor front() const
Get the first factor.
Definition FactorGraph.h:359
const_iterator end() const
Iterator to end of factors.
Definition FactorGraph.h:356
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
FACTOR FactorType
factor type
Definition FactorGraph.h:99
void replace(size_t index, sharedFactor factor)
replace a factor by index
Definition FactorGraph.h:388
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
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
iterator begin()
non-const STL-style begin()
Definition FactorGraph.h:372
const_iterator begin() const
Iterator to beginning of factors.
Definition FactorGraph.h:353
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
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
friend class boost::serialization::access
Serialization function.
Definition FactorGraph.h:438
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
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
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
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
FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition FactorGraph.h:171
iterator end()
non-const STL-style end()
Definition FactorGraph.h:375
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
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
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
virtual ~FactorGraph()=default
Default destructor Public and virtual so boost serialization can call it.
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
Bayes tree.
Definition BayesTree.h:67
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
Helper.
Definition FactorGraph.h:54
Helper.
Definition FactorGraph.h:67
Helper.
Definition FactorGraph.h:80