diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/mesh_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/mesh_color.h index 142a06e..b667534 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/mesh_color.h +++ b/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 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. */ diff --git a/hydra_visualizer/src/adapters/mesh_color.cpp b/hydra_visualizer/src/adapters/mesh_color.cpp index 5ece643..872fc6c 100644 --- a/hydra_visualizer/src/adapters/mesh_color.cpp +++ b/hydra_visualizer/src/adapters/mesh_color.cpp @@ -1,10 +1,20 @@ #include "hydra_visualizer/adapters/mesh_color.h" #include +#include +#include #include "hydra_visualizer/color/color_parsing.h" namespace hydra { +namespace { + +static const auto splt_reg = + config::RegistrationWithConfig("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_) { diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index df9f584..7d4ff98 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/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( name, rclcpp::QoS(1).transient_local())) {}