gtsam 4.2.0
gtsam
FactorGraph-inst.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
22#pragma once
23
25
26#include <stdio.h>
27#include <algorithm>
28#include <iostream> // for cout :-(
29#include <fstream>
30#include <sstream>
31#include <string>
32
33namespace gtsam {
34
35/* ************************************************************************* */
36template <class FACTOR>
37void FactorGraph<FACTOR>::print(const std::string& s,
38 const KeyFormatter& formatter) const {
39 std::cout << (s.empty() ? "" : s + " ") << std::endl;
40 std::cout << "size: " << size() << std::endl;
41 for (size_t i = 0; i < factors_.size(); i++) {
42 std::stringstream ss;
43 ss << "factor " << i << ": ";
44 if (factors_[i]) factors_[i]->print(ss.str(), formatter);
45 }
46}
47
48/* ************************************************************************* */
49template <class FACTOR>
50bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
51 // check whether the two factor graphs have the same number of factors.
52 if (factors_.size() != fg.size()) return false;
53
54 // check whether the factors are the same, in same order.
55 for (size_t i = 0; i < factors_.size(); i++) {
56 sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
57 if (f1 == nullptr && f2 == nullptr) continue;
58 if (f1 == nullptr || f2 == nullptr) return false;
59 if (!f1->equals(*f2, tol)) return false;
60 }
61 return true;
62}
63
64/* ************************************************************************ */
65template <class FACTOR>
66double FactorGraph<FACTOR>::error(const HybridValues &values) const {
67 double error = 0.0;
68 for (auto &f : factors_) {
69 error += f->error(values);
70 }
71 return error;
72}
73
74/* ************************************************************************* */
75template <class FACTOR>
77 size_t size_ = 0;
78 for (const sharedFactor& factor : factors_)
79 if (factor) size_++;
80 return size_;
81}
82
83/* ************************************************************************* */
84template <class FACTOR>
86 KeySet keys;
87 for (const sharedFactor& factor : this->factors_) {
88 if (factor) keys.insert(factor->begin(), factor->end());
89 }
90 return keys;
91}
92
93/* ************************************************************************* */
94template <class FACTOR>
96 KeyVector keys;
97 keys.reserve(2 * size()); // guess at size
98 for (const sharedFactor& factor : factors_)
99 if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
100 std::sort(keys.begin(), keys.end());
101 auto last = std::unique(keys.begin(), keys.end());
102 keys.erase(last, keys.end());
103 return keys;
104}
105
106/* ************************************************************************* */
107template <class FACTOR>
108template <typename CONTAINER, typename>
110 bool useEmptySlots) {
111 const size_t num_factors = factors.size();
112 FactorIndices newFactorIndices(num_factors);
113 if (useEmptySlots) {
114 size_t i = 0;
115 for (size_t j = 0; j < num_factors; ++j) {
116 // Loop to find the next available factor slot
117 do {
118 if (i >= size())
119 // Make room for remaining factors, happens only once.
120 resize(size() + num_factors - j);
121 else if (at(i))
122 ++i; // Move on to the next slot or past end.
123 else
124 break; // We found an empty slot, break to fill it.
125 } while (true);
126
127 // Use the current slot, updating graph and newFactorSlots.
128 at(i) = factors[j];
129 newFactorIndices[j] = i;
130 }
131 } else {
132 // We're not looking for unused slots, so just add the factors at the end.
133 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
134 push_back(factors);
135 }
136 return newFactorIndices;
137}
138
139/* ************************************************************************* */
140template <class FACTOR>
141void FactorGraph<FACTOR>::dot(std::ostream& os,
142 const KeyFormatter& keyFormatter,
143 const DotWriter& writer) const {
144 writer.graphPreamble(&os);
145
146 // Create nodes for each variable in the graph
147 for (Key key : keys()) {
148 auto position = writer.variablePos(key);
149 writer.drawVariable(key, keyFormatter, position, &os);
150 }
151 os << "\n";
152
153 // Create factors and variable connections
154 for (size_t i = 0; i < size(); ++i) {
155 const auto& factor = at(i);
156 if (factor) {
157 const KeyVector& factorKeys = factor->keys();
158 writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os);
159 }
160 }
161
162 os << "}\n";
163 std::flush(os);
164}
165
166/* ************************************************************************* */
167template <class FACTOR>
168std::string FactorGraph<FACTOR>::dot(const KeyFormatter& keyFormatter,
169 const DotWriter& writer) const {
170 std::stringstream ss;
171 dot(ss, keyFormatter, writer);
172 return ss.str();
173}
174
175/* ************************************************************************* */
176template <class FACTOR>
177void FactorGraph<FACTOR>::saveGraph(const std::string& filename,
178 const KeyFormatter& keyFormatter,
179 const DotWriter& writer) const {
180 std::ofstream of(filename.c_str());
181 dot(of, keyFormatter, writer);
182 of.close();
183}
184
185} // 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:34
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:85
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
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
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition: FactorGraph-inst.h:95
double error(const HybridValues &values) const
Add error for all factors.
Definition: FactorGraph-inst.h:66
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
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition: FactorGraph-inst.h:50
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:101
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition: FactorGraph.h:131
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
DotWriter is a helper class for writing graphviz .dot files.
Definition: DotWriter.h:35
void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition: DotWriter.cpp:42
void processFactor(size_t i, const KeyVector &keys, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Draw a single factor, specified by its index i and its variable keys.
Definition: DotWriter.cpp:96
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition: DotWriter.cpp:79
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition: DotWriter.cpp:30