Added options for NonlinearFactorGraph printing to dot files

CMakeLists.txt

@@ -106,6 +106,8 @@ endif()
 # BOOST_ROOT: path to install prefix for boost
 # Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
 
+set(BOOST_ROOT /usr/local/lib/boost_1_54_0)
+
 # If using Boost shared libs, disable auto linking
 if(MSVC)
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -117,6 +117,27 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
       default: throw std::runtime_error("Invalid enum value");
       }
      return Point2(x,y);
+    } else if(const Point3* p = dynamic_cast<const Point3*>(&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;
+      default: throw std::runtime_error("Invalid enum value");
+      }
+      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;
+      default: throw std::runtime_error("Invalid enum value");
+      }
+      return Point2(x,y);
     } else {
       return boost::none;
     }
@@ -142,6 +163,8 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   }
 
   // Create nodes for each variable in the graph
+  bool firstTimePoses = true;
+  Key lastKey;
   BOOST_FOREACH(Key key, keys) {
     // Label the node with the label from the KeyFormatter
     stm << "  var" << key << "[label=\"" << keyFormatter(key) << "\"";
@@ -151,9 +174,18 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
         stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\"";
     }
     stm << "];\n";
+
+    if (firstTimePoses) {
+    	lastKey = key;
+    	firstTimePoses = false;
+    } else {
+    	stm << "  var" << key << "--" << "var" << lastKey << ";\n";
+    	lastKey = key;
+    }
   }
   stm << "\n";
 
+
   if(graphvizFormatting.mergeSimilarFactors) {
     // Remove duplicate factors
     FastSet<vector<Key> > structure;
@@ -186,13 +218,39 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   } else {
     // Create factors and variable connections
     for(size_t i = 0; i < this->size(); ++i) {
-      // Make each factor a dot
+      if(graphvizFormatting.plotFactorPoints){
-      stm << "  factor" << i << "[label=\"\", shape=point];\n";
+		// Make each factor a dot
+		stm << "  factor" << i << "[label=\"\", shape=point";
+		{
+		map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
+		if(pos != graphvizFormatting.factorPositions.end())
+		  stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
+		}
+		stm << "];\n";
 
-      // Make factor-variable connections
+		// Make factor-variable connections
-      if(this->at(i)) {
+		if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
-        BOOST_FOREACH(Key key, *this->at(i)) {
+		  BOOST_FOREACH(Key key, *this->at(i)) {
-          stm << "  var" << key << "--" << "factor" << i << ";\n"; } }
+		  stm << "  var" << key << "--" << "factor" << i << ";\n";
+		  }
+		}
+
+	  }
+      else {
+    	if(this->at(i)) {
+    	  Key k;
+    	  bool firstTime = true;
+    	  BOOST_FOREACH(Key key, *this->at(i)) {
+    		if(firstTime){
+    		  k = key;
+    		  firstTime = false;
+    		  continue;
+    		}
+    		stm << "  var" << key << "--" << "var" << k << ";\n";
+    		k = key;
+    	  }
+    	}
+      }
     }
   }
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -45,13 +45,16 @@ namespace gtsam {
     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
     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) {}
+      mergeSimilarFactors(false), plotFactorPoints(true),
+      connectKeysToFactor(true) {}
   };