Go to the documentation of this file.
36 template <
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 <<
": ";
49 template <
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;
65 template <
class FACTOR>
68 for (
auto &
f : factors_) {
75 template <
class FACTOR>
84 template <
class FACTOR>
88 if (factor)
keys.insert(factor->begin(), factor->end());
94 template <
class FACTOR>
99 if (factor)
keys.insert(
keys.end(), factor->begin(), factor->end());
100 std::sort(
keys.begin(),
keys.end());
107 template <
class FACTOR>
108 template <
typename CONTAINER,
typename>
110 bool useEmptySlots) {
115 for (
size_t j = 0;
j < num_factors; ++
j) {
129 newFactorIndices[
j] =
i;
133 for (
size_t i = 0;
i < num_factors; ++
i) newFactorIndices[
i] =
i +
size();
136 return newFactorIndices;
140 template <
class FACTOR>
154 for (
size_t i = 0;
i <
size(); ++
i) {
155 const auto& factor = at(
i);
157 const KeyVector& factorKeys = factor->keys();
167 template <
class FACTOR>
170 std::stringstream
ss;
171 dot(
ss, keyFormatter, writer);
176 template <
class FACTOR>
181 dot(of, keyFormatter, writer);
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
FastVector< sharedFactor > factors_
void drawVariable(Key key, const KeyFormatter &keyFormatter, const std::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
double error(const HybridValues &values) const
Point3_ position(const NavState_ &X)
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
const GaussianFactorGraph factors
KeyVector keyVector() const
const KeyFormatter & formatter
double f2(const Vector2 &x)
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
ofstream os("timeSchurFactors.csv")
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
std::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
static std::stringstream ss
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
double dot(const V1 &a, const V2 &b)
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.
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
std::shared_ptr< Factor > sharedFactor
Shared pointer to a factor.
std::uint64_t Key
Integer nonlinear key type.
DotWriter is a helper class for writing graphviz .dot files.
FastVector< FactorIndex > FactorIndices
Define collection types:
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:17