Advancements to NonlinearFactorGraph Graphviz output - place nodes at their solved locations according to Values, and several formatting options.
Richard Roberts
parent
5e7df8cd23
commit
1e47e15d4e
|
|
@ -18,9 +18,12 @@
|
|||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -43,24 +46,110 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearFactorGraph::saveGraph(std::ostream &stm, const KeyFormatter& keyFormatter) const {
|
||||
void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
||||
const GraphvizFormatting& graphvizFormatting,
|
||||
const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
stm << "graph {\n";
|
||||
stm << " size=\"" << graphvizFormatting.figureWidthInches << "," <<
|
||||
graphvizFormatting.figureHeightInches << "\";\n\n";
|
||||
|
||||
FastSet<Key> keys = this->keys();
|
||||
|
||||
// Local utility function to extract x and y coordinates
|
||||
struct { boost::optional<Point2> operator()(
|
||||
const Value& value, const GraphvizFormatting& graphvizFormatting)
|
||||
{
|
||||
if(const Pose2* p = dynamic_cast<const Pose2*>(&value)) {
|
||||
double x, y;
|
||||
switch (graphvizFormatting.paperHorizontalAxis) {
|
||||
case GraphvizFormatting::X: x = p->x(); break;
|
||||
case GraphvizFormatting::Y: x = p->y(); break;
|
||||
case GraphvizFormatting::Z: x = 0.0; break;
|
||||
case GraphvizFormatting::NEGX: x = -p->x(); break;
|
||||
case GraphvizFormatting::NEGY: x = -p->y(); break;
|
||||
case GraphvizFormatting::NEGZ: x = 0.0; break;
|
||||
}
|
||||
switch (graphvizFormatting.paperVerticalAxis) {
|
||||
case GraphvizFormatting::X: y = p->x(); break;
|
||||
case GraphvizFormatting::Y: y = p->y(); break;
|
||||
case GraphvizFormatting::Z: y = 0.0; break;
|
||||
case GraphvizFormatting::NEGX: y = -p->x(); break;
|
||||
case GraphvizFormatting::NEGY: y = -p->y(); break;
|
||||
case GraphvizFormatting::NEGZ: y = 0.0; break;
|
||||
}
|
||||
return Point2(x,y);
|
||||
} else if(const Pose3* p = dynamic_cast<const Pose3*>(&value)) {
|
||||
double x, y;
|
||||
switch (graphvizFormatting.paperHorizontalAxis) {
|
||||
case GraphvizFormatting::X: x = p->x(); break;
|
||||
case GraphvizFormatting::Y: x = p->y(); break;
|
||||
case GraphvizFormatting::Z: x = p->z(); break;
|
||||
case GraphvizFormatting::NEGX: x = -p->x(); break;
|
||||
case GraphvizFormatting::NEGY: x = -p->y(); break;
|
||||
case GraphvizFormatting::NEGZ: x = -p->z(); break;
|
||||
}
|
||||
switch (graphvizFormatting.paperVerticalAxis) {
|
||||
case GraphvizFormatting::X: y = p->x(); break;
|
||||
case GraphvizFormatting::Y: y = p->y(); break;
|
||||
case GraphvizFormatting::Z: y = p->z(); break;
|
||||
case GraphvizFormatting::NEGX: y = -p->x(); break;
|
||||
case GraphvizFormatting::NEGY: y = -p->y(); break;
|
||||
case GraphvizFormatting::NEGZ: y = -p->z(); break;
|
||||
}
|
||||
return Point2(x,y);
|
||||
} else {
|
||||
return boost::none;
|
||||
}
|
||||
}} getXY;
|
||||
|
||||
// Create nodes for each variable in the graph
|
||||
BOOST_FOREACH(Key key, this->keys()) {
|
||||
BOOST_FOREACH(Key key, keys) {
|
||||
// Label the node with the label from the KeyFormatter
|
||||
stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"];\n"; }
|
||||
stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"";
|
||||
if(values.exists(key)) {
|
||||
boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
|
||||
if(xy)
|
||||
stm << ", pos=\"" << xy->x() << "," << xy->y() << "\"";
|
||||
}
|
||||
stm << "];\n";
|
||||
}
|
||||
stm << "\n";
|
||||
|
||||
// Create factors and variable connections
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point];\n";
|
||||
if(graphvizFormatting.mergeSimilarFactors) {
|
||||
// Remove duplicate factors
|
||||
FastSet<vector<Key> > structure;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
if(factor) {
|
||||
vector<Key> factorKeys = factor->keys();
|
||||
std::sort(factorKeys.begin(), factorKeys.end());
|
||||
structure.insert(factorKeys);
|
||||
}
|
||||
}
|
||||
|
||||
// Make factor-variable connections
|
||||
if(this->at(i)) {
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
stm << " var" << key << "--" << "factor" << i << ";\n"; } }
|
||||
// Create factors and variable connections
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const vector<Key>& factorKeys, structure) {
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point];\n";
|
||||
|
||||
// Make factor-variable connections
|
||||
BOOST_FOREACH(Key key, factorKeys) {
|
||||
stm << " var" << key << "--" << "factor" << i << ";\n"; }
|
||||
|
||||
++ i;
|
||||
}
|
||||
} else {
|
||||
// Create factors and variable connections
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point];\n";
|
||||
|
||||
// Make factor-variable connections
|
||||
if(this->at(i)) {
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
stm << " var" << key << "--" << "factor" << i << ";\n"; } }
|
||||
}
|
||||
}
|
||||
|
||||
stm << "}\n";
|
||||
|
|
|
|||
|
|
@ -27,6 +27,26 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Formatting options when saving in GraphViz format using
|
||||
* NonlinearFactorGraph::saveGraph.
|
||||
*/
|
||||
struct GraphvizFormatting {
|
||||
enum Axis { X, Y, Z, NEGX, NEGY, NEGZ }; ///< World axes to be assigned to paper axes
|
||||
Axis paperHorizontalAxis; ///< The world axis assigned to the horizontal paper axis
|
||||
Axis paperVerticalAxis; ///< The world axis assigned to the vertical paper axis
|
||||
double figureWidthInches; ///< The figure width on paper in inches
|
||||
double figureHeightInches; ///< The figure height on paper in inches
|
||||
bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity
|
||||
/// Default constructor sets up robot coordinates. Paper horizontal is robot Y,
|
||||
/// paper vertical is robot X. Default figure size of 5x5 in.
|
||||
GraphvizFormatting() :
|
||||
paperHorizontalAxis(Y), paperVerticalAxis(X),
|
||||
figureWidthInches(5), figureHeightInches(5),
|
||||
mergeSimilarFactors(false) {}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
|
||||
* which derive from NonlinearFactor. The values structures are typically (in SAM) more general
|
||||
|
|
@ -47,7 +67,9 @@ namespace gtsam {
|
|||
void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Write the graph in GraphViz format for visualization */
|
||||
void saveGraph(std::ostream& stm, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
void saveGraph(std::ostream& stm, const Values& values = Values(),
|
||||
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** return keys as an ordered set - ordering is by key value */
|
||||
FastSet<Key> keys() const;
|
||||
|
|
|
|||
Loading…
Reference in New Issue