From 548182e74897cd3948e97023aad95b3a9d21ce8f Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 8 Mar 2024 19:58:44 -0500 Subject: [PATCH] Plane projection mesh modifier (#223) * Added plane projection mesh modifier * Added plane projection mesh modifier widget * Project mesh in place without losing data through conversion * Update mesh normals to plane normal if they exist * Reverse orientation of plane if normal points towards mesh origin --- noether_gui/CMakeLists.txt | 6 +- .../plane_projection_modifier_widget.h | 25 ++++ noether_gui/src/plugins.cpp | 7 ++ .../plane_projection_modifier_widget.cpp | 40 +++++++ noether_tpp/CMakeLists.txt | 1 + .../plane_projection_modifier.h | 18 +++ .../plane_projection_modifier.cpp | 110 ++++++++++++++++++ 7 files changed, 206 insertions(+), 1 deletion(-) create mode 100644 noether_gui/include/noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h create mode 100644 noether_gui/src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp create mode 100644 noether_tpp/include/noether_tpp/mesh_modifiers/plane_projection_modifier.h create mode 100644 noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt index 27aa3a45..8a9e750f 100644 --- a/noether_gui/CMakeLists.txt +++ b/noether_gui/CMakeLists.txt @@ -45,7 +45,9 @@ qt5_wrap_cpp(${PROJECT_NAME}_widget_mocs include/${PROJECT_NAME}/widgets/tool_path_modifiers/linear_approach_modifier_widget.h include/${PROJECT_NAME}/widgets/tool_path_modifiers/linear_departure_modifier_widget.h include/${PROJECT_NAME}/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.h - include/${PROJECT_NAME}/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h) + include/${PROJECT_NAME}/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h + # Mesh Modifiers + include/${PROJECT_NAME}/widgets/mesh_modifiers/plane_projection_modifier_widget.h) qt5_wrap_ui(${PROJECT_NAME}_widget_ui_mocs ui/vector3d_editor_widget.ui @@ -90,6 +92,8 @@ add_library(${PROJECT_NAME} SHARED src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp src/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.cpp src/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.cpp + # Mesh Modifiers + src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp ${${PROJECT_NAME}_widget_mocs} ${${PROJECT_NAME}_widget_ui_mocs} ) diff --git a/noether_gui/include/noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h b/noether_gui/include/noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h new file mode 100644 index 00000000..4134e79d --- /dev/null +++ b/noether_gui/include/noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +class QDoubleSpinBox; + +namespace noether +{ +class PlaneProjectionMeshModifierWidget : public MeshModifierWidget +{ + Q_OBJECT +public: + PlaneProjectionMeshModifierWidget(QWidget* parent = nullptr); + + MeshModifier::ConstPtr create() const override; + + void configure(const YAML::Node&) override; + void save(YAML::Node&) const override; + +private: + QDoubleSpinBox* distance_threshold_; +}; + +} // namespace noether diff --git a/noether_gui/src/plugins.cpp b/noether_gui/src/plugins.cpp index fdb95589..13bbb097 100644 --- a/noether_gui/src/plugins.cpp +++ b/noether_gui/src/plugins.cpp @@ -24,6 +24,8 @@ #include #include #include +// Mesh Modifiers +#include #include #include @@ -115,6 +117,9 @@ struct PlaneSlicerRasterPlannerWidgetPlugin : ToolPathPlannerWidgetPlugin } }; +// Mesh Modifiers +using PlaneProjectionMeshModifierWidgetPlugin = WidgetPluginImpl; + } // namespace noether EXPORT_DIRECTION_GENERATOR_WIDGET_PLUGIN(noether::FixedDirectionGeneratorWidgetPlugin, FixedDirectionGenerator) @@ -143,3 +148,5 @@ EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearApproachToolPathModifierW EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearDepartureToolPathModifierWidgetPlugin, LinearDepartureModifier) EXPORT_TPP_WIDGET_PLUGIN(noether::PlaneSlicerRasterPlannerWidgetPlugin, PlaneSlicerRasterPlanner) + +EXPORT_MESH_MODIFIER_WIDGET_PLUGIN(noether::PlaneProjectionMeshModifierWidgetPlugin, PlaneProjectionModifier) diff --git a/noether_gui/src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp b/noether_gui/src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp new file mode 100644 index 00000000..d0889dcb --- /dev/null +++ b/noether_gui/src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp @@ -0,0 +1,40 @@ +#include +#include + +#include +#include +#include +#include + +static std::string DIST_THRESH_KEY = "distance_threshold"; + +namespace noether +{ +PlaneProjectionMeshModifierWidget::PlaneProjectionMeshModifierWidget(QWidget* parent) + : MeshModifierWidget(parent), distance_threshold_(new QDoubleSpinBox(this)) +{ + distance_threshold_->setDecimals(3); + distance_threshold_->setMinimum(0.0); + distance_threshold_->setSingleStep(0.010); + distance_threshold_->setValue(0.010); + + auto layout = new QFormLayout(this); + layout->addRow(new QLabel("Distance threshold (m)", this), distance_threshold_); +} + +MeshModifier::ConstPtr PlaneProjectionMeshModifierWidget::create() const +{ + return std::make_unique(distance_threshold_->value()); +} + +void PlaneProjectionMeshModifierWidget::configure(const YAML::Node& node) +{ + distance_threshold_->setValue(getEntry(node, DIST_THRESH_KEY)); +} + +void PlaneProjectionMeshModifierWidget::save(YAML::Node& node) const +{ + node[DIST_THRESH_KEY] = distance_threshold_->value(); +} + +} // namespace noether diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 86774bb5..6900a058 100644 --- a/noether_tpp/CMakeLists.txt +++ b/noether_tpp/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED src/mesh_modifiers/euclidean_clustering_modifier.cpp src/mesh_modifiers/fill_holes_modifier.cpp src/mesh_modifiers/windowed_sinc_smoothing.cpp + src/mesh_modifiers/plane_projection_modifier.cpp # Tool Path Modifiers src/tool_path_modifiers/circular_lead_in_modifier.cpp src/tool_path_modifiers/circular_lead_out_modifier.cpp diff --git a/noether_tpp/include/noether_tpp/mesh_modifiers/plane_projection_modifier.h b/noether_tpp/include/noether_tpp/mesh_modifiers/plane_projection_modifier.h new file mode 100644 index 00000000..3ebc38c3 --- /dev/null +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/plane_projection_modifier.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +namespace noether +{ +class PlaneProjectionMeshModifier : public MeshModifier +{ +public: + PlaneProjectionMeshModifier(double distance_threshold); + + std::vector modify(const pcl::PolygonMesh& mesh) const override; + +protected: + double distance_threshold_; +}; + +} // namespace noether diff --git a/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp b/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp new file mode 100644 index 00000000..c722be50 --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp @@ -0,0 +1,110 @@ +#include +#include + +#include +#include + +#include + +namespace noether +{ +std::vector::const_iterator findField(const std::vector& fields, + const std::string& name) +{ + return std::find_if( + fields.begin(), fields.end(), [&name](const pcl::PCLPointField& field) { return field.name == name; }); +} + +std::vector::const_iterator findFieldOrThrow(const std::vector& fields, + const std::string& name) +{ + auto it = findField(fields, name); + if (it == fields.end()) + throw std::runtime_error("Failed to find field '" + name + "'"); + return it; +} + +void projectInPlace(pcl::PCLPointCloud2& cloud, const Eigen::Vector4f& plane_coeffs) +{ + // Find the x, y, and z fields + auto x_it = findFieldOrThrow(cloud.fields, "x"); + auto y_it = findFieldOrThrow(cloud.fields, "y"); + auto z_it = findFieldOrThrow(cloud.fields, "z"); + + auto nx_it = findField(cloud.fields, "normal_x"); + auto ny_it = findField(cloud.fields, "normal_y"); + auto nz_it = findField(cloud.fields, "normal_z"); + + // Check that the xyz fields are floats and contiguous + if ((y_it->offset - x_it->offset != 4) || (z_it->offset - y_it->offset != 4)) + throw std::runtime_error("XYZ fields are not contiguous floats"); + + bool update_normals = nx_it != cloud.fields.end() && ny_it != cloud.fields.end() && nz_it != cloud.fields.end(); + + for (std::size_t r = 0; r < cloud.height; ++r) + { + for (std::size_t c = 0; c < cloud.width; ++c) + { + auto offset = r * cloud.row_step + c * cloud.point_step; + float* xyz = reinterpret_cast(cloud.data.data() + offset + x_it->offset); + Eigen::Map pt(xyz); + + // Project the point + float d = plane_coeffs.dot(pt); + pt.head<3>() -= d * plane_coeffs.head<3>(); + + if (update_normals) + { + float* nx = reinterpret_cast(cloud.data.data() + offset + nx_it->offset); + *nx = plane_coeffs[0]; + + float* ny = reinterpret_cast(cloud.data.data() + offset + ny_it->offset); + *ny = plane_coeffs[1]; + + float* nz = reinterpret_cast(cloud.data.data() + offset + nz_it->offset); + *nz = plane_coeffs[2]; + } + } + } +} + +PlaneProjectionMeshModifier::PlaneProjectionMeshModifier(double distance_threshold) + : MeshModifier(), distance_threshold_(distance_threshold) +{ +} + +std::vector PlaneProjectionMeshModifier::modify(const pcl::PolygonMesh& mesh) const +{ + auto cloud = pcl::make_shared>(); + pcl::fromPCLPointCloud2(mesh.cloud, *cloud); + + // Fit a plane model to the vertices using RANSAC + auto model = pcl::make_shared>(cloud); + auto ransac = pcl::make_shared>(model); + ransac->setDistanceThreshold(distance_threshold_); + ransac->computeModel(); + + // Extract the inliers + std::vector inliers; + ransac->getInliers(inliers); + + // Get the optimized model coefficients + Eigen::VectorXf plane_coeffs(4); + ransac->getModelCoefficients(plane_coeffs); + + // Project mesh origin on to plane + float d = plane_coeffs.dot(Eigen::Vector4f(0.0, 0.0, 0.0, 1.0)); + if (d > 0.0) + plane_coeffs *= -1; + // Eigen::Vector3f projected_origin = origin.head<3>() - d * plane_coeffs.head<3>(); + + // Extract the inlier submesh + pcl::PolygonMesh output_mesh = extractSubMeshFromInlierVertices(mesh, inliers); + + // Project the inlier vertices onto the plane + projectInPlace(output_mesh.cloud, plane_coeffs); + + return { output_mesh }; +} + +} // namespace noether