From 21232252806a947c5ac2d284d778b318449dd301 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 27 Jan 2022 14:34:38 -0500
Subject: [PATCH] allow factorPositions

---
 gtsam/inference/BayesNet-inst.h        | 1 +
 gtsam/inference/DotWriter.cpp          | 5 ++++-
 gtsam/inference/DotWriter.h            | 8 +++++++-
 gtsam/inference/FactorGraph-inst.h     | 3 ++-
 gtsam/nonlinear/GraphvizFormatting.cpp | 2 +-
 gtsam/nonlinear/GraphvizFormatting.h   | 3 ---
 6 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h
index c201475c5..afde5498d 100644
--- a/gtsam/inference/BayesNet-inst.h
+++ b/gtsam/inference/BayesNet-inst.h
@@ -48,6 +48,7 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
   }
   os << "\n";
 
+  // Reverse order as typically Bayes nets stored in reverse topological sort.
   for (auto conditional : boost::adaptors::reverse(*this)) {
     auto frontals = conditional->frontals();
     const Key me = frontals.front();
diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp
index a6a33bc74..ad5330575 100644
--- a/gtsam/inference/DotWriter.cpp
+++ b/gtsam/inference/DotWriter.cpp
@@ -102,7 +102,10 @@ void DotWriter::processFactor(size_t i, const KeyVector& keys,
       ConnectVariables(keys[0], keys[1], keyFormatter, os);
     } else {
       // Create dot for the factor.
-      DrawFactor(i, position, os);
+      if (!position && factorPositions.count(i))
+        DrawFactor(i, factorPositions.at(i), os);
+      else
+        DrawFactor(i, position, os);
 
       // Make factor-variable connections
       if (connectKeysToFactor) {
diff --git a/gtsam/inference/DotWriter.h b/gtsam/inference/DotWriter.h
index 13683e338..23302ee60 100644
--- a/gtsam/inference/DotWriter.h
+++ b/gtsam/inference/DotWriter.h
@@ -42,7 +42,7 @@ struct GTSAM_EXPORT DotWriter {
 
   /**
    * Variable positions can be optionally specified and will be included in the
-   * dor file with a "!' sign, so "neato" can use it to render them.
+   * dot file with a "!' sign, so "neato" can use it to render them.
    */
   std::map<Key, Vector2> variablePositions;
 
@@ -56,6 +56,12 @@ struct GTSAM_EXPORT DotWriter {
   /** A set of keys that will be displayed as a box */
   std::set<Key> boxes;
 
+  /**
+   * Factor positions can be optionally specified and will be included in the
+   * dot file with a "!' sign, so "neato" can use it to render them.
+   */
+  std::map<size_t, Vector2> factorPositions;
+
   explicit DotWriter(double figureWidthInches = 5,
                      double figureHeightInches = 5,
                      bool plotFactorPoints = true,
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index 3d85be49e..a2ae07101 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/gtsam/inference/FactorGraph-inst.h
@@ -135,7 +135,8 @@ void FactorGraph<FACTOR>::dot(std::ostream& os,
 
   // Create nodes for each variable in the graph
   for (Key key : keys()) {
-    writer.drawVariable(key, keyFormatter, boost::none, &os);
+    auto position = writer.variablePos(key);
+    writer.drawVariable(key, keyFormatter, position, &os);
   }
   os << "\n";
 
diff --git a/gtsam/nonlinear/GraphvizFormatting.cpp b/gtsam/nonlinear/GraphvizFormatting.cpp
index 1f0b3a875..ca3466b6a 100644
--- a/gtsam/nonlinear/GraphvizFormatting.cpp
+++ b/gtsam/nonlinear/GraphvizFormatting.cpp
@@ -124,7 +124,7 @@ boost::optional<Vector2> GraphvizFormatting::extractPosition(
 boost::optional<Vector2> GraphvizFormatting::variablePos(const Values& values,
                                                          const Vector2& min,
                                                          Key key) const {
-  if (!values.exists(key)) return boost::none;
+  if (!values.exists(key)) return DotWriter::variablePos(key);
   boost::optional<Vector2> xy = extractPosition(values.at(key));
   if (xy) {
     xy->x() = scale * (xy->x() - min.x());
diff --git a/gtsam/nonlinear/GraphvizFormatting.h b/gtsam/nonlinear/GraphvizFormatting.h
index d71e73f31..03cdb3469 100644
--- a/gtsam/nonlinear/GraphvizFormatting.h
+++ b/gtsam/nonlinear/GraphvizFormatting.h
@@ -41,9 +41,6 @@ struct GTSAM_EXPORT GraphvizFormatting : public DotWriter {
   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()