Added options for NonlinearFactorGraph printing to dot files
							parent
							
								
									5b90fefddc
								
							
						
					
					
						commit
						e2cd2c2054
					
				|  | @ -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 | ||||
|  |  | |||
|  | @ -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
 | ||||
|       stm << "  factor" << i << "[label=\"\", shape=point];\n"; | ||||
|       if(graphvizFormatting.plotFactorPoints){ | ||||
| 		// 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
 | ||||
|       if(this->at(i)) { | ||||
|         BOOST_FOREACH(Key key, *this->at(i)) { | ||||
|           stm << "  var" << key << "--" << "factor" << i << ";\n"; } } | ||||
| 		// Make factor-variable connections
 | ||||
| 		if(graphvizFormatting.connectKeysToFactor && this->at(i)) { | ||||
| 		  BOOST_FOREACH(Key key, *this->at(i)) { | ||||
| 		  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; | ||||
|     	  } | ||||
|     	} | ||||
|       } | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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) {} | ||||
|   }; | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue