Feature/edge colors

hydra_visualizer/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(visualization_msgs REQUIRED)
 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/text.cpp

hydra_visualizer/config/visualizer_config.yaml

@@ -1,60 +1,69 @@
 ---
 renderer:
-  layer_z_step: 5.5  # unit separation between layers
-  collapse_layers: false  # whether or not to apply offsets to each of the layers
+  layer_z_step: 5.5 # unit separation between layers
+  collapse_layers: false # whether or not to apply offsets to each of the layers
   layers:
     2:
       z_offset_scale: 2.0
       visualize: true
-      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.9, interlayer_use_color: true}
+      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.9 }
     3:
       z_offset_scale: 3.0
       visualize: true
-      nodes: {scale: 0.2, color: {type: ParentColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.9, use_sphere: true}
+      nodes:
+        {
+          scale: 0.2,
+          color: { type: ParentColorAdapter, colormap: { palette: colorbrewer } },
+          alpha: 0.9,
+          use_sphere: true,
+        }
       edges:
         scale: 0.01
         alpha: 0.5
-        use_color: false
+        color: { type: UniformEdgeColorAdapter }
         interlayer_use_source: false
         interlayer_scale: 0.08
         interlayer_alpha: 0.4
-        interlayer_use_color: true
         interlayer_insertion_skip: 0
     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}
+      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
-        use_color: false
+        color: { type: UniformEdgeColorAdapter }
         interlayer_use_source: true
         interlayer_scale: 0.08
         interlayer_alpha: 0.4
-        interlayer_use_color: true
         interlayer_insertion_skip: 0
   partitions:
     2:
       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}
+      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:
       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}
+      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
-        use_color: false
+        color: { type: UniformEdgeColorAdapter }
         interlayer_use_source: false
         interlayer_scale: 0.08
         interlayer_alpha: 0.4
-        interlayer_use_color: true
         interlayer_insertion_skip: 0

hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h

@@ -0,0 +1,131 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above copyright notice,
+ *     this list of conditions and the following disclaimer in the documentation
+ *     and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#pragma once
+#include <config_utilities/factory.h>
+#include <spark_dsg/color.h>
+#include <spark_dsg/dynamic_scene_graph.h>
+
+#include <memory>
+
+#include "hydra_visualizer/color/colormap_utilities.h"
+
+namespace hydra {
+
+#define REGISTER_COLOR_ADAPTER(adapter)    \
+  inline static const auto registration_ = \
+      config::RegistrationWithConfig<EdgeColorAdapter, adapter, Config>(#adapter)
+
+struct EdgeColorAdapter {
+  using Ptr = std::shared_ptr<EdgeColorAdapter>;
+  using EdgeColor = std::pair<spark_dsg::Color, spark_dsg::Color>;
+
+  virtual ~EdgeColorAdapter() = default;
+
+  /**
+   * @brief Get color for an edge.
+   * @param graph Current scene graph node is from
+   * @param edge Edge to get color for
+   * @returns Visualizer color for source and target ends of the edge.
+   */
+  virtual EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph,
+                             const spark_dsg::SceneGraphEdge& edge) const = 0;
+
+  /**
+   * @brief Set any pre-draw information
+   * @param graph Graph to get information for
+   *
+   * Allows color adapters to gather statistics about the scene graph before generating
+   * any edge colors when drawing the scene graph
+   */
+  virtual void setGraph(const spark_dsg::DynamicSceneGraph& /* graph */,
+                        spark_dsg::LayerId /* layer */) {}
+};
+
+struct UniformEdgeColorAdapter : EdgeColorAdapter {
+  struct Config {
+    // TODO(lschmid): Consider using the named colors, or even better integrate
+    // optionally named colors directly into the config utilities parsing.
+    spark_dsg::Color color;
+  } const config;
+
+  explicit UniformEdgeColorAdapter(const Config& config);
+  EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph,
+                     const spark_dsg::SceneGraphEdge& edge) const override;
+
+ private:
+  REGISTER_COLOR_ADAPTER(UniformEdgeColorAdapter);
+};
+
+void declare_config(UniformEdgeColorAdapter::Config& config);
+
+struct EdgeValueFunctor {
+  virtual ~EdgeValueFunctor() = default;
+  virtual double eval(const spark_dsg::DynamicSceneGraph& graph,
+                      const spark_dsg::SceneGraphEdge& edge) const = 0;
+};
+
+struct EdgeWeightFunctor : EdgeValueFunctor {
+  double eval(const spark_dsg::DynamicSceneGraph& graph,
+              const spark_dsg::SceneGraphEdge& edge) const override;
+
+  inline static const auto registration =
+      config::Registration<EdgeValueFunctor, EdgeWeightFunctor>("weight");
+};
+
+struct ValueEdgeColorAdapter : EdgeColorAdapter {
+  struct Config {
+    visualizer::RangeColormap::Config colormap;
+    std::string value_functor{"weight"};
+  } const config;
+
+  explicit ValueEdgeColorAdapter(const Config& config);
+  void setGraph(const spark_dsg::DynamicSceneGraph& graph,
+                spark_dsg::LayerId layer) override;
+  EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph,
+                     const spark_dsg::SceneGraphEdge& edge) const override;
+
+ private:
+  double min_value_;
+  double max_value_;
+  std::unique_ptr<EdgeValueFunctor> functor_;
+  const visualizer::RangeColormap colormap_;
+  REGISTER_COLOR_ADAPTER(ValueEdgeColorAdapter);
+};
+
+void declare_config(ValueEdgeColorAdapter::Config& config);
+
+#undef REGISTER_COLOR_ADAPTER
+
+}  // namespace hydra

hydra_visualizer/include/hydra_visualizer/layer_info.h

@@ -37,6 +37,7 @@
 #include <spark_dsg/color.h>
 #include <spark_dsg/dynamic_scene_graph.h>
 
+#include "hydra_visualizer/adapters/edge_color.h"
 #include "hydra_visualizer/adapters/graph_color.h"
 #include "hydra_visualizer/adapters/text.h"
 
@@ -74,10 +75,9 @@ struct LayerConfig {
     double scale = 0.03;  //[ 0.001, 1.0]
     //! @brief intralayer edge alpha
     double alpha = 1.0;  //[ 0.0, 1.0]
-    //! @brief Color to use for edge
-    NamedColors color = NamedColors::BLACK;
-    //! @brief show intralayer edge using node colors
-    bool use_color = true;
+    //! @brief Color to use for edge. Unspecified uses node colors.
+    config::VirtualConfig<EdgeColorAdapter, true> color{
+        UniformEdgeColorAdapter::Config()};
     //! @brief draw interlayer edges
     bool draw_interlayer = true;
     //! @brief use edge source layer for config
@@ -86,7 +86,7 @@ struct LayerConfig {
     double interlayer_scale = 0.03;  // [0.001, 1.0]
     //! @brief interlayer edge alpha
     double interlayer_alpha = 1.0;  // [0.0, 1.0]
-    //! @brief show interlayer edge using node colors
+    //! @brief If true color dsg-mesh edges
     bool interlayer_use_color = true;
     //! @brief Number of edges to skip when drawing interlayer edges
     size_t interlayer_insertion_skip = 0;  // [0, 1000]
@@ -155,6 +155,8 @@ class LayerInfo {
   using FilterFunction = std::function<bool(const spark_dsg::SceneGraphNode&)>;
   using ColorFunction =
       std::function<spark_dsg::Color(const spark_dsg::SceneGraphNode&)>;
+  using EdgeColorFunction = std::function<std::pair<spark_dsg::Color, spark_dsg::Color>(
+      const spark_dsg::SceneGraphEdge&)>;
   using TextFunction = std::function<std::string(const spark_dsg::SceneGraphNode&)>;
 
   LayerInfo(const LayerConfig config);
@@ -163,17 +165,18 @@ class LayerInfo {
 
   bool shouldVisualize(const spark_dsg::SceneGraphNode& node) const;
   spark_dsg::Color text_color() const;
-  spark_dsg::Color edge_color() const;
 
   const LayerConfig config;
 
   double z_offset;
   ColorFunction node_color;
+  EdgeColorFunction edge_color;
   TextFunction node_text;
   mutable FilterFunction filter;
 
  private:
-  std::unique_ptr<GraphColorAdapter> color_adapter_;
+  std::unique_ptr<GraphColorAdapter> node_color_adapter_;
+  std::unique_ptr<EdgeColorAdapter> edge_color_adapter_;
   std::unique_ptr<GraphTextAdapter> text_adapter_;
 };