From 9cf8c4477b7972beb55c396ae31ee32ab31b594c Mon Sep 17 00:00:00 2001
From: senselessDev <senselessDev@users.noreply.github.com>
Date: Sat, 22 Jan 2022 22:08:32 +0100
Subject: [PATCH] add position to Point2 nodes in GraphViz

---
 gtsam/nonlinear/GraphvizFormatting.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

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<Vector2> GraphvizFormatting::operator()(
   } else if (const GenericValue<Vector2>* p =
                  dynamic_cast<const GenericValue<Vector2>*>(&value)) {
     t << p->value().x(), p->value().y(), 0;
+  } else if (const GenericValue<Vector>* p =
+                 dynamic_cast<const GenericValue<Vector>*>(&value)) {
+    if (p->dim() == 2) {
+      const Eigen::Ref<const Vector2> p_2d(p->value());
+      t << p_2d.x(), p_2d.y(), 0;
+    } else if (p->dim() == 3) {
+      const Eigen::Ref<const Vector3> p_3d(p->value());
+      t = p_3d;
+    } else {
+      return boost::none;
+    }
   } else if (const GenericValue<Pose3>* p =
                  dynamic_cast<const GenericValue<Pose3>*>(&value)) {
     t = p->value().translation();