36template <
class FACTOR>
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++) {
43 ss <<
"factor " << i <<
": ";
44 if (factors_[i]) factors_[i]->print(ss.str(), formatter);
49template <
class FACTOR>
52 if (factors_.size() != fg.
size())
return false;
55 for (
size_t i = 0; i < factors_.size(); i++) {
57 if (f1 ==
nullptr && f2 ==
nullptr)
continue;
58 if (f1 ==
nullptr || f2 ==
nullptr)
return false;
59 if (!f1->equals(*f2, tol))
return false;
65template <
class FACTOR>
68 for (
auto &f : factors_) {
69 error += f->error(values);
75template <
class FACTOR>
84template <
class FACTOR>
88 if (factor) keys.insert(factor->begin(), factor->end());
94template <
class FACTOR>
97 keys.reserve(2 * size());
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());
107template <
class FACTOR>
108template <
typename CONTAINER,
typename>
110 bool useEmptySlots) {
111 const size_t num_factors = factors.size();
115 for (
size_t j = 0; j < num_factors; ++j) {
120 resize(size() + num_factors - j);
129 newFactorIndices[j] = i;
133 for (
size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
136 return newFactorIndices;
140template <
class FACTOR>
147 for (
Key key : keys()) {
154 for (
size_t i = 0; i < size(); ++i) {
155 const auto& factor = at(i);
157 const KeyVector& factorKeys = factor->keys();
158 writer.
processFactor(i, factorKeys, keyFormatter, boost::none, &os);
167template <
class FACTOR>
170 std::stringstream ss;
171 dot(ss, keyFormatter, writer);
176template <
class FACTOR>
180 std::ofstream of(filename.c_str());
181 dot(of, keyFormatter, writer);
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