diff --git a/gtsam/nonlinear/GraphvizFormatting.cpp b/gtsam/nonlinear/GraphvizFormatting.cpp index c37f07c8a8..e5b81c66b9 100644 --- a/gtsam/nonlinear/GraphvizFormatting.cpp +++ b/gtsam/nonlinear/GraphvizFormatting.cpp @@ -53,6 +53,17 @@ boost::optional GraphvizFormatting::operator()( } else if (const GenericValue* p = dynamic_cast*>(&value)) { t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = + dynamic_cast*>(&value)) { + if (p->dim() == 2) { + const Eigen::Ref p_2d(p->value()); + t << p_2d.x(), p_2d.y(), 0; + } else if (p->dim() == 3) { + const Eigen::Ref p_3d(p->value()); + t = p_3d; + } else { + return boost::none; + } } else if (const GenericValue* p = dynamic_cast*>(&value)) { t = p->value().translation();