Merge pull request #1058 from senselessDev/point2_fix_graphviz

borglab · Jan 24, 2022 · f9d1af3 · f9d1af3
2 parents f43900f + 9cf8c44
commit f9d1af3
Showing 1 changed file with 11 additions and 0 deletions.
diff --git 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();