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_ENABLE_BOOST_SERIALIZATION
33 #include <boost/serialization/nvp.hpp>
34 #include <boost/serialization/vector.hpp>
35 #endif
36 
37 #include <string>
38 #include <type_traits>
39 #include <utility>
40 #include <iosfwd>
41 
42 namespace gtsam {
44 typedef FastVector<FactorIndex> FactorIndices;
45 
46 // Forward declarations
47 template <class CLIQUE>
48 class BayesTree;
49 
50 class HybridValues;
51 
57 template <class FACTOR>
58 class FactorGraph {
59  public:
60  typedef FACTOR FactorType;
61  typedef std::shared_ptr<FACTOR>
66 
67  private:
69  typedef std::shared_ptr<This>
71 
73  template <typename DERIVEDFACTOR>
74  using IsDerived = typename std::enable_if<
76 
78  template <typename T>
79  using HasDerivedValueType = typename std::enable_if<
81 
83  template <typename T>
84  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
85  FactorType, typename T::value_type::element_type>::value>::type;
86 
87  protected:
90 
91 
93 
95  bool isEqual(const FactorGraph& other) const {
96  return factors_ == other.factors_;
97  }
98 
101 
104 
106  template <typename ITERATOR>
107  FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
108  push_back(firstFactor, lastFactor);
109  }
110 
112  template <class CONTAINER>
113  explicit FactorGraph(const CONTAINER& factors) {
115  }
116 
118 
119  public:
122 
125  virtual ~FactorGraph() = default;
126 
131  template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
132  FactorGraph(std::initializer_list<std::shared_ptr<DERIVEDFACTOR>> sharedFactors)
133  : factors_(sharedFactors) {}
134 
138 
143  void reserve(size_t size) { factors_.reserve(size); }
144 
146  template <class DERIVEDFACTOR>
147  IsDerived<DERIVEDFACTOR> push_back(std::shared_ptr<DERIVEDFACTOR> factor) {
148  factors_.push_back(std::shared_ptr<FACTOR>(factor));
149  }
150 
152  template <class DERIVEDFACTOR, class... Args>
154  factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
156  std::forward<Args>(args)...));
157  }
158 
163  template <class DERIVEDFACTOR>
164  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
165  factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
167  }
168 
170  template <class DERIVEDFACTOR>
171  IsDerived<DERIVEDFACTOR> add(std::shared_ptr<DERIVEDFACTOR> factor) {
172  push_back(factor);
173  }
174 
176  template <class DERIVEDFACTOR>
178  This>::type&
179  operator+=(std::shared_ptr<DERIVEDFACTOR> factor) {
180  push_back(factor);
181  return *this;
182  }
183 
189  template <class DERIVEDFACTOR>
191  std::shared_ptr<DERIVEDFACTOR> factor) {
192  push_back(factor);
193  return *this;
194  }
195 
199 
204  template <typename ITERATOR>
206  ITERATOR lastFactor) {
207  factors_.insert(end(), firstFactor, lastFactor);
208  }
209 
211  template <typename ITERATOR>
213  ITERATOR lastFactor) {
214  for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
215  }
216 
220 
225  template <typename CONTAINER>
226  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
227  push_back(container.begin(), container.end());
228  }
229 
231  template <typename CONTAINER>
232  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
233  push_back(container.begin(), container.end());
234  }
235 
240  template <class FACTOR_OR_CONTAINER>
241  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
242  push_back(factorOrContainer);
243  }
244 
249  template <class FACTOR_OR_CONTAINER>
250  This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
251  push_back(factorOrContainer);
252  return *this;
253  }
254 
258 
264  template <class CLIQUE>
265  typename std::enable_if<
267  push_back(const BayesTree<CLIQUE>& bayesTree) {
268  bayesTree.addFactorsToGraph(this);
269  }
270 
275  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
276  FactorIndices add_factors(const CONTAINER& factors,
277  bool useEmptySlots = false);
278 
282 
284  virtual void print(const std::string& s = "FactorGraph",
286 
288  bool equals(const This& fg, double tol = 1e-9) const;
290 
291  public:
294 
297  size_t size() const { return factors_.size(); }
298 
301  bool empty() const { return factors_.empty(); }
302 
306  const sharedFactor at(size_t i) const { return factors_.at(i); }
307 
311  sharedFactor& at(size_t i) { return factors_.at(i); }
312 
317  template <typename F>
318  std::shared_ptr<F> at(size_t i) {
319  return std::dynamic_pointer_cast<F>(factors_.at(i));
320  }
321 
323  template <typename F>
324  const std::shared_ptr<F> at(size_t i) const {
325  return std::dynamic_pointer_cast<F>(factors_.at(i));
326  }
327 
331  const sharedFactor operator[](size_t i) const { return at(i); }
332 
336  sharedFactor& operator[](size_t i) { return at(i); }
337 
339  const_iterator begin() const { return factors_.begin(); }
340 
342  const_iterator end() const { return factors_.end(); }
343 
345  sharedFactor front() const { return factors_.front(); }
346 
348  sharedFactor back() const { return factors_.back(); }
349 
351  double error(const HybridValues &values) const;
352 
356 
358  iterator begin() { return factors_.begin(); }
359 
361  iterator end() { return factors_.end(); }
362 
367  virtual void resize(size_t size) { factors_.resize(size); }
368 
371  void remove(size_t i) { factors_.at(i).reset(); }
372 
374  void replace(size_t index, sharedFactor factor) { at(index) = factor; }
375 
377  iterator erase(iterator item) { return factors_.erase(item); }
378 
381  return factors_.erase(first, last);
382  }
383 
387 
389  void dot(std::ostream& os,
390  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
391  const DotWriter& writer = DotWriter()) const;
392 
394  std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
395  const DotWriter& writer = DotWriter()) const;
396 
398  void saveGraph(const std::string& filename,
399  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
400  const DotWriter& writer = DotWriter()) const;
401 
405 
407  size_t nrFactors() const;
408 
411  KeySet keys() const;
412 
416  KeyVector keyVector() const;
417 
420  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
421 
422  private:
423 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
424 
425  friend class boost::serialization::access;
426  template <class ARCHIVE>
427  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
428  ar& BOOST_SERIALIZATION_NVP(factors_);
429  }
430 #endif
431 
433 }; // FactorGraph
434 } // namespace gtsam
435 
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Definition: FactorGraph-inst.h:109
DotWriter.h
Graphviz formatter.
gtsam::FactorGraph::const_iterator
FastVector< sharedFactor >::const_iterator const_iterator
Definition: FactorGraph.h:65
gtsam::FactorGraph::FactorType
FACTOR FactorType
factor type
Definition: FactorGraph.h:60
gtsam::HybridValues
Definition: HybridValues.h:37
gtsam::FactorGraph::end
iterator end()
Definition: FactorGraph.h:361
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
Definition: FactorGraph.h:92
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Definition: FactorGraph-inst.h:66
Testable.h
Concept check for values that can be used in unit tests.
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Definition: BayesTree-inst.h:168
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor)
Definition: FactorGraph.h:164
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
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
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::FactorGraph::at
std::shared_ptr< F > at(size_t i)
Definition: FactorGraph.h:318
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Definition: FactorGraph-inst.h:95
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
type
Definition: pytypes.h:1525
gtsam::FactorGraph::operator+=
This & operator+=(const FACTOR_OR_CONTAINER &factorOrContainer)
Definition: FactorGraph.h:250
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::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition: FactorGraph.h:95
gtsam::FactorGraph< Factor >::HasDerivedValueType
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:80
gtsam::FactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
Shared pointer for this class.
Definition: FactorGraph.h:70
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::FastSet
Definition: FastSet.h:51
HybridValues
iterator
Definition: pytypes.h:1460
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:212
os
ofstream os("timeSchurFactors.csv")
Eigen::last
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
Definition: IndexedViewHelper.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::FactorGraph::FactorGraph
FactorGraph(std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)
Definition: FactorGraph.h:132
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
FactorGraph-inst.h
Factor Graph Base Class.
gtsam::FactorGraph::erase
iterator erase(iterator item)
Definition: FactorGraph.h:377
gtsam::FactorGraph
Definition: BayesTree.h:34
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
Key.h
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
gtsam::FactorGraph::front
sharedFactor front() const
Definition: FactorGraph.h:345
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
gtsam::FactorGraph< Factor >::IsDerived
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:75
gtsam::FactorGraph::push_back
HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Definition: FactorGraph.h:205
relicense.filename
filename
Definition: relicense.py:57
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::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
Definition: FactorGraph.h:374
gtsam::FactorGraph::operator+=
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type & operator+=(std::shared_ptr< DERIVEDFACTOR > factor)
Append factor to factor graph.
Definition: FactorGraph.h:179
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::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
gtsam::FactorGraph::FactorGraph
FactorGraph(const CONTAINER &factors)
Definition: FactorGraph.h:113
gtsam::FactorGraph::iterator
FastVector< sharedFactor >::iterator iterator
Definition: FactorGraph.h:64
gtsam::FactorGraph::FactorGraph
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Definition: FactorGraph.h:107
gtsam::FactorGraph::remove
void remove(size_t i)
Definition: FactorGraph.h:371
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph::back
sharedFactor back() const
Definition: FactorGraph.h:348
gtsam::FactorGraph::at
sharedFactor & at(size_t i)
Definition: FactorGraph.h:311
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::FactorGraph::erase
iterator erase(iterator first, iterator last)
Definition: FactorGraph.h:380
gtsam::FactorGraph< Factor >::HasDerivedElementType
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:85
gtsam::FactorGraph::empty
bool empty() const
Definition: FactorGraph.h:301
gtsam
traits
Definition: SFMdata.h:40
gtsam::BayesTree
Definition: BayesTree.h:66
gtsam::FactorGraph::This
FactorGraph< FACTOR > This
Typedef for this class.
Definition: FactorGraph.h:68
gtsam::FactorGraph::push_back
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Definition: FactorGraph.h:267
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::FactorGraph::operator[]
sharedFactor & operator[](size_t i)
Definition: FactorGraph.h:336
args
Definition: pytypes.h:2210
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::end
const_iterator end() const
Definition: FactorGraph.h:342
GTSAM_CONCEPT_TESTABLE_TYPE
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
Definition: FactorGraph-inst.h:76
gtsam::FactorGraph::begin
iterator begin()
Definition: FactorGraph.h:358
gtsam::FactorGraph::begin
const_iterator begin() const
Definition: FactorGraph.h:339
gtsam::FactorGraph::push_back
HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container)
Definition: FactorGraph.h:226
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FactorGraph::at
const std::shared_ptr< F > at(size_t i) const
Const version of templated at method.
Definition: FactorGraph.h:324
gtsam::FactorGraph::reserve
void reserve(size_t size)
Definition: FactorGraph.h:143
gtsam::FactorGraph::exists
bool exists(size_t idx) const
Definition: FactorGraph.h:420
gtsam::FactorGraph::~FactorGraph
virtual ~FactorGraph()=default
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
gtsam::FactorGraph::sharedFactor
std::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:62
gtsam::FactorGraph::FactorGraph
FactorGraph()
Definition: FactorGraph.h:103
gtsam::FactorGraph::operator[]
const sharedFactor operator[](size_t i) const
Definition: FactorGraph.h:331
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition: DotWriter.h:36
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:232
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::FactorGraph::value_type
sharedFactor value_type
Definition: FactorGraph.h:63
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::FactorGraph::add
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Definition: FactorGraph.h:241
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::FactorGraph::operator,
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type & operator,(std::shared_ptr< DERIVEDFACTOR > factor)
Overload comma operator to allow for append chaining.
Definition: FactorGraph.h:190


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:17