Feature/refactor delta

hydra_ros/include/hydra_ros/frontend/object_visualizer.h

@@ -34,8 +34,8 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include <hydra/frontend/mesh_segmenter.h>
+#include <hydra_visualizer/color/colormap_utilities.h>
 #include <ianvs/lazy_publisher_group.h>
-#include <kimera_pgmo/mesh_delta.h>
 
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -48,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);
@@ -58,8 +60,8 @@ class ObjectVisualizer : public MeshSegmenter::Sink {
 
   void call(uint64_t timestamp_ns,
             const kimera_pgmo::MeshDelta& delta,
-            const std::vector<size_t>& active,
-            const LabelIndices& label_indices) const override;
+            const LabelIndices& label_indices,
+            const MeshSegmenter::LabelClusters& clusters) const override;
 
  protected:
   void fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta,
@@ -70,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/include/hydra_ros/frontend/ros_frontend_publisher.h

@@ -34,12 +34,11 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include <hydra/frontend/graph_builder.h>
-#include <kimera_pgmo_ros/conversion/mesh_delta.h>
 #include <pose_graph_tools_ros/conversions.h>
 
 #include <map>
-#include <queue>
 
+#include <kimera_pgmo_msgs/msg/mesh_delta.hpp>
 #include <kimera_pgmo_msgs/srv/mesh_delta_query.hpp>
 
 #include "hydra_ros/utils/dsg_streaming_interface.h"
@@ -49,6 +48,9 @@ namespace hydra {
 class RosFrontendPublisher : public GraphBuilder::Sink {
  public:
   using MeshDeltaSrv = kimera_pgmo_msgs::srv::MeshDeltaQuery;
+  using MeshDeltaRequest = kimera_pgmo_msgs::srv::MeshDeltaQuery::Request::SharedPtr;
+  using MeshDeltaResponse = kimera_pgmo_msgs::srv::MeshDeltaQuery::Response::SharedPtr;
+  using MeshDeltaMsg = kimera_pgmo_msgs::msg::MeshDelta;
 
   struct Config {
     //! @brief Configuration for dsg publisher
@@ -65,16 +67,14 @@ class RosFrontendPublisher : public GraphBuilder::Sink {
   std::string printInfo() const override { return "RosFrontendPublisher"; }
 
  protected:
-  void processMeshDeltaQuery(const MeshDeltaSrv::Request::SharedPtr req,
-                             MeshDeltaSrv::Response::SharedPtr resp);
+  void processMeshDeltaQuery(const MeshDeltaRequest req, MeshDeltaResponse resp);
 
   std::unique_ptr<DsgSender> dsg_sender_;
-  mutable std::map<uint16_t, kimera_pgmo::MeshDelta::Ptr> stored_delta_;
-
   pose_graph_tools::PoseGraphPublisher mesh_graph_pub_;
-  kimera_pgmo::PgmoMeshDeltaPublisher mesh_update_pub_;
+  rclcpp::Publisher<MeshDeltaMsg>::SharedPtr mesh_update_pub_;
   rclcpp::Service<MeshDeltaSrv>::SharedPtr mesh_delta_server_;
-  rclcpp::TypeAdapter<kimera_pgmo::MeshDelta, kimera_pgmo_msgs::msg::MeshDelta>
-      mesh_delta_converter_;
+
+  mutable std::map<uint16_t, MeshDeltaMsg::SharedPtr> stored_delta_;
 };
+
 }  // namespace hydra

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,8 +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;
 
@@ -50,25 +60,44 @@ 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 std::vector<size_t>& active,
-                            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);
     msg->header.frame_id = GlobalInfo::instance().getFrames().odom;
     msg->ns = "active_vertices";
     msg->id = 0;
+    std::vector<size_t> active(delta.getNumActiveVertices());
+    std::iota(active.begin(), active.end(), delta.getNumArchivedVertices());
     fillMarkerFromCloud(delta, active, *msg);
     return msg;
   });
@@ -85,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,
@@ -101,15 +151,15 @@ void ObjectVisualizer::fillMarkerFromCloud(const kimera_pgmo::MeshDelta& delta,
   msg.points.reserve(indices.size());
   msg.colors.reserve(indices.size());
   for (const auto idx : indices) {
-    const auto& p = delta.vertex_updates->at(idx);
+    const auto& p = delta.getVertex(idx);
     auto& point = msg.points.emplace_back();
-    point.x = p.x;
-    point.y = p.y;
-    point.z = p.z;
+    point.x = p.pos.x();
+    point.y = p.pos.y();
+    point.z = p.pos.z();
     auto& color = msg.colors.emplace_back();
-    color.r = p.r / 255.0f;
-    color.g = p.g / 255.0f;
-    color.b = p.b / 255.0f;
+    color.r = p.traits.color[0] / 255.0f;
+    color.g = p.traits.color[1] / 255.0f;
+    color.b = p.traits.color[2] / 255.0f;
     color.a = config.point_alpha;
   }
 }

hydra_ros/src/frontend/ros_frontend_publisher.cpp

@@ -39,10 +39,11 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 #include <hydra/common/global_info.h>
+#include <kimera_pgmo/mesh_delta.h>
+#include <kimera_pgmo_ros/conversion/mesh_delta.h>
 
 namespace hydra {
 
-using kimera_pgmo::MeshDeltaTypeAdapter;
 using pose_graph_tools::PoseGraphTypeAdapter;
 using BaseInterface = rclcpp::node_interfaces::NodeBaseInterface;
 using rclcpp::CallbackGroupType;
@@ -66,7 +67,8 @@ void declare_config(RosFrontendPublisher::Config& config) {
 }
 
 RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh)
-    : config(config::checkValid(get_config())) {
+    : config(config::checkValid(get_config())),
+      dsg_sender_(new DsgSender(config.dsg_sender, nh)) {
   auto group = nh.as<BaseInterface>()->create_callback_group(
       CallbackGroupType::MutuallyExclusive);
   mesh_delta_server_ =
@@ -75,46 +77,48 @@ RosFrontendPublisher::RosFrontendPublisher(ianvs::NodeHandle nh)
                                       this,
                                       rclcpp::ServicesQoS(),
                                       group);
-  dsg_sender_ = std::make_unique<DsgSender>(config.dsg_sender, nh);
-  mesh_graph_pub_ = nh.create_publisher<PoseGraphTypeAdapter>(
-      "mesh_graph_incremental", rclcpp::QoS(100).transient_local());
-  mesh_update_pub_ = nh.create_publisher<MeshDeltaTypeAdapter>(
-      "full_mesh_update", rclcpp::QoS(100).transient_local());
+
+  const auto qos = rclcpp::QoS(100).transient_local();
+  mesh_graph_pub_ =
+      nh.create_publisher<PoseGraphTypeAdapter>("mesh_graph_incremental", qos);
+  mesh_update_pub_ = nh.create_publisher<MeshDeltaMsg>("full_mesh_update", qos);
 }
 
 void RosFrontendPublisher::call(uint64_t timestamp_ns,
                                 const DynamicSceneGraph& graph,
                                 const BackendInput& backend_input) const {
   // TODO(nathan) make sure pgmo stamps the deformation graph
   mesh_graph_pub_->publish(backend_input.deformation_graph);
+
   if (backend_input.mesh_update) {
     backend_input.mesh_update->timestamp_ns = timestamp_ns;
-    mesh_update_pub_->publish(*backend_input.mesh_update);
-    stored_delta_.insert(
-        {backend_input.mesh_update->sequence_number, backend_input.mesh_update});
-    if (config.mesh_delta_queue_size > 0 &&
-        stored_delta_.size() > static_cast<size_t>(config.mesh_delta_queue_size)) {
+    auto delta_msg = std::make_shared<MeshDeltaMsg>();
+    kimera_pgmo::conversions::to_ros(*backend_input.mesh_update, *delta_msg);
+    mesh_update_pub_->publish(*delta_msg);
+
+    stored_delta_.insert({backend_input.mesh_update->info.sequence_number, delta_msg});
+    if (config.mesh_delta_queue_size &&
+        stored_delta_.size() > config.mesh_delta_queue_size) {
       stored_delta_.erase(stored_delta_.begin());
     }
   }
 
   dsg_sender_->sendGraph(graph, rclcpp::Time(timestamp_ns));
 }
 
-void RosFrontendPublisher::processMeshDeltaQuery(
-    const MeshDeltaSrv::Request::SharedPtr req,
-    MeshDeltaSrv::Response::SharedPtr resp) {
-  LOG(INFO) << "Received request for " << req->sequence_numbers.size()
-            << " mesh deltas...";
+void RosFrontendPublisher::processMeshDeltaQuery(const MeshDeltaRequest req,
+                                                 MeshDeltaResponse resp) {
+  LOG(INFO) << "Received request for " << req->sequence_numbers.size() << " deltas...";
   for (const auto& seq : req->sequence_numbers) {
     auto& msg = resp->deltas.emplace_back();
-    // Check TypeAdater documentation TODO(Yun)
     if (!stored_delta_.count(seq)) {
       LOG(ERROR) << "Mesh delta sequence " << seq << " not found";
       continue;
     }
-    mesh_delta_converter_.convert_to_ros_message(*stored_delta_.at(seq), msg);
+
+    msg = *stored_delta_.at(seq);
   }
+
   LOG(INFO) << "Responding with " << resp->deltas.size() << " deltas...";
 }
 

hydra_visualizer/src/drawing.cpp

@@ -648,12 +648,11 @@ kimera_pgmo_msgs::msg::Mesh makeMeshMsg(const std_msgs::msg::Header& header,
 
   MeshColorAdapter adapter(mesh, coloring);
   msg.vertices.resize(mesh.points.size());
-  msg.vertex_colors.resize(mesh.points.size());
   for (size_t i = 0; i < mesh.points.size(); ++i) {
     auto& vertex = msg.vertices[i];
-    tf2::convert(mesh.points[i].cast<double>().eval(), vertex);
-    auto& color = msg.vertex_colors[i];
-    color = visualizer::makeColorMsg(adapter.getVertexColor(i));
+    tf2::convert(mesh.points[i].cast<double>().eval(), vertex.pos);
+    vertex.has_color = true;
+    vertex.color = visualizer::makeColorMsg(adapter.getVertexColor(i));
   }
 
   msg.triangles.resize(mesh.faces.size());