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

@@ -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}
 )

noether_gui/include/noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <noether_gui/widgets.h>
+#include <noether_tpp/core/mesh_modifier.h>
+
+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

noether_gui/src/plugins.cpp

@@ -24,6 +24,8 @@
 #include <noether_gui/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h>
+// Mesh Modifiers
+#include <noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h>
 
 #include <QWidget>
 #include <QMessageBox>
@@ -115,6 +117,9 @@ struct PlaneSlicerRasterPlannerWidgetPlugin : ToolPathPlannerWidgetPlugin
   }
 };
 
+// Mesh Modifiers
+using PlaneProjectionMeshModifierWidgetPlugin = WidgetPluginImpl<PlaneProjectionMeshModifierWidget, MeshModifierWidget>;
+
 }  // 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)

noether_gui/src/widgets/mesh_modifiers/plane_projection_modifier_widget.cpp

@@ -0,0 +1,40 @@
+#include <noether_gui/widgets/mesh_modifiers/plane_projection_modifier_widget.h>
+#include <noether_gui/utils.h>
+
+#include <noether_tpp/mesh_modifiers/plane_projection_modifier.h>
+#include <QDoubleSpinBox>
+#include <QFormLayout>
+#include <QLabel>
+
+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<PlaneProjectionMeshModifier>(distance_threshold_->value());
+}
+
+void PlaneProjectionMeshModifierWidget::configure(const YAML::Node& node)
+{
+  distance_threshold_->setValue(getEntry<double>(node, DIST_THRESH_KEY));
+}
+
+void PlaneProjectionMeshModifierWidget::save(YAML::Node& node) const
+{
+  node[DIST_THRESH_KEY] = distance_threshold_->value();
+}
+
+}  // namespace noether

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

noether_tpp/include/noether_tpp/mesh_modifiers/plane_projection_modifier.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <noether_tpp/core/mesh_modifier.h>
+
+namespace noether
+{
+class PlaneProjectionMeshModifier : public MeshModifier
+{
+public:
+  PlaneProjectionMeshModifier(double distance_threshold);
+
+  std::vector<pcl::PolygonMesh> modify(const pcl::PolygonMesh& mesh) const override;
+
+protected:
+  double distance_threshold_;
+};
+
+}  // namespace noether

noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp

@@ -0,0 +1,110 @@
+#include <noether_tpp/mesh_modifiers/plane_projection_modifier.h>
+#include <noether_tpp/mesh_modifiers/subset_extraction/subset_extractor.h>
+
+#include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/sample_consensus/ransac.h>
+
+#include <pcl/conversions.h>
+
+namespace noether
+{
+std::vector<pcl::PCLPointField>::const_iterator findField(const std::vector<pcl::PCLPointField>& fields,
+                                                          const std::string& name)
+{
+  return std::find_if(
+      fields.begin(), fields.end(), [&name](const pcl::PCLPointField& field) { return field.name == name; });
+}
+
+std::vector<pcl::PCLPointField>::const_iterator findFieldOrThrow(const std::vector<pcl::PCLPointField>& 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<float*>(cloud.data.data() + offset + x_it->offset);
+      Eigen::Map<Eigen::Vector4f> 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<float*>(cloud.data.data() + offset + nx_it->offset);
+        *nx = plane_coeffs[0];
+
+        float* ny = reinterpret_cast<float*>(cloud.data.data() + offset + ny_it->offset);
+        *ny = plane_coeffs[1];
+
+        float* nz = reinterpret_cast<float*>(cloud.data.data() + offset + nz_it->offset);
+        *nz = plane_coeffs[2];
+      }
+    }
+  }
+}
+
+PlaneProjectionMeshModifier::PlaneProjectionMeshModifier(double distance_threshold)
+  : MeshModifier(), distance_threshold_(distance_threshold)
+{
+}
+
+std::vector<pcl::PolygonMesh> PlaneProjectionMeshModifier::modify(const pcl::PolygonMesh& mesh) const
+{
+  auto cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
+
+  // Fit a plane model to the vertices using RANSAC
+  auto model = pcl::make_shared<pcl::SampleConsensusModelPlane<pcl::PointXYZ>>(cloud);
+  auto ransac = pcl::make_shared<pcl::RandomSampleConsensus<pcl::PointXYZ>>(model);
+  ransac->setDistanceThreshold(distance_threshold_);
+  ransac->computeModel();
+
+  // Extract the inliers
+  std::vector<int> 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