diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 67bd0c456..0479b06a5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -129,7 +129,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if(values.exists(key)) { boost::optional xy = getXY(values.at(key), graphvizFormatting); if(xy) - stm << ", pos=\"" << (xy->x() - minX) << "," << (xy->y() - minY) << "!\""; + stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; } @@ -150,7 +150,13 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, size_t i = 0; BOOST_FOREACH(const vector& factorKeys, structure) { // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point];\n"; + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::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 BOOST_FOREACH(Key key, factorKeys) { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 216e9e1c4..d86831703 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -37,12 +38,14 @@ namespace gtsam { Axis paperVerticalAxis; ///< The world axis assigned to the vertical paper axis double figureWidthInches; ///< The figure width on paper in inches 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 + std::map 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), + figureWidthInches(5), figureHeightInches(5), scale(1), mergeSimilarFactors(false) {} };