gtsam 4.2.0
gtsam
FactorGraph.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
21// \callgraph
22
23#pragma once
24
26#include <gtsam/inference/Key.h>
28#include <gtsam/base/Testable.h>
29
30#include <Eigen/Core> // for Eigen::aligned_allocator
31
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>
36
37#include <string>
38#include <type_traits>
39#include <utility>
40#include <iosfwd>
41
42namespace gtsam {
44typedef FastVector<FactorIndex> FactorIndices;
45
46// Forward declarations
47template <class CLIQUE>
48class BayesTree;
49
50class HybridValues;
51
53template <class C>
55 C& obj;
56
57 public:
58 explicit CRefCallPushBack(C& obj) : obj(obj) {}
59 template <typename A>
60 void operator()(const A& a) {
61 obj.push_back(a);
62 }
63};
64
66template <class C>
68 C& obj;
69
70 public:
71 explicit RefCallPushBack(C& obj) : obj(obj) {}
72 template <typename A>
73 void operator()(A& a) {
74 obj.push_back(a);
75 }
76};
77
79template <class C>
81 C& obj;
82
83 public:
84 explicit CRefCallAddCopy(C& obj) : obj(obj) {}
85 template <typename A>
86 void operator()(const A& a) {
87 obj.addCopy(a);
88 }
89};
90
96template <class FACTOR>
98 public:
99 typedef FACTOR FactorType;
100 typedef boost::shared_ptr<FACTOR>
102 typedef sharedFactor value_type;
103 typedef typename FastVector<sharedFactor>::iterator iterator;
104 typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
105
106 private:
107 typedef FactorGraph<FACTOR> This;
108 typedef boost::shared_ptr<This>
109 shared_ptr;
110
112 template <typename DERIVEDFACTOR>
113 using IsDerived = typename std::enable_if<
114 std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
115
117 template <typename T>
118 using HasDerivedValueType = typename std::enable_if<
119 std::is_base_of<FactorType, typename T::value_type>::value>::type;
120
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;
125
126 protected:
128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
129
130
132
134 bool isEqual(const FactorGraph& other) const {
135 return factors_ == other.factors_;
136 }
137
140
143
145 template <typename ITERATOR>
146 FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
147 push_back(firstFactor, lastFactor);
148 }
149
151 template <class CONTAINER>
152 explicit FactorGraph(const CONTAINER& factors) {
153 push_back(factors);
154 }
155
157
158 public:
161
164 virtual ~FactorGraph() = default;
165
170 template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
171 FactorGraph(std::initializer_list<boost::shared_ptr<DERIVEDFACTOR>> sharedFactors)
172 : factors_(sharedFactors) {}
173
177
182 void reserve(size_t size) { factors_.reserve(size); }
183
185 template <class DERIVEDFACTOR>
186 IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
187 factors_.push_back(boost::shared_ptr<FACTOR>(factor));
188 }
189
191 template <class DERIVEDFACTOR, class... Args>
192 IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
193 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
194 Eigen::aligned_allocator<DERIVEDFACTOR>(),
195 std::forward<Args>(args)...));
196 }
197
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));
206 }
207
209 template <class DERIVEDFACTOR>
210 IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
211 push_back(factor);
212 }
213
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
219 operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
220 return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
221 factor);
222 }
223
227
232 template <typename ITERATOR>
233 HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
234 ITERATOR lastFactor) {
235 factors_.insert(end(), firstFactor, lastFactor);
236 }
237
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);
243 }
244
248
253 template <typename CONTAINER>
254 HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
255 push_back(container.begin(), container.end());
256 }
257
259 template <typename CONTAINER>
260 HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
261 push_back(container.begin(), container.end());
262 }
263
268 template <class FACTOR_OR_CONTAINER>
269 void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
270 push_back(factorOrContainer);
271 }
272
277 template <class FACTOR_OR_CONTAINER>
278 boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
279 const FACTOR_OR_CONTAINER& factorOrContainer) {
280 return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
281 factorOrContainer);
282 }
283
287
293 template <class CLIQUE>
294 typename std::enable_if<
295 std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
296 push_back(const BayesTree<CLIQUE>& bayesTree) {
297 bayesTree.addFactorsToGraph(this);
298 }
299
304 template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
305 FactorIndices add_factors(const CONTAINER& factors,
306 bool useEmptySlots = false);
307
311
313 virtual void print(const std::string& s = "FactorGraph",
314 const KeyFormatter& formatter = DefaultKeyFormatter) const;
315
317 bool equals(const This& fg, double tol = 1e-9) const;
319
320 public:
323
326 size_t size() const { return factors_.size(); }
327
330 bool empty() const { return factors_.empty(); }
331
335 const sharedFactor at(size_t i) const { return factors_.at(i); }
336
340 sharedFactor& at(size_t i) { return factors_.at(i); }
341
345 const sharedFactor operator[](size_t i) const { return at(i); }
346
350 sharedFactor& operator[](size_t i) { return at(i); }
351
353 const_iterator begin() const { return factors_.begin(); }
354
356 const_iterator end() const { return factors_.end(); }
357
359 sharedFactor front() const { return factors_.front(); }
360
362 sharedFactor back() const { return factors_.back(); }
363
365 double error(const HybridValues &values) const;
366
370
372 iterator begin() { return factors_.begin(); }
373
375 iterator end() { return factors_.end(); }
376
381 virtual void resize(size_t size) { factors_.resize(size); }
382
385 void remove(size_t i) { factors_.at(i).reset(); }
386
388 void replace(size_t index, sharedFactor factor) { at(index) = factor; }
389
391 iterator erase(iterator item) { return factors_.erase(item); }
392
394 iterator erase(iterator first, iterator last) {
395 return factors_.erase(first, last);
396 }
397
401
403 void dot(std::ostream& os,
404 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
405 const DotWriter& writer = DotWriter()) const;
406
408 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
409 const DotWriter& writer = DotWriter()) const;
410
412 void saveGraph(const std::string& filename,
413 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
414 const DotWriter& writer = DotWriter()) const;
415
419
421 size_t nrFactors() const;
422
425 KeySet keys() const;
426
430 KeyVector keyVector() const;
431
434 inline bool exists(size_t idx) const { return idx < size() && at(idx); }
435
436 private:
439 template <class ARCHIVE>
440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
441 ar& BOOST_SERIALIZATION_NVP(factors_);
442 }
443
445}; // FactorGraph
446} // namespace gtsam
447
A thin wrapper around std::vector that uses a custom allocator.
Concept check for values that can be used in unit tests.
Graphviz formatter.
Factor Graph Base Class.
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
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
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition: FactorGraph-inst.h:50
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
the error.