diff --git a/gtsam/nonlinear/GraphvizFormatting.cpp b/gtsam/nonlinear/GraphvizFormatting.cpp new file mode 100644 index 000000000..c37f07c8a --- /dev/null +++ b/gtsam/nonlinear/GraphvizFormatting.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2021, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file GraphvizFormatting.cpp + * @brief Graphviz formatter for NonlinearFactorGraph + * @author Frank Dellaert + * @date December, 2021 + */ + +#include +#include + +// TODO(frank): nonlinear should not depend on geometry: +#include +#include + +#include + +namespace gtsam { + +Vector2 GraphvizFormatting::findBounds(const Values& values, + const KeySet& keys) const { + Vector2 min; + min.x() = std::numeric_limits::infinity(); + min.y() = std::numeric_limits::infinity(); + for (const Key& key : keys) { + if (values.exists(key)) { + boost::optional xy = operator()(values.at(key)); + if (xy) { + if (xy->x() < min.x()) min.x() = xy->x(); + if (xy->y() < min.y()) min.y() = xy->y(); + } + } + } + return min; +} + +boost::optional GraphvizFormatting::operator()( + const Value& value) const { + Vector3 t; + if (const GenericValue* p = + dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = + dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = + dynamic_cast*>(&value)) { + t = p->value().translation(); + } else if (const GenericValue* p = + dynamic_cast*>(&value)) { + t = p->value(); + } else { + return boost::none; + } + double x, y; + switch (paperHorizontalAxis) { + case X: + x = t.x(); + break; + case Y: + x = t.y(); + break; + case Z: + x = t.z(); + break; + case NEGX: + x = -t.x(); + break; + case NEGY: + x = -t.y(); + break; + case NEGZ: + x = -t.z(); + break; + default: + throw std::runtime_error("Invalid enum value"); + } + switch (paperVerticalAxis) { + case X: + y = t.x(); + break; + case Y: + y = t.y(); + break; + case Z: + y = t.z(); + break; + case NEGX: + y = -t.x(); + break; + case NEGY: + y = -t.y(); + break; + case NEGZ: + y = -t.z(); + break; + default: + throw std::runtime_error("Invalid enum value"); + } + return Vector2(x, y); +} + +// Return affinely transformed variable position if it exists. +boost::optional GraphvizFormatting::variablePos(const Values& values, + const Vector2& min, + Key key) const { + if (!values.exists(key)) return boost::none; + boost::optional xy = operator()(values.at(key)); + if (xy) { + xy->x() = scale * (xy->x() - min.x()); + xy->y() = scale * (xy->y() - min.y()); + } + return xy; +} + +// Return affinely transformed factor position if it exists. +boost::optional GraphvizFormatting::factorPos(const Vector2& min, + size_t i) const { + if (factorPositions.size() == 0) return boost::none; + auto it = factorPositions.find(i); + if (it == factorPositions.end()) return boost::none; + auto pos = it->second; + return Vector2(scale * (pos.x() - min.x()), scale * (pos.y() - min.y())); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/GraphvizFormatting.h b/gtsam/nonlinear/GraphvizFormatting.h new file mode 100644 index 000000000..c36b09a8f --- /dev/null +++ b/gtsam/nonlinear/GraphvizFormatting.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2021, 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file GraphvizFormatting.h + * @brief Graphviz formatter for NonlinearFactorGraph + * @author Frank Dellaert + * @date December, 2021 + */ + +#pragma once + +#include + +namespace gtsam { + +class Values; +class Value; + +/** + * Formatting options and functions for saving a NonlinearFactorGraph instance + * in GraphViz format. + */ +struct GTSAM_EXPORT GraphvizFormatting : public DotWriter { + /// World axes to be assigned to paper axes + enum Axis { X, Y, Z, NEGX, NEGY, NEGZ }; + + Axis paperHorizontalAxis; ///< The world axis assigned to the horizontal + ///< paper axis + Axis paperVerticalAxis; ///< The world axis assigned to the vertical paper + ///< axis + double scale; ///< Scale all positions to reduce / increase density + bool mergeSimilarFactors; ///< Merge multiple factors that have the same + ///< connectivity + + /// (optional for each factor) Manually specify factor "dot" positions: + std::map factorPositions; + + /// 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), + scale(1), + mergeSimilarFactors(false) {} + + // Find bounds + Vector2 findBounds(const Values& values, const KeySet& keys) const; + + /// Extract a Vector2 from either Vector2, Pose2, Pose3, or Point3 + boost::optional operator()(const Value& value) const; + + /// Return affinely transformed variable position if it exists. + boost::optional variablePos(const Values& values, const Vector2& min, + Key key) const; + + /// Return affinely transformed factor position if it exists. + boost::optional factorPos(const Vector2& min, size_t i) const; +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 8e4cf277c..a6f715d9e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include // for GTSAM_USE_TBB @@ -35,7 +36,6 @@ #include #include -#include using namespace std; @@ -91,89 +91,25 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) } /* ************************************************************************* */ -void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, - const GraphvizFormatting& formatting, - const KeyFormatter& keyFormatter) const -{ - stm << "graph {\n"; - stm << " size=\"" << formatting.figureWidthInches << "," << - formatting.figureHeightInches << "\";\n\n"; +void NonlinearFactorGraph::dot(std::ostream& os, const Values& values, + const GraphvizFormatting& writer, + const KeyFormatter& keyFormatter) const { + writer.writePreamble(&os); + // Find bounds (imperative) KeySet keys = this->keys(); - - // Local utility function to extract x and y coordinates - struct { boost::optional operator()( - const Value& value, const GraphvizFormatting& graphvizFormatting) - { - Vector3 t; - if (const GenericValue* p = dynamic_cast*>(&value)) { - t << p->value().x(), p->value().y(), 0; - } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t << p->value().x(), p->value().y(), 0; - } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().translation(); - } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value(); - } else { - return boost::none; - } - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = t.x(); break; - case GraphvizFormatting::Y: x = t.y(); break; - case GraphvizFormatting::Z: x = t.z(); break; - case GraphvizFormatting::NEGX: x = -t.x(); break; - case GraphvizFormatting::NEGY: x = -t.y(); break; - case GraphvizFormatting::NEGZ: x = -t.z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = t.x(); break; - case GraphvizFormatting::Y: y = t.y(); break; - case GraphvizFormatting::Z: y = t.z(); break; - case GraphvizFormatting::NEGX: y = -t.x(); break; - case GraphvizFormatting::NEGY: y = -t.y(); break; - case GraphvizFormatting::NEGZ: y = -t.z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - }} getXY; - - // Find bounds - double minX = numeric_limits::infinity(), maxX = -numeric_limits::infinity(); - double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); - for (const Key& key : keys) { - if (values.exists(key)) { - boost::optional xy = getXY(values.at(key), formatting); - if(xy) { - if(xy->x() < minX) - minX = xy->x(); - if(xy->x() > maxX) - maxX = xy->x(); - if(xy->y() < minY) - minY = xy->y(); - if(xy->y() > maxY) - maxY = xy->y(); - } - } - } + Vector2 min = writer.findBounds(values, keys); // Create nodes for each variable in the graph - for(Key key: keys){ - // Label the node with the label from the KeyFormatter - stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; - if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), formatting); - if(xy) - stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\""; - } - stm << "];\n"; + for (Key key : keys) { + auto position = writer.variablePos(values, min, key); + writer.DrawVariable(key, keyFormatter, position, &os); } - stm << "\n"; + os << "\n"; - if (formatting.mergeSimilarFactors) { + if (writer.mergeSimilarFactors) { // Remove duplicate factors - std::set structure; + std::set structure; for (const sharedFactor& factor : factors_) { if (factor) { KeyVector factorKeys = factor->keys(); @@ -184,86 +120,40 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - for(const KeyVector& factorKeys: structure){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = formatting.factorPositions.find(i); - if(pos != formatting.factorPositions.end()) - stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," - << formatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; - - // Make factor-variable connections - for(Key key: factorKeys) { - stm << " var" << key << "--" << "factor" << i << ";\n"; - } - - ++ i; + for (const KeyVector& factorKeys : structure) { + writer.ProcessFactor(i++, factorKeys, boost::none, &os); } } else { // Create factors and variable connections - for(size_t i = 0; i < size(); ++i) { + for (size_t i = 0; i < size(); ++i) { const NonlinearFactor::shared_ptr& factor = at(i); - // If null pointer, move on to the next - if (!factor) { - continue; - } - - if (formatting.plotFactorPoints) { - const KeyVector& keys = factor->keys(); - if (formatting.binaryEdges && keys.size() == 2) { - stm << " var" << keys[0] << "--" - << "var" << keys[1] << ";\n"; - } else { - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = - formatting.factorPositions.find(i); - if (pos != formatting.factorPositions.end()) - stm << ", pos=\"" << formatting.scale * (pos->second.x() - minX) - << "," << formatting.scale * (pos->second.y() - minY) - << "!\""; - } - stm << "];\n"; - - // Make factor-variable connections - if (formatting.connectKeysToFactor && factor) { - for (Key key : *factor) { - stm << " var" << key << "--" - << "factor" << i << ";\n"; - } - } - } - } else { - Key k; - bool firstTime = true; - for (Key key : *this->at(i)) { - if (firstTime) { - k = key; - firstTime = false; - continue; - } - stm << " var" << key << "--" - << "var" << k << ";\n"; - k = key; - } + if (factor) { + const KeyVector& factorKeys = factor->keys(); + writer.ProcessFactor(i, factorKeys, writer.factorPos(min, i), &os); } } } - stm << "}\n"; + os << "}\n"; + std::flush(os); +} + +/* ************************************************************************* */ +std::string NonlinearFactorGraph::dot( + const Values& values, const GraphvizFormatting& writer, + const KeyFormatter& keyFormatter) const { + std::stringstream ss; + dot(ss, values, writer, keyFormatter); + return ss.str(); } /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph( - const std::string& file, const Values& values, - const GraphvizFormatting& graphvizFormatting, + const std::string& filename, const Values& values, + const GraphvizFormatting& writer, const KeyFormatter& keyFormatter) const { - std::ofstream of(file); - saveGraph(of, values, graphvizFormatting, keyFormatter); + std::ofstream of(filename); + dot(of, values, writer, keyFormatter); of.close(); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 61cbbafb9..c958d6302 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -41,32 +42,6 @@ namespace gtsam { template class ExpressionFactor; - /** - * Formatting options when saving in GraphViz format using - * NonlinearFactorGraph::saveGraph. - */ - struct GTSAM_EXPORT 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 - double scale; ///< Scale all positions to reduce / increase density - bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity - bool plotFactorPoints; ///< Plots each factor as a dot between the variables - bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor - bool binaryEdges; ///< just use non-dotted edges for binary factors - std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. - /// 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), scale(1), - mergeSimilarFactors(false), plotFactorPoints(true), - connectKeysToFactor(true), binaryEdges(true) {} - }; - - /** * 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 @@ -115,21 +90,6 @@ namespace gtsam { /** Test equality */ bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; - /// Write the graph in GraphViz format for visualization - void saveGraph(std::ostream& stm, const Values& values = Values(), - const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /** - * Write the graph in GraphViz format to file for visualization. - * - * This is a wrapper friendly version since wrapped languages don't have - * access to C++ streams. - */ - void saveGraph(const std::string& file, const Values& values = Values(), - const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& values) const; @@ -246,7 +206,33 @@ namespace gtsam { emplace_shared>(key, prior, covariance); } - private: + /// @name Graph Display + /// @{ + + using FactorGraph::dot; + using FactorGraph::saveGraph; + + /// Output to graphviz format, stream version, with Values/extra options. + void dot( + std::ostream& os, const Values& values, + const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// Output to graphviz format string, with Values/extra options. + std::string dot( + const Values& values, + const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// output to file with graphviz format, with Values/extra options. + void saveGraph( + const std::string& filename, const Values& values, + const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + private: /** * Linearize from Scatter rather than from Ordering. Made private because @@ -275,6 +261,14 @@ namespace gtsam { Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} + + /** \deprecated */ + void GTSAM_DEPRECATED saveGraph( + std::ostream& os, const Values& values = Values(), + const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + dot(os, values, graphvizFormatting, keyFormatter); + } #endif }; diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index fdb080a63..4dec08f45 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -15,6 +15,7 @@ * @brief testNonlinearFactorGraph * @author Carlos Nieto * @author Christian Potthast + * @author Frank Dellaert */ #include @@ -285,6 +286,7 @@ TEST(testNonlinearFactorGraph, addPrior) { EXPECT(0 != graph.error(values)); } +/* ************************************************************************* */ TEST(NonlinearFactorGraph, printErrors) { const NonlinearFactorGraph fg = createNonlinearFactorGraph(); @@ -309,6 +311,53 @@ TEST(NonlinearFactorGraph, printErrors) for (bool visit : visited) EXPECT(visit==true); } +/* ************************************************************************* */ +TEST(NonlinearFactorGraph, dot) { + string expected = + "graph {\n" + " size=\"5,5\";\n" + "\n" + " var7782220156096217089[label=\"l1\"];\n" + " var8646911284551352321[label=\"x1\"];\n" + " var8646911284551352322[label=\"x2\"];\n" + "\n" + " factor0[label=\"\", shape=point];\n" + " var8646911284551352321--factor0;\n" + " var8646911284551352321--var8646911284551352322;\n" + " var8646911284551352321--var7782220156096217089;\n" + " var8646911284551352322--var7782220156096217089;\n" + "}\n"; + + const NonlinearFactorGraph fg = createNonlinearFactorGraph(); + string actual = fg.dot(); + EXPECT(actual == expected); +} + +/* ************************************************************************* */ +TEST(NonlinearFactorGraph, dot_extra) { + string expected = + "graph {\n" + " size=\"5,5\";\n" + "\n" + " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n" + "\n" + " factor0[label=\"\", shape=point];\n" + " var8646911284551352321--factor0;\n" + " var8646911284551352321--var8646911284551352322;\n" + " var8646911284551352321--var7782220156096217089;\n" + " var8646911284551352322--var7782220156096217089;\n" + "}\n"; + + const NonlinearFactorGraph fg = createNonlinearFactorGraph(); + const Values c = createValues(); + + stringstream ss; + fg.dot(ss, c); + EXPECT(ss.str() == expected); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */