Loading...
Searching...
No Matches
FactorGraph-inst.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <stdio.h>
#include <algorithm>
#include <iostream> // for cout :-(
#include <fstream>
#include <sstream>
#include <string>
namespace gtsam {
/* ************************************************************************* */
template <class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s,
const KeyFormatter& formatter) const {
std::cout << (s.empty() ? "" : s + " ") << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i]) factors_[i]->print(ss.str(), formatter);
}
}
/* ************************************************************************* */
template <class FACTOR>
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
// check whether the two factor graphs have the same number of factors.
if (factors_.size() != fg.size()) return false;
// check whether the factors are the same, in same order.
for (size_t i = 0; i < factors_.size(); i++) {
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
if (f1 == nullptr && f2 == nullptr) continue;
if (f1 == nullptr || f2 == nullptr) return false;
if (!f1->equals(*f2, tol)) return false;
}
return true;
}
/* ************************************************************************ */
template <class FACTOR>
double FactorGraph<FACTOR>::error(const HybridValues &values) const {
double error = 0.0;
for (auto &f : factors_) {
error += f->error(values);
}
return error;
}
/* ************************************************************************* */
template <class FACTOR>
size_t FactorGraph<FACTOR>::nrFactors() const {
size_t size_ = 0;
for (const sharedFactor& factor : factors_)
if (factor) size_++;
return size_;
}
/* ************************************************************************* */
template <class FACTOR>
KeySet FactorGraph<FACTOR>::keys() const {
KeySet keys;
for (const sharedFactor& factor : this->factors_) {
if (factor) keys.insert(factor->begin(), factor->end());
}
return keys;
}
/* ************************************************************************* */
template <class FACTOR>
KeyVector FactorGraph<FACTOR>::keyVector() const {
KeyVector keys;
keys.reserve(2 * size()); // guess at size
for (const sharedFactor& factor : factors_)
if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
std::sort(keys.begin(), keys.end());
auto last = std::unique(keys.begin(), keys.end());
keys.erase(last, keys.end());
return keys;
}
/* ************************************************************************* */
template <class FACTOR>
template <typename CONTAINER, typename>
FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
bool useEmptySlots) {
const size_t num_factors = factors.size();
FactorIndices newFactorIndices(num_factors);
if (useEmptySlots) {
size_t i = 0;
for (size_t j = 0; j < num_factors; ++j) {
// Loop to find the next available factor slot
do {
if (i >= size())
// Make room for remaining factors, happens only once.
resize(size() + num_factors - j);
else if (at(i))
++i; // Move on to the next slot or past end.
else
break; // We found an empty slot, break to fill it.
} while (true);
// Use the current slot, updating graph and newFactorSlots.
at(i) = factors[j];
newFactorIndices[j] = i;
}
} else {
// We're not looking for unused slots, so just add the factors at the end.
for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
push_back(factors);
}
return newFactorIndices;
}
/* ************************************************************************* */
template <class FACTOR>
void FactorGraph<FACTOR>::dot(std::ostream& os,
const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
writer.graphPreamble(&os);
// Create nodes for each variable in the graph
for (Key key : keys()) {
auto position = writer.variablePos(key);
writer.drawVariable(key, keyFormatter, position, &os);
}
os << "\n";
// Create factors and variable connections
for (size_t i = 0; i < size(); ++i) {
const auto& factor = at(i);
if (factor) {
const KeyVector& factorKeys = factor->keys();
writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os);
}
}
os << "}\n";
std::flush(os);
}
/* ************************************************************************* */
template <class FACTOR>
std::string FactorGraph<FACTOR>::dot(const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
std::stringstream ss;
dot(ss, keyFormatter, writer);
return ss.str();
}
/* ************************************************************************* */
template <class FACTOR>
void FactorGraph<FACTOR>::saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
std::ofstream of(filename.c_str());
dot(of, keyFormatter, writer);
of.close();
}
} // namespace gtsam
FactorGraph.h
Factor Graph Base Class.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
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::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
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
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
gtsam::FactorGraph::add_factors
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
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
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::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
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::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
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
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
gtsam::DotWriter::drawVariable
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
gtsam::DotWriter::processFactor
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
gtsam::DotWriter::variablePos
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition DotWriter.cpp:79
gtsam::DotWriter::graphPreamble
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition DotWriter.cpp:30