Feature/partition configs

hydra_ros/include/hydra_ros/visualizer/feature_color_adaptors.h

@@ -1,6 +1,6 @@
 #pragma once
 #include <hydra/openset/embedding_distances.h>
-#include <hydra_visualizer/adapters/graph_color.h>
+#include <hydra_visualizer/adapters/node_color.h>
 #include <hydra_visualizer/adapters/text.h>
 
 #include <rclcpp/subscription.hpp>
@@ -10,7 +10,7 @@
 
 namespace hydra {
 
-class FeatureScoreColor : public GraphColorAdapter {
+class FeatureScoreColor : public NodeColorAdapter {
  public:
   struct Config {
     std::string ns = "~";
@@ -23,7 +23,7 @@ class FeatureScoreColor : public GraphColorAdapter {
   explicit FeatureScoreColor(const Config& config);
   ~FeatureScoreColor();
   void setGraph(const spark_dsg::DynamicSceneGraph& graph,
-                spark_dsg::LayerId layer) override;
+                spark_dsg::LayerKey layer) override;
   spark_dsg::Color getColor(const spark_dsg::DynamicSceneGraph& graph,
                             const spark_dsg::SceneGraphNode& node) const override;
 
@@ -46,15 +46,11 @@ class FeatureScoreColor : public GraphColorAdapter {
   Eigen::VectorXf feature_;
   std::unordered_map<spark_dsg::NodeId, float> values_;
   std::unique_ptr<EmbeddingDistance> metric_;
-
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<GraphColorAdapter, FeatureScoreColor, Config>(
-          "FeatureScoreColor");
 };
 
 void declare_config(FeatureScoreColor::Config& config);
 
-class NearestFeatureColor : public GraphColorAdapter {
+class NearestFeatureColor : public NodeColorAdapter {
  public:
   struct Config {
     config::VirtualConfig<EmbeddingDistance> metric{CosineDistance::Config()};
@@ -70,15 +66,11 @@ class NearestFeatureColor : public GraphColorAdapter {
   std::unique_ptr<EmbeddingDistance> metric_;
   std::unique_ptr<EmbeddingGroup> features_;
   const visualizer::DiscreteColormap colormap_;
-
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<GraphColorAdapter, NearestFeatureColor, Config>(
-          "NearestFeatureColor");
 };
 
 void declare_config(NearestFeatureColor::Config& config);
 
-class NearestFeatureLabel : public visualizer::GraphTextAdapter {
+class NearestFeatureLabel : public visualizer::NodeTextAdapter {
  public:
   struct Config {
     config::VirtualConfig<EmbeddingDistance> metric{CosineDistance::Config()};
@@ -94,10 +86,6 @@ class NearestFeatureLabel : public visualizer::GraphTextAdapter {
  private:
   std::unique_ptr<EmbeddingDistance> metric_;
   std::unique_ptr<EmbeddingGroup> features_;
-
-  inline static const auto registration_ =
-      config::RegistrationWithConfig<GraphTextAdapter, NearestFeatureLabel, Config>(
-          "NearestFeatureLabel");
 };
 
 }  // namespace hydra

hydra_ros/src/visualizer/feature_color_adaptors.cpp

@@ -12,6 +12,24 @@
 #include <spark_dsg/node_symbol.h>
 
 namespace hydra {
+namespace {
+
+static const auto score_registration =
+    config::RegistrationWithConfig<NodeColorAdapter,
+                                   FeatureScoreColor,
+                                   FeatureScoreColor::Config>("FeatureScoreColor");
+
+static const auto nearest_registration =
+    config::RegistrationWithConfig<NodeColorAdapter,
+                                   NearestFeatureColor,
+                                   NearestFeatureColor::Config>("NearestFeatureColor");
+
+static const auto text_registration =
+    config::RegistrationWithConfig<visualizer::NodeTextAdapter,
+                                   NearestFeatureLabel,
+                                   NearestFeatureLabel::Config>("NearestFeatureLabel");
+
+}  // namespace
 
 using namespace spark_dsg;
 using semantic_inference_msgs::msg::FeatureVectorStamped;
@@ -103,13 +121,13 @@ FeatureScoreColor::FeatureScoreColor(const Config& config)
 
 FeatureScoreColor::~FeatureScoreColor() = default;
 
-void FeatureScoreColor::setGraph(const DynamicSceneGraph& graph, LayerId layer_id) {
+void FeatureScoreColor::setGraph(const DynamicSceneGraph& graph, LayerKey layer_key) {
   values_.clear();
   if (!has_feature_ || !metric_) {
     return;
   }
 
-  const auto& layer = graph.getLayer(layer_id);
+  const auto& layer = graph.getLayer(layer_key.layer, layer_key.partition);
   range_.min = 1.0f;
   range_.max = 0.0f;
   for (const auto& [node_id, node] : layer.nodes()) {

hydra_visualizer/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.22.1)
 project(hydra_visualizer)
 
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
@@ -31,8 +31,8 @@ add_library(
   ${PROJECT_NAME}
   src/plugins/traversability_plugin.cpp
   src/adapters/edge_color.cpp
-  src/adapters/graph_color.cpp
   src/adapters/mesh_color.cpp
+  src/adapters/node_color.cpp
   src/adapters/text.cpp
   src/color/colormap_utilities.cpp
   src/drawing.cpp
@@ -50,6 +50,7 @@ add_library(
   src/plugins/pose_plugin.cpp
   src/scene_graph_renderer.cpp
   src/utils/ear_clipping.cpp
+  src/utils/layer_key_selector.cpp
   src/utils/marker_group_pub.cpp
   src/utils/marker_tracker.cpp
   src/utils/polygon_utilities.cpp
@@ -89,7 +90,11 @@ target_link_libraries(
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest)
-  ament_add_gtest(test_${PROJECT_NAME} tests/main.cpp tests/test_ear_clipping.cpp)
+  ament_add_gtest(test_${PROJECT_NAME}
+      tests/main.cpp
+      tests/test_ear_clipping.cpp
+      tests/test_layer_key_selector.cpp
+  )
   target_link_libraries(
     test_${PROJECT_NAME}
     ${PROJECT_NAME}

hydra_visualizer/app/hydra_visualizer_node.cpp

@@ -40,8 +40,6 @@
 #include <glog/logging.h>
 #include <ianvs/node_init.h>
 
-#include <filesystem>
-
 #include <rclcpp/rclcpp.hpp>
 
 #include "hydra_visualizer/visualizer_node.h"

hydra_visualizer/config/visualizer_config.yaml

@@ -9,50 +9,37 @@ renderer:
       nodes: {scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false}
       text: {draw: true, collapse: true, adapter: {type: LabelTextAdapter}, height: 0.5, scale: 0.45}
       bounding_boxes: {draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5}
-      edges: {interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.5}
     3:
       z_offset_scale: 3.0
       visualize: true
       nodes: {scale: 0.2, color: {type: ParentColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.9, use_sphere: true}
-      edges:
-        scale: 0.01
-        alpha: 0.5
-        color: {type: UniformEdgeColorAdapter}
-        interlayer_use_source: false
-        interlayer_scale: 0.08
-        interlayer_alpha: 0.4
-        interlayer_insertion_skip: 0
+      edges: {scale: 0.01, alpha: 0.5, color: {type: UniformEdgeColorAdapter}}
     4:
       z_offset_scale: 4.2
       visualize: true
       nodes: {scale: 0.6, color: {type: IdColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.8, use_sphere: false}
       text: {draw: true, height: 1.25, scale: 1.0}
-      edges:
-        scale: 0.1
-        alpha: 0.2
-        color: {type: UniformEdgeColorAdapter}
-        interlayer_use_source: true
-        interlayer_scale: 0.08
-        interlayer_alpha: 0.4
-        interlayer_insertion_skip: 0
-  partitions:
-    2:
+      edges: {scale: 0.1, alpha: 0.2, color: {type: UniformEdgeColorAdapter}}
+    2p*:
       z_offset_scale: 0.0
       visualize: true
       nodes: {scale: 0.15, alpha: 0.9, use_sphere: false, color: {type: PartitionColorAdapter}}
       edges: {scale: 0.05, alpha: 0.9, draw_interlayer: false}
       text: {draw_layer: true, height: 0.9, scale: 0.8}
-    3:
+    3p1:
+      z_offset_scale: 3.0
+      visualize: true
+      nodes: {scale: 0.2, color: {type: LabelColorAdapter}, alpha: 0.9, use_sphere: true}
+      boundaries: {draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0}
+      edges: {scale: 0.01, alpha: 0.5, color: {type: UniformEdgeColorAdapter}}
+    3p2:
       z_offset_scale: 3.0
       visualize: true
       nodes: {scale: 0.2, color: {type: LabelColorAdapter}, alpha: 0.9, use_sphere: true}
       boundaries: {draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0}
-      text: {draw: true, collapse: true, adapter: {type: IdTextAdapter}, height: 0.5, scale: 0.45}
-      edges:
-        scale: 0.1
-        alpha: 0.8
-        color: {type: TraversabilityEdgeColorAdapter}
-        interlayer_use_source: false
-        interlayer_scale: 0.08
-        interlayer_alpha: 0.4
-        interlayer_insertion_skip: 0
+      text: {draw: false, collapse: true, adapter: {type: IdTextAdapter}, height: 0.5, scale: 0.45}
+      edges: {scale: 0.1, alpha: 0.8, color: {type: TraversabilityEdgeColorAdapter}}
+  interlayer_edges:
+    - {from: 3*, to: 2p*, draw: false}
+    - {from: 3*, to: 2, use_child_color: true, scale: 0.08, alpha: 0.5}
+    - {from: 4, to: 3*, scale: 0.08, alpha: 0.4}

hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h

@@ -70,7 +70,7 @@ struct EdgeColorAdapter {
    * any edge colors when drawing the scene graph
    */
   virtual void setGraph(const spark_dsg::DynamicSceneGraph& /* graph */,
-                        spark_dsg::LayerId /* layer */) {}
+                        spark_dsg::LayerKey /* layer */) {}
 };
 
 struct UniformEdgeColorAdapter : EdgeColorAdapter {
@@ -112,7 +112,7 @@ struct ValueEdgeColorAdapter : EdgeColorAdapter {
 
   explicit ValueEdgeColorAdapter(const Config& config);
   void setGraph(const spark_dsg::DynamicSceneGraph& graph,
-                spark_dsg::LayerId layer) override;
+                spark_dsg::LayerKey layer) override;
   EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph,
                      const spark_dsg::SceneGraphEdge& edge) const override;
 
@@ -137,7 +137,7 @@ struct TraversabilityEdgeColorAdapter : EdgeColorAdapter {
 
   explicit TraversabilityEdgeColorAdapter(const Config& config);
   void setGraph(const spark_dsg::DynamicSceneGraph& graph,
-                spark_dsg::LayerId layer) override;
+                spark_dsg::LayerKey layer) override;
   EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph,
                      const spark_dsg::SceneGraphEdge& edge) const override;