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 
33 namespace gtsam {
34 
35 /* ************************************************************************* */
36 template <class FACTOR>
37 void 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 /* ************************************************************************* */
49 template <class FACTOR>
50 bool 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 /* ************************************************************************ */
65 template <class FACTOR>
67  double error = 0.0;
68  for (auto &f : factors_) {
69  error += f->error(values);
70  }
71  return error;
72 }
73 
74 /* ************************************************************************* */
75 template <class FACTOR>
77  size_t size_ = 0;
78  for (const sharedFactor& factor : factors_)
79  if (factor) size_++;
80  return size_;
81 }
82 
83 /* ************************************************************************* */
84 template <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 /* ************************************************************************* */
94 template <class FACTOR>
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 /* ************************************************************************* */
107 template <class FACTOR>
108 template <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 /* ************************************************************************* */
140 template <class FACTOR>
141 void 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, {}, &os);
159  }
160  }
161 
162  os << "}\n";
163  std::flush(os);
164 }
165 
166 /* ************************************************************************* */
167 template <class FACTOR>
168 std::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 /* ************************************************************************* */
176 template <class FACTOR>
177 void 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
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Definition: FactorGraph-inst.h:109
gtsam::HybridValues
Definition: HybridValues.h:37
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
Definition: FactorGraph.h:92
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
resize
v resize(3)
gtsam::DotWriter::drawVariable
void drawVariable(Key key, const KeyFormatter &keyFormatter, const std::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition: DotWriter.cpp:42
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Definition: FactorGraph-inst.h:66
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::position
Point3_ position(const NavState_ &X)
Definition: navigation/expressions.h:37
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::keyVector
KeyVector keyVector() const
Definition: FactorGraph-inst.h:95
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
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
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::FastSet< Key >
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::DotWriter::variablePos
std::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition: DotWriter.cpp:79
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
ss
static std::stringstream ss
Definition: testBTree.cpp:31
gtsam::FactorGraph< Factor >
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
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::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::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
gtsam::DotWriter::processFactor
void processFactor(size_t i, const KeyVector &keys, const KeyFormatter &keyFormatter, const std::optional< Vector2 > &position, std::ostream *os) const
Draw a single factor, specified by its index i and its variable keys.
Definition: DotWriter.cpp:98
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
FactorGraph.h
Factor Graph Base Class.
gtsam::DotWriter::graphPreamble
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition: DotWriter.cpp:30
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::nrFactors
size_t nrFactors() const
Definition: FactorGraph-inst.h:76
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FactorGraph< Factor >::sharedFactor
std::shared_ptr< Factor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:62
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
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::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:31