add quick mesh color adapter for offline visualization (#58)

Author: nathanhhughes

hydra_visualizer/include/hydra_visualizer/adapters/mesh_color.h

@@ -186,6 +186,32 @@ struct SeenDurationMeshColoring : public MeshColoring {
 
 void declare_config(SeenDurationMeshColoring::Config& config);
 
+/**
+ * @brief Functor to partially apply a color functor
+ *
+ * Points with a positive distance to the plane will use the child coloring functor,
+ * otherwise they will use the original mesh color
+ */
+struct SplitMeshColoring : public MeshColoring {
+  struct Config {
+    config::VirtualConfig<MeshColoring> coloring;
+    Eigen::Vector3f normal = Eigen::Vector3f::Ones();
+    Eigen::Vector3f origin = Eigen::Vector3f::Zero();
+    spark_dsg::Color default_color = spark_dsg::Color::gray();
+  } const config;
+
+  explicit SplitMeshColoring(const Config& config);
+  virtual ~SplitMeshColoring() = default;
+
+  void setMesh(const spark_dsg::Mesh& mesh) override;
+  spark_dsg::Color getVertexColor(const spark_dsg::Mesh& mesh, size_t i) const override;
+
+ private:
+  MeshColoring::Ptr coloring_;
+};
+
+void declare_config(SplitMeshColoring::Config& config);
+
 /**
  * @brief Utility class to color a mesh based on a mesh coloring for visualization.
  */

hydra_visualizer/src/adapters/mesh_color.cpp

@@ -1,10 +1,20 @@
 #include "hydra_visualizer/adapters/mesh_color.h"
 
 #include <config_utilities/config.h>
+#include <config_utilities/types/eigen_matrix.h>
+#include <config_utilities/validation.h>
 
 #include "hydra_visualizer/color/color_parsing.h"
 
 namespace hydra {
+namespace {
+
+static const auto splt_reg =
+    config::RegistrationWithConfig<MeshColoring,
+                                   SplitMeshColoring,
+                                   SplitMeshColoring::Config>("SplitMeshColoring");
+
+}
 
 using spark_dsg::Color;
 using spark_dsg::Mesh;
@@ -124,6 +134,35 @@ void SeenDurationMeshColoring::setMaxDuration(spark_dsg::Mesh::Timestamp max) {
   max_ = max;
 }
 
+void declare_config(SplitMeshColoring::Config& config) {
+  using namespace config;
+  name("SplitMeshColoring::Config");
+  config.coloring.setOptional();
+  field(config.coloring, "coloring");
+  field(config.normal, "normal");
+  field(config.origin, "origin");
+  field(config.default_color, "default_color");
+}
+
+SplitMeshColoring::SplitMeshColoring(const Config& config)
+    : config(config::checkValid(config)), coloring_(config.coloring.create()) {}
+
+void SplitMeshColoring::setMesh(const Mesh& mesh) {
+  if (coloring_) {
+    coloring_->setMesh(mesh);
+  }
+}
+
+Color SplitMeshColoring::getVertexColor(const Mesh& mesh, size_t i) const {
+  const auto& pos = mesh.pos(i);
+  const auto dist = config.normal.dot(pos - config.origin);
+  if (dist >= 0.0f && coloring_) {
+    return coloring_->getVertexColor(mesh, i);
+  }
+
+  return mesh.has_colors ? mesh.color(i) : config.default_color;
+}
+
 MeshColorAdapter::MeshColorAdapter(const Mesh& mesh, MeshColoring::ConstPtr coloring)
     : mesh(mesh), coloring_(std::move(coloring)) {
   if (coloring_) {

hydra_visualizer/src/plugins/mesh_plugin.cpp

@@ -64,7 +64,8 @@ MeshPlugin::MeshPlugin(const Config& config,
                        ianvs::NodeHandle nh,
                        const std::string& name)
     : VisualizerPlugin(name),
-      config_("mesh_plugin", config::checkValid(config)),
+      config_(
+          "mesh_plugin", config::checkValid(config), [this]() { has_change_ = true; }),
       mesh_pub_(nh.create_publisher<kimera_pgmo_msgs::msg::Mesh>(
           name, rclcpp::QoS(1).transient_local())) {}