From 9ce9231e14eba9e34b62da9ab337484c9263fb67 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 2 Jan 2013 19:27:34 +0000
Subject: [PATCH] Fixes in Graphviz visualization of NonlinearFactorGraph -
 shift positions to start at 0,0 and add '!' charater in dot file to pin nodes

---
 gtsam/nonlinear/NonlinearFactorGraph.cpp | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 0c22f1365..67bd0c456 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -103,6 +103,25 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     }
   }} 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();
+  BOOST_FOREACH(Key key, keys) {
+    if(values.exists(key)) {
+      boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
+      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
   BOOST_FOREACH(Key key, keys) {
     // Label the node with the label from the KeyFormatter
@@ -110,7 +129,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     if(values.exists(key)) {
       boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
       if(xy)
-        stm << ", pos=\"" << xy->x() << "," << xy->y() << "\"";
+        stm << ", pos=\"" << (xy->x() - minX) << "," << (xy->y() - minY) << "!\"";
     }
     stm << "];\n";
   }