gtsam  4.1.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 
25 #include <gtsam/base/FastVector.h>
26 #include <gtsam/base/Testable.h>
27 #include <gtsam/inference/Key.h>
28 
29 #include <Eigen/Core> // for Eigen::aligned_allocator
30 
31 #include <boost/assign/list_inserter.hpp>
32 #include <boost/bind.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 
41 namespace gtsam {
43 typedef FastVector<FactorIndex> FactorIndices;
44 
45 // Forward declarations
46 template <class CLIQUE>
47 class BayesTree;
48 
50 template <class C>
52  C& obj;
53 
54  public:
55  explicit CRefCallPushBack(C& obj) : obj(obj) {}
56  template <typename A>
57  void operator()(const A& a) {
58  obj.push_back(a);
59  }
60 };
61 
63 template <class C>
65  C& obj;
66 
67  public:
68  explicit RefCallPushBack(C& obj) : obj(obj) {}
69  template <typename A>
70  void operator()(A& a) {
71  obj.push_back(a);
72  }
73 };
74 
76 template <class C>
78  C& obj;
79 
80  public:
81  explicit CRefCallAddCopy(C& obj) : obj(obj) {}
82  template <typename A>
83  void operator()(const A& a) {
84  obj.addCopy(a);
85  }
86 };
87 
93 template <class FACTOR>
94 class FactorGraph {
95  public:
96  typedef FACTOR FactorType;
97  typedef boost::shared_ptr<FACTOR>
99  typedef sharedFactor value_type;
100  typedef typename FastVector<sharedFactor>::iterator iterator;
101  typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
102 
103  private:
104  typedef FactorGraph<FACTOR> This;
105  typedef boost::shared_ptr<This>
106  shared_ptr;
107 
109  template <typename DERIVEDFACTOR>
110  using IsDerived = typename std::enable_if<
111  std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
112 
114  template <typename T>
115  using HasDerivedValueType = typename std::enable_if<
116  std::is_base_of<FactorType, typename T::value_type>::value>::type;
117 
119  template <typename T>
120  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
121  FactorType, typename T::value_type::element_type>::value>::type;
122 
123  protected:
125  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
126 
127 
128  FastVector<sharedFactor> factors_;
129 
132 
135 
137  template <typename ITERATOR>
138  FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
139  push_back(firstFactor, lastFactor);
140  }
141 
143  template <class CONTAINER>
144  explicit FactorGraph(const CONTAINER& factors) {
145  push_back(factors);
146  }
147 
149 
150  public:
153 
158  void reserve(size_t size) { factors_.reserve(size); }
159 
161  template <class DERIVEDFACTOR>
162  IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
163  factors_.push_back(boost::shared_ptr<FACTOR>(factor));
164  }
165 
167  template <class DERIVEDFACTOR, class... Args>
168  IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
169  factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
170  Eigen::aligned_allocator<DERIVEDFACTOR>(),
171  std::forward<Args>(args)...));
172  }
173 
178  template <class DERIVEDFACTOR>
179  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
180  factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
181  Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
182  }
183 
185  template <class DERIVEDFACTOR>
186  IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
187  push_back(factor);
188  }
189 
191  template <class DERIVEDFACTOR>
192  typename std::enable_if<
193  std::is_base_of<FactorType, DERIVEDFACTOR>::value,
194  boost::assign::list_inserter<RefCallPushBack<This>>>::type
195  operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
196  return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
197  factor);
198  }
199 
203 
208  template <typename ITERATOR>
209  HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
210  ITERATOR lastFactor) {
211  factors_.insert(end(), firstFactor, lastFactor);
212  }
213 
215  template <typename ITERATOR>
216  HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
217  ITERATOR lastFactor) {
218  for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
219  }
220 
224 
229  template <typename CONTAINER>
230  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
231  push_back(container.begin(), container.end());
232  }
233 
235  template <typename CONTAINER>
236  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
237  push_back(container.begin(), container.end());
238  }
239 
244  template <class FACTOR_OR_CONTAINER>
245  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
246  push_back(factorOrContainer);
247  }
248 
253  template <class FACTOR_OR_CONTAINER>
254  boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
255  const FACTOR_OR_CONTAINER& factorOrContainer) {
256  return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
257  factorOrContainer);
258  }
259 
263 
269  template <class CLIQUE>
270  typename std::enable_if<
271  std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
272  push_back(const BayesTree<CLIQUE>& bayesTree) {
273  bayesTree.addFactorsToGraph(this);
274  }
275 
280  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
281  FactorIndices add_factors(const CONTAINER& factors,
282  bool useEmptySlots = false);
283 
287 
289  void print(const std::string& s = "FactorGraph",
290  const KeyFormatter& formatter = DefaultKeyFormatter) const;
291 
293  bool equals(const This& fg, double tol = 1e-9) const;
295 
296  public:
299 
302  size_t size() const { return factors_.size(); }
303 
306  bool empty() const { return factors_.empty(); }
307 
311  const sharedFactor at(size_t i) const { return factors_.at(i); }
312 
316  sharedFactor& at(size_t i) { return factors_.at(i); }
317 
321  const sharedFactor operator[](size_t i) const { return at(i); }
322 
326  sharedFactor& operator[](size_t i) { return at(i); }
327 
329  const_iterator begin() const { return factors_.begin(); }
330 
332  const_iterator end() const { return factors_.end(); }
333 
335  sharedFactor front() const { return factors_.front(); }
336 
338  sharedFactor back() const { return factors_.back(); }
339 
343 
345  iterator begin() { return factors_.begin(); }
346 
348  iterator end() { return factors_.end(); }
349 
354  void resize(size_t size) { factors_.resize(size); }
355 
358  void remove(size_t i) { factors_[i].reset(); }
359 
361  void replace(size_t index, sharedFactor factor) { at(index) = factor; }
362 
364  iterator erase(iterator item) { return factors_.erase(item); }
365 
367  iterator erase(iterator first, iterator last) {
368  return factors_.erase(first, last);
369  }
370 
374 
376  size_t nrFactors() const;
377 
380  KeySet keys() const;
381 
385  KeyVector keyVector() const;
386 
389  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
390 
391  private:
393  friend class boost::serialization::access;
394  template <class ARCHIVE>
395  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
396  ar& BOOST_SERIALIZATION_NVP(factors_);
397  }
398 
400 }; // FactorGraph
401 } // namespace gtsam
402 
403 #include <gtsam/inference/FactorGraph-inst.h>
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
Testable.h
Concept check for values that can be used in unit tests.
gtsam::FactorGraph::end
iterator end()
non-const STL-style end()
Definition: FactorGraph.h:348
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:230
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition: FactorGraph.h:128
gtsam::BayesTree
Definition: BayesTree.h:65
gtsam::CRefCallPushBack
Helper.
Definition: FactorGraph.h:51
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:311
gtsam::FactorGraph::back
sharedFactor back() const
Get the last factor.
Definition: FactorGraph.h:338
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:162
gtsam::FactorGraph::erase
iterator erase(iterator first, iterator last)
Erase factors and rearrange other factors to take up the empty space.
Definition: FactorGraph.h:367
gtsam::FactorGraph::push_back
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Push back a BayesTree as a collection of factors.
Definition: FactorGraph.h:272
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:209
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:302
gtsam::CRefCallAddCopy
Helper.
Definition: FactorGraph.h:77
gtsam::FactorGraph::begin
const_iterator begin() const
Iterator to beginning of factors.
Definition: FactorGraph.h:329
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:254
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:51
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:216
gtsam::FactorGraph::end
const_iterator end() const
Iterator to end of factors.
Definition: FactorGraph.h:332
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition: BayesTree-inst.h:151
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:195
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:168
gtsam::FactorGraph::begin
iterator begin()
non-const STL-style begin()
Definition: FactorGraph.h:345
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:306
gtsam::FactorGraph::front
sharedFactor front() const
Get the first factor.
Definition: FactorGraph.h:335
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:321
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
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:389
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:98
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
replace a factor by index
Definition: FactorGraph.h:361
Key.h
gtsam::FactorGraph::FactorType
FACTOR FactorType
factor type
Definition: FactorGraph.h:96
gtsam::FactorGraph::erase
iterator erase(iterator item)
Erase factor and rearrange other factors to take up the empty space.
Definition: FactorGraph.h:364
gtsam::FastSet< Key >
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:100
gtsam::FactorGraph::print
void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:38
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:316
gtsam::KeyFormatter
boost::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::FactorGraph::add
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition: FactorGraph.h:245
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:236
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:186
gtsam::FactorGraph::resize
void resize(size_t size)
Directly resize the number of factors in the graph.
Definition: FactorGraph.h:354
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:326
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:76
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:179
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition: FactorGraph-inst.h:86
gtsam::FactorGraph::FactorGraph
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Constructor from iterator over factors (shared_ptr or plain objects)
Definition: FactorGraph.h:138
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition: FactorGraph-inst.h:67
gtsam::FactorGraph::FactorGraph
FactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: FactorGraph.h:144
gtsam::RefCallPushBack
Helper.
Definition: FactorGraph.h:64
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:158
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition: FactorGraph.h:358