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>
27 #include <gtsam/base/FastVector.h>
28 #include <gtsam/base/Testable.h>
29 
30 #include <Eigen/Core> // for Eigen::aligned_allocator
31 
32 #ifdef GTSAM_USE_BOOST_FEATURES
33 #include <boost/assign/list_inserter.hpp>
34 #endif
35 
36 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
37 #include <boost/serialization/nvp.hpp>
38 #include <boost/serialization/vector.hpp>
39 #endif
40 
41 #include <string>
42 #include <type_traits>
43 #include <utility>
44 #include <iosfwd>
45 
46 namespace gtsam {
48 typedef FastVector<FactorIndex> FactorIndices;
49 
50 // Forward declarations
51 template <class CLIQUE>
52 class BayesTree;
53 
54 class HybridValues;
55 
57 template <class C>
59  C& obj;
60 
61  public:
62  explicit CRefCallPushBack(C& obj) : obj(obj) {}
63  template <typename A>
64  void operator()(const A& a) {
65  obj.push_back(a);
66  }
67 };
68 
70 template <class C>
72  C& obj;
73 
74  public:
75  explicit RefCallPushBack(C& obj) : obj(obj) {}
76  template <typename A>
77  void operator()(A& a) {
78  obj.push_back(a);
79  }
80 };
81 
83 template <class C>
85  C& obj;
86 
87  public:
88  explicit CRefCallAddCopy(C& obj) : obj(obj) {}
89  template <typename A>
90  void operator()(const A& a) {
91  obj.addCopy(a);
92  }
93 };
94 
100 template <class FACTOR>
101 class FactorGraph {
102  public:
103  typedef FACTOR FactorType;
104  typedef std::shared_ptr<FACTOR>
109 
110  private:
112  typedef std::shared_ptr<This>
114 
116  template <typename DERIVEDFACTOR>
117  using IsDerived = typename std::enable_if<
119 
121  template <typename T>
122  using HasDerivedValueType = typename std::enable_if<
124 
126  template <typename T>
127  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
128  FactorType, typename T::value_type::element_type>::value>::type;
129 
130  protected:
133 
134 
136 
138  bool isEqual(const FactorGraph& other) const {
139  return factors_ == other.factors_;
140  }
141 
144 
147 
149  template <typename ITERATOR>
150  FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
151  push_back(firstFactor, lastFactor);
152  }
153 
155  template <class CONTAINER>
156  explicit FactorGraph(const CONTAINER& factors) {
157  push_back(factors);
158  }
159 
161 
162  public:
165 
168  virtual ~FactorGraph() = default;
169 
174  template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
175  FactorGraph(std::initializer_list<std::shared_ptr<DERIVEDFACTOR>> sharedFactors)
176  : factors_(sharedFactors) {}
177 
181 
186  void reserve(size_t size) { factors_.reserve(size); }
187 
189  template <class DERIVEDFACTOR>
190  IsDerived<DERIVEDFACTOR> push_back(std::shared_ptr<DERIVEDFACTOR> factor) {
191  factors_.push_back(std::shared_ptr<FACTOR>(factor));
192  }
193 
195  template <class DERIVEDFACTOR, class... Args>
197  factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
199  std::forward<Args>(args)...));
200  }
201 
206  template <class DERIVEDFACTOR>
207  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
208  factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
210  }
211 
213  template <class DERIVEDFACTOR>
214  IsDerived<DERIVEDFACTOR> add(std::shared_ptr<DERIVEDFACTOR> factor) {
215  push_back(factor);
216  }
217 
218 #ifdef GTSAM_USE_BOOST_FEATURES
219  template <class DERIVEDFACTOR>
221  typename std::enable_if<
223  boost::assign::list_inserter<RefCallPushBack<This>>>::type
224  operator+=(std::shared_ptr<DERIVEDFACTOR> factor) {
225  return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
226  factor);
227  }
228 #endif
229 
233 
238  template <typename ITERATOR>
240  ITERATOR lastFactor) {
241  factors_.insert(end(), firstFactor, lastFactor);
242  }
243 
245  template <typename ITERATOR>
247  ITERATOR lastFactor) {
248  for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
249  }
250 
254 
259  template <typename CONTAINER>
260  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
261  push_back(container.begin(), container.end());
262  }
263 
265  template <typename CONTAINER>
266  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
267  push_back(container.begin(), container.end());
268  }
269 
274  template <class FACTOR_OR_CONTAINER>
275  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
276  push_back(factorOrContainer);
277  }
278 
279 #ifdef GTSAM_USE_BOOST_FEATURES
280 
284  template <class FACTOR_OR_CONTAINER>
285  boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
286  const FACTOR_OR_CONTAINER& factorOrContainer) {
287  return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
288  factorOrContainer);
289  }
290 #endif
291 
295 
301  template <class CLIQUE>
302  typename std::enable_if<
304  push_back(const BayesTree<CLIQUE>& bayesTree) {
305  bayesTree.addFactorsToGraph(this);
306  }
307 
312  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
313  FactorIndices add_factors(const CONTAINER& factors,
314  bool useEmptySlots = false);
315 
319 
321  virtual void print(const std::string& s = "FactorGraph",
323 
325  bool equals(const This& fg, double tol = 1e-9) const;
327 
328  public:
331 
334  size_t size() const { return factors_.size(); }
335 
338  bool empty() const { return factors_.empty(); }
339 
343  const sharedFactor at(size_t i) const { return factors_.at(i); }
344 
348  sharedFactor& at(size_t i) { return factors_.at(i); }
349 
353  const sharedFactor operator[](size_t i) const { return at(i); }
354 
358  sharedFactor& operator[](size_t i) { return at(i); }
359 
361  const_iterator begin() const { return factors_.begin(); }
362 
364  const_iterator end() const { return factors_.end(); }
365 
367  sharedFactor front() const { return factors_.front(); }
368 
370  sharedFactor back() const { return factors_.back(); }
371 
373  double error(const HybridValues &values) const;
374 
378 
380  iterator begin() { return factors_.begin(); }
381 
383  iterator end() { return factors_.end(); }
384 
389  virtual void resize(size_t size) { factors_.resize(size); }
390 
393  void remove(size_t i) { factors_.at(i).reset(); }
394 
396  void replace(size_t index, sharedFactor factor) { at(index) = factor; }
397 
399  iterator erase(iterator item) { return factors_.erase(item); }
400 
402  iterator erase(iterator first, iterator last) {
403  return factors_.erase(first, last);
404  }
405 
409 
411  void dot(std::ostream& os,
412  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
413  const DotWriter& writer = DotWriter()) const;
414 
416  std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
417  const DotWriter& writer = DotWriter()) const;
418 
420  void saveGraph(const std::string& filename,
421  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
422  const DotWriter& writer = DotWriter()) const;
423 
427 
429  size_t nrFactors() const;
430 
433  KeySet keys() const;
434 
438  KeyVector keyVector() const;
439 
442  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
443 
444  private:
445 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
446 
447  friend class boost::serialization::access;
448  template <class ARCHIVE>
449  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
450  ar& BOOST_SERIALIZATION_NVP(factors_);
451  }
452 #endif
453 
455 }; // FactorGraph
456 } // namespace gtsam
457 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
sharedFactor back() const
Definition: FactorGraph.h:370
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Definition: FactorGraph.h:275
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 & operator+=(bfloat16 &a, const bfloat16 &b)
Definition: BFloat16.h:184
void replace(size_t index, sharedFactor factor)
Definition: FactorGraph.h:396
Graphviz formatter.
Concept check for values that can be used in unit tests.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
std::string serialize(const T &input)
serializes to a string
void operator()(const A &a)
Definition: FactorGraph.h:64
HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Definition: FactorGraph.h:239
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Definition: FactorGraph.h:150
sharedFactor front() const
Definition: FactorGraph.h:367
const sharedFactor operator[](size_t i) const
Definition: FactorGraph.h:353
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
Definition: pytypes.h:2012
HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator (factors are copied)
Definition: FactorGraph.h:246
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
Push back non-pointer objects in a container (factors are copied).
Definition: FactorGraph.h:266
IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor)
Definition: FactorGraph.h:207
leaf::MyValues values
const GaussianFactorGraph factors
typename std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value >::type IsDerived
Check if a DERIVEDFACTOR is in fact derived from FactorType.
Definition: FactorGraph.h:118
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
FastVector< sharedFactor >::iterator iterator
Definition: FactorGraph.h:107
bool exists(size_t idx) const
Definition: FactorGraph.h:442
FACTOR FactorType
factor type
Definition: FactorGraph.h:103
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
const KeyFormatter & formatter
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
size_t size() const
Definition: FactorGraph.h:334
const_iterator begin() const
Definition: FactorGraph.h:361
sharedFactor value_type
Definition: FactorGraph.h:106
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
sharedFactor & at(size_t i)
Definition: FactorGraph.h:348
FactorGraph(const CONTAINER &factors)
Definition: FactorGraph.h:156
bool empty() const
Definition: FactorGraph.h:338
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
iterator begin()
Definition: FactorGraph.h:380
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
FactorGraph(std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)
Definition: FactorGraph.h:175
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
void operator()(const A &a)
Definition: FactorGraph.h:90
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
sharedFactor & operator[](size_t i)
Definition: FactorGraph.h:358
const_iterator end() const
Definition: FactorGraph.h:364
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
DotWriter is a helper class for writing graphviz .dot files.
Definition: DotWriter.h:36
A thin wrapper around std::vector that uses a custom allocator.
traits
Definition: chartTesting.h:28
typename std::enable_if< std::is_base_of< FactorType, typename T::value_type >::value >::type HasDerivedValueType
Check if T has a value_type derived from FactorType.
Definition: FactorGraph.h:123
virtual void resize(size_t size)
Definition: FactorGraph.h:389
FastVector< sharedFactor >::const_iterator const_iterator
Definition: FactorGraph.h:108
ofstream os("timeSchurFactors.csv")
iterator erase(iterator first, iterator last)
Definition: FactorGraph.h:402
HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container)
Definition: FactorGraph.h:260
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Definition: FactorGraph.h:304
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
FactorGraph< FACTOR > This
Typedef for this class.
Definition: FactorGraph.h:111
static EIGEN_DEPRECATED const end_t end
std::shared_ptr< This > shared_ptr
Shared pointer for this class.
Definition: FactorGraph.h:113
static double error
Definition: testRot3.cpp:37
void reserve(size_t size)
Definition: FactorGraph.h:186
typename std::enable_if< std::is_base_of< FactorType, typename T::value_type::element_type >::value >::type HasDerivedElementType
Check if T has a pointer type derived from FactorType.
Definition: FactorGraph.h:128
const G double tol
Definition: Group.h:86
std::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:105
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Factor Graph Base Class.
iterator erase(iterator item)
Definition: FactorGraph.h:399


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:13