add active bounding boxes for objects to rviz

Author: nathanhhughes

hydra_ros/include/hydra_ros/frontend/object_visualizer.h

@@ -34,6 +34,7 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include <hydra/frontend/mesh_segmenter.h>
+#include <hydra_visualizer/color/colormap_utilities.h>
 #include <ianvs/lazy_publisher_group.h>
 
 #include <visualization_msgs/msg/marker.hpp>
@@ -47,6 +48,8 @@ class ObjectVisualizer : public MeshSegmenter::Sink {
     double point_scale = 0.1;
     double point_alpha = 0.7;
     bool use_spheres = false;
+    double bounding_box_scale = 0.1;
+    visualizer::CategoricalColormap::Config colormap;
   } const config;
 
   explicit ObjectVisualizer(const Config& config);
@@ -57,7 +60,8 @@ class ObjectVisualizer : public MeshSegmenter::Sink {
 
   void call(uint64_t timestamp_ns,
             const kimera_pgmo::MeshDelta& delta,
-            const LabelIndices& label_indices) const override;
+            const LabelIndices& label_indices,
+            const MeshSegmenter::LabelClusters& clusters) const override;
 
  protected:
   void fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta,
@@ -68,10 +72,7 @@ class ObjectVisualizer : public MeshSegmenter::Sink {
   ianvs::NodeHandle nh_;
   ianvs::RosPublisherGroup<visualization_msgs::msg::Marker> pubs_;
 
- private:
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<MeshSegmenter::Sink, ObjectVisualizer, Config>(
-          "ObjectVisualizer");
+  const visualizer::CategoricalColormap colormap_;
 };
 
 void declare_config(ObjectVisualizer::Config& conf);

hydra_ros/launch/datasets/uhumans2.launch.yaml

@@ -38,3 +38,8 @@ launch:
         - {name: map_frame, value: map}
         - {name: lcd_config_path, value: $(find-pkg-share hydra)/config/lcd/uhumans2.yaml}
         - {name: extra_yaml, value: $(if $(var use_gt_semantics) $(var extra_yaml_gt) $(var extra_yaml_no_gt))}
+  - node:
+      pkg: tf2_ros
+      name: world_to_map
+      exec: static_transform_publisher
+      args: --frame-id world --child-frame-id map

hydra_ros/src/frontend/object_visualizer.cpp

@@ -38,9 +38,18 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 #include <hydra/common/global_info.h>
+#include <hydra_visualizer/drawing.h>
 #include <kimera_pgmo/mesh_delta.h>
 
 namespace hydra {
+namespace {
+
+static const auto registration =
+    config::RegistrationWithConfig<MeshSegmenter::Sink,
+                                   ObjectVisualizer,
+                                   ObjectVisualizer::Config>("ObjectVisualizer");
+
+}
 
 using visualization_msgs::msg::Marker;
 
@@ -51,18 +60,36 @@ void declare_config(ObjectVisualizer::Config& config) {
   field(config.point_scale, "point_scale");
   field(config.point_alpha, "point_alpha");
   field(config.use_spheres, "use_spheres");
+  field(config.bounding_box_scale, "bounding_box_scale");
+  field(config.colormap, "colormap");
 }
 
 ObjectVisualizer::ObjectVisualizer(const Config& config)
     : config(config::checkValid(config)),
       nh_(ianvs::NodeHandle::this_node(config.module_ns)),
-      pubs_(nh_) {}
+      pubs_(nh_),
+      colormap_(config.colormap) {}
 
 std::string ObjectVisualizer::printInfo() const { return config::toString(config); }
 
+struct DeltaPointAdaptor : spark_dsg::BoundingBox::PointAdaptor {
+  DeltaPointAdaptor(const kimera_pgmo::MeshDelta& delta,
+                    const std::vector<size_t>& indices)
+      : delta(delta), indices(indices) {}
+
+  size_t size() const override { return indices.size(); }
+
+  Eigen::Vector3f get(size_t index) const override {
+    return delta.getVertex(indices.at(index)).pos;
+  }
+  const kimera_pgmo::MeshDelta& delta;
+  const std::vector<size_t>& indices;
+};
+
 void ObjectVisualizer::call(uint64_t timestamp_ns,
                             const kimera_pgmo::MeshDelta& delta,
-                            const LabelIndices& label_indices) const {
+                            const LabelIndices& label_indices,
+                            const MeshSegmenter::LabelClusters& clusters) const {
   pubs_.publish("active_vertices", [&]() {
     auto msg = std::make_unique<Marker>();
     msg->header.stamp = rclcpp::Time(timestamp_ns);
@@ -87,6 +114,27 @@ void ObjectVisualizer::call(uint64_t timestamp_ns,
       return msg;
     });
   }
+
+  pubs_.publish("object_clusters", [&]() {
+    auto msg = std::make_unique<Marker>();
+    msg->header.stamp = rclcpp::Time(timestamp_ns);
+    msg->header.frame_id = GlobalInfo::instance().getFrames().odom;
+    msg->type = Marker::LINE_LIST;
+    msg->action = Marker::ADD;
+    msg->ns = "object_clusters";
+    msg->id = 0;
+    msg->scale.x = config.bounding_box_scale;
+    for (const auto& [label, label_clusters] : clusters) {
+      const auto color = visualizer::makeColorMsg(colormap_(label));
+      for (const auto& cluster : label_clusters) {
+        const DeltaPointAdaptor adaptor(delta, cluster.indices);
+        const spark_dsg::BoundingBox bbox(adaptor);
+        visualizer::drawBoundingBox(bbox, color, *msg);
+      }
+    }
+
+    return msg;
+  });
 }
 
 void ObjectVisualizer::fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta,