gtsam 4.1.1 gtsam
FactorGraph-inst.h
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
22#pragma once
23
25
26#include <stdio.h>
27#include <algorithm>
28#include <iostream> // for cout :-(
29#include <sstream>
30#include <string>
31
32namespace gtsam {
33
34/* ************************************************************************* */
35template <class FACTOR>
36void FactorGraph<FACTOR>::print(const std::string& s,
37 const KeyFormatter& formatter) const {
38 std::cout << (s.empty() ? "" : s + " ") << std::endl;
39 std::cout << "size: " << size() << std::endl;
40 for (size_t i = 0; i < factors_.size(); i++) {
41 std::stringstream ss;
42 ss << "factor " << i << ": ";
43 if (factors_[i]) factors_[i]->print(ss.str(), formatter);
44 }
45}
46
47/* ************************************************************************* */
48template <class FACTOR>
49bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
50 // check whether the two factor graphs have the same number of factors.
51 if (factors_.size() != fg.size()) return false;
52
53 // check whether the factors are the same, in same order.
54 for (size_t i = 0; i < factors_.size(); i++) {
55 sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
56 if (f1 == nullptr && f2 == nullptr) continue;
57 if (f1 == nullptr || f2 == nullptr) return false;
58 if (!f1->equals(*f2, tol)) return false;
59 }
60 return true;
61}
62
63/* ************************************************************************* */
64template <class FACTOR>
66 size_t size_ = 0;
67 for (const sharedFactor& factor : factors_)
68 if (factor) size_++;
69 return size_;
70}
71
72/* ************************************************************************* */
73template <class FACTOR>
75 KeySet keys;
76 for (const sharedFactor& factor : this->factors_) {
77 if (factor) keys.insert(factor->begin(), factor->end());
78 }
79 return keys;
80}
81
82/* ************************************************************************* */
83template <class FACTOR>
85 KeyVector keys;
86 keys.reserve(2 * size()); // guess at size
87 for (const sharedFactor& factor : factors_)
88 if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
89 std::sort(keys.begin(), keys.end());
90 auto last = std::unique(keys.begin(), keys.end());
91 keys.erase(last, keys.end());
92 return keys;
93}
94
95/* ************************************************************************* */
96template <class FACTOR>
97template <typename CONTAINER, typename>
99 bool useEmptySlots) {
100 const size_t num_factors = factors.size();
101 FactorIndices newFactorIndices(num_factors);
102 if (useEmptySlots) {
103 size_t i = 0;
104 for (size_t j = 0; j < num_factors; ++j) {
105 // Loop to find the next available factor slot
106 do {
107 if (i >= size())
108 // Make room for remaining factors, happens only once.
109 resize(size() + num_factors - j);
110 else if (at(i))
111 ++i; // Move on to the next slot or past end.
112 else
113 break; // We found an empty slot, break to fill it.
114 } while (true);
115
116 // Use the current slot, updating graph and newFactorSlots.
117 at(i) = factors[j];
118 newFactorIndices[j] = i;
119 }
120 } else {
121 // We're not looking for unused slots, so just add the factors at the end.
122 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
123 push_back(factors);
124 }
125 return newFactorIndices;
126}
127
128} // namespace gtsam
Factor Graph Base Class.
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:33
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
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:93
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:36
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:74
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:98
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition: FactorGraph-inst.h:84
size_t nrFactors() const
return the number of non-null factors
Definition: FactorGraph-inst.h:65
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:305
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:49
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:97
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition: FactorGraph.h:127