12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added options for NonlinearFactorGraph printing to dot files

release/4.3a0
djensen3 2013-11-26 15:41:15 +00:00
parent 5b90fefddc
commit e2cd2c2054
3 changed files with 70 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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

70
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -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;
}
}
}
}
}

5
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -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) {}
};