GraphvizFormatting refactor

- separate file
- inherit from DotWriter
- made dot/dot/saveGraph the pattern
- deprecated saveGraph(ostream) method

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 <gtsam/nonlinear/GraphvizFormatting.h>
+#include <gtsam/nonlinear/Values.h>
+
+// TODO(frank): nonlinear should not depend on geometry:
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+
+#include <limits>
+
+namespace gtsam {
+
+Vector2 GraphvizFormatting::findBounds(const Values& values,
+                                       const KeySet& keys) const {
+  Vector2 min;
+  min.x() = std::numeric_limits<double>::infinity();
+  min.y() = std::numeric_limits<double>::infinity();
+  for (const Key& key : keys) {
+    if (values.exists(key)) {
+      boost::optional<Vector2> 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<Vector2> GraphvizFormatting::operator()(
+    const Value& value) const {
+  Vector3 t;
+  if (const GenericValue<Pose2>* p =
+          dynamic_cast<const GenericValue<Pose2>*>(&value)) {
+    t << p->value().x(), p->value().y(), 0;
+  } else if (const GenericValue<Vector2>* p =
+                 dynamic_cast<const GenericValue<Vector2>*>(&value)) {
+    t << p->value().x(), p->value().y(), 0;
+  } else if (const GenericValue<Pose3>* p =
+                 dynamic_cast<const GenericValue<Pose3>*>(&value)) {
+    t = p->value().translation();
+  } else if (const GenericValue<Point3>* p =
+                 dynamic_cast<const GenericValue<Point3>*>(&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<Vector2> GraphvizFormatting::variablePos(const Values& values,
+                                                         const Vector2& min,
+                                                         Key key) const {
+  if (!values.exists(key)) return boost::none;
+  boost::optional<Vector2> 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<Vector2> 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

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 <gtsam/inference/DotWriter.h>
+
+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<size_t, Vector2> 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<Vector2> operator()(const Value& value) const;
+
+  /// Return affinely transformed variable position if it exists.
+  boost::optional<Vector2> variablePos(const Values& values, const Vector2& min,
+                                       Key key) const;
+
+  /// Return affinely transformed factor position if it exists.
+  boost::optional<Vector2> factorPos(const Vector2& min, size_t i) const;
+};
+
+}  // namespace gtsam

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -26,6 +26,7 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/DotWriter.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
@@ -35,7 +36,6 @@
 
 #include <cmath>
 #include <fstream>
-#include <limits>
 
 using namespace std;
 
@@ -91,89 +91,25 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol)
 }
 
 /* ************************************************************************* */
-void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
+void NonlinearFactorGraph::dot(std::ostream& os, const Values& values,
-    const GraphvizFormatting& formatting,
+                               const GraphvizFormatting& writer,
-    const KeyFormatter& keyFormatter) const
+                               const KeyFormatter& keyFormatter) const {
-{
+  writer.writePreamble(&os);
-  stm << "graph {\n";
-  stm << "  size=\"" << formatting.figureWidthInches << "," <<
-    formatting.figureHeightInches << "\";\n\n";
 
+  // Find bounds (imperative)
   KeySet keys = this->keys();
-
+  Vector2 min = writer.findBounds(values, keys);
-  // Local utility function to extract x and y coordinates
-  struct { boost::optional<Point2> operator()(
-    const Value& value, const GraphvizFormatting& graphvizFormatting)
-  {
-    Vector3 t;
-    if (const GenericValue<Pose2>* p = dynamic_cast<const GenericValue<Pose2>*>(&value)) {
-      t << p->value().x(), p->value().y(), 0;
-    } else if (const GenericValue<Point2>* p = dynamic_cast<const GenericValue<Point2>*>(&value)) {
-      t << p->value().x(), p->value().y(), 0;
-    } else if (const GenericValue<Pose3>* p = dynamic_cast<const GenericValue<Pose3>*>(&value)) {
-      t = p->value().translation();
-    } else if (const GenericValue<Point3>* p = dynamic_cast<const GenericValue<Point3>*>(&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<double>::infinity(), maxX = -numeric_limits<double>::infinity();
-  double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
-  for (const Key& key : keys) {
-    if (values.exists(key)) {
-      boost::optional<Point2> 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();
-      }
-    }
-  }
 
   // Create nodes for each variable in the graph
-  for(Key key: keys){
+  for (Key key : keys) {
-    // Label the node with the label from the KeyFormatter
+    auto position = writer.variablePos(values, min, key);
-    stm << "  var" << key << "[label=\"" << keyFormatter(key) << "\"";
+    writer.DrawVariable(key, keyFormatter, position, &os);
-    if(values.exists(key)) {
-      boost::optional<Point2> xy = getXY(values.at(key), formatting);
-      if(xy)
-      stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\"";
-    }
-    stm << "];\n";
   }
-  stm << "\n";
+  os << "\n";
 
-  if (formatting.mergeSimilarFactors) {
+  if (writer.mergeSimilarFactors) {
     // Remove duplicate factors
-    std::set<KeyVector > structure;
+    std::set<KeyVector> 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){
+    for (const KeyVector& factorKeys : structure) {
-      // Make each factor a dot
+      writer.ProcessFactor(i++, factorKeys, boost::none, &os);
-      stm << "  factor" << i << "[label=\"\", shape=point";
-      {
-        map<size_t, Point2>::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;
     }
   } 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) {
-      if (!factor) {
+        const KeyVector& factorKeys = factor->keys();
-        continue;
+        writer.ProcessFactor(i, factorKeys, writer.factorPos(min, i), &os);
-      }
-
-      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<size_t, Point2>::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;
-        }
       }
     }
   }
 
-  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 std::string& filename, const Values& values,
-    const GraphvizFormatting& graphvizFormatting,
+    const GraphvizFormatting& writer,
     const KeyFormatter& keyFormatter) const {
-  std::ofstream of(file);
+  std::ofstream of(filename);
-  saveGraph(of, values, graphvizFormatting, keyFormatter);
+  dot(of, values, writer, keyFormatter);
   of.close();
 }
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -23,6 +23,7 @@
 
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/GraphvizFormatting.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/nonlinear/PriorFactor.h>
 
@@ -41,32 +42,6 @@ namespace gtsam {
   template<typename T>
   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<size_t, Point2> 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<PriorFactor<T>>(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
 
   };

tests/testNonlinearFactorGraph.cpp

@@ -15,6 +15,7 @@
  * @brief   testNonlinearFactorGraph
  * @author  Carlos Nieto
  * @author  Christian Potthast
+ * @author  Frank Dellaert
  */
 
 #include <gtsam/base/Testable.h>
@@ -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); }
 /* ************************************************************************* */