From ffd8182428291a2e4ed71bfcd0cbe7fb5173b95f Mon Sep 17 00:00:00 2001
From: Michael Ripperger <michael.ripperger@swri.org>
Date: Tue, 8 Oct 2024 14:19:11 -0500
Subject: [PATCH] Minor updates (#273)

* Added check to ransac model fit

* Added check to half edge index validity

* Added check to validity of vertex indices in computation of normal

* Added additional checks to normals from mesh faces modifier

* Updated PCL normal estimation mesh modifier widget
---
 .../normal_estimation_pcl_widget.cpp          |  5 ++-
 .../normals_from_mesh_faces_modifier.cpp      | 32 +++++++++++++++----
 .../plane_projection_modifier.cpp             |  3 +-
 .../edge/boundary_edge_planner.cpp            |  6 +++-
 4 files changed, 36 insertions(+), 10 deletions(-)

diff --git a/noether_gui/src/widgets/mesh_modifiers/normal_estimation_pcl_widget.cpp b/noether_gui/src/widgets/mesh_modifiers/normal_estimation_pcl_widget.cpp
index 101c08e3..6c196595 100644
--- a/noether_gui/src/widgets/mesh_modifiers/normal_estimation_pcl_widget.cpp
+++ b/noether_gui/src/widgets/mesh_modifiers/normal_estimation_pcl_widget.cpp
@@ -15,8 +15,9 @@ NormalEstimationPCLMeshModifierWidget::NormalEstimationPCLMeshModifierWidget(QWi
 {
   auto* layout = new QVBoxLayout(this);
 
+  // Create a form layout with the normals radius parameter
   {
-    auto* form_layout = new QFormLayout(this);
+    auto* form_layout = new QFormLayout();
 
     radius_->setMinimum(0.0);
     radius_->setValue(0.010);
@@ -30,6 +31,8 @@ NormalEstimationPCLMeshModifierWidget::NormalEstimationPCLMeshModifierWidget(QWi
   // Set up the Vector3d editor
   auto* widget = new QWidget(this);
   view_point_->setupUi(widget);
+  view_point_->group_box->setTitle("View Point");
+
   layout->addWidget(widget);
 }
 
diff --git a/noether_tpp/src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp b/noether_tpp/src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp
index dfdd8e77..eeaf45d8 100644
--- a/noether_tpp/src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp
+++ b/noether_tpp/src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp
@@ -15,9 +15,18 @@ Eigen::Vector3f computeFaceNormal(const pcl::PolygonMesh& mesh,
 
   // Get the vertices of this triangle
   VAFC v_circ = tri_mesh.getVertexAroundFaceCirculator(face_idx);
-  const Eigen::Vector3f v1 = getPoint(mesh.cloud, v_circ++.getTargetIndex().get());
-  const Eigen::Vector3f v2 = getPoint(mesh.cloud, v_circ++.getTargetIndex().get());
-  const Eigen::Vector3f v3 = getPoint(mesh.cloud, v_circ++.getTargetIndex().get());
+
+  const TriangleMesh::VertexIndex v1_idx = v_circ++.getTargetIndex();
+  const TriangleMesh::VertexIndex v2_idx = v_circ++.getTargetIndex();
+  const TriangleMesh::VertexIndex v3_idx = v_circ++.getTargetIndex();
+
+  // Check the validity of the vertices
+  if (!v1_idx.isValid() || !v2_idx.isValid() || !v3_idx.isValid())
+    return Eigen::Vector3f::Constant(std::numeric_limits<float>::quiet_NaN());
+
+  const Eigen::Vector3f v1 = getPoint(mesh.cloud, v1_idx.get());
+  const Eigen::Vector3f v2 = getPoint(mesh.cloud, v2_idx.get());
+  const Eigen::Vector3f v3 = getPoint(mesh.cloud, v3_idx.get());
 
   // Get the edges v1 -> v2 and v1 -> v3
   const Eigen::Vector3f edge_12 = v2 - v1;
@@ -43,14 +52,23 @@ std::vector<pcl::PolygonMesh> NormalsFromMeshFacesMeshModifier::modify(const pcl
 
     using FAVC = TriangleMesh::FaceAroundVertexCirculator;
     FAVC circ = tri_mesh.getFaceAroundVertexCirculator(TriangleMesh::VertexIndex(i));
+
     FAVC circ_end = circ;
     do
     {
-      TriangleMesh::FaceIndex face_idx = circ.getTargetIndex();
-      if (face_idx.isValid())
+      if (n_faces > mesh.polygons.size())
+        throw std::runtime_error("Vertex " + std::to_string(i) +
+                                 " appears to participate in more faces than exist in the mesh; this mesh likely "
+                                 "contains degenerate half-edges.");
+
+      if (circ.isValid())
       {
-        avg_face_normal += computeFaceNormal(mesh, tri_mesh, face_idx);
-        ++n_faces;
+        TriangleMesh::FaceIndex face_idx = circ.getTargetIndex();
+        if (face_idx.isValid())
+        {
+          avg_face_normal += computeFaceNormal(mesh, tri_mesh, face_idx);
+          ++n_faces;
+        }
       }
     } while (++circ != circ_end);
 
diff --git a/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp b/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp
index 1bdf1ed7..2b2f4f9c 100644
--- a/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp
+++ b/noether_tpp/src/mesh_modifiers/plane_projection_modifier.cpp
@@ -85,7 +85,8 @@ std::vector<pcl::PolygonMesh> PlaneProjectionMeshModifier::modify(const pcl::Pol
   {
     // Fit a plane model to the vertices using RANSAC
     model->setIndices(remaining_indices);
-    ransac->computeModel();
+    if (!ransac->computeModel())
+      break;
 
     // Extract the inliers
     std::vector<int> inliers;
diff --git a/noether_tpp/src/tool_path_planners/edge/boundary_edge_planner.cpp b/noether_tpp/src/tool_path_planners/edge/boundary_edge_planner.cpp
index 2bed2312..ed7e2304 100644
--- a/noether_tpp/src/tool_path_planners/edge/boundary_edge_planner.cpp
+++ b/noether_tpp/src/tool_path_planners/edge/boundary_edge_planner.cpp
@@ -31,7 +31,11 @@ std::vector<typename MeshT::HalfEdgeIndices> getBoundaryHalfEdges(const MeshT& m
       IHEAFC circ_end = circ;
       do
       {
-        visited[pcl::geometry::toEdgeIndex(circ.getTargetIndex()).get()] = true;
+        HalfEdgeIndex he_idx = circ.getTargetIndex();
+        if (!he_idx.isValid())
+          break;
+
+        visited[pcl::geometry::toEdgeIndex(he_idx).get()] = true;
         boundary_he.push_back(circ.getTargetIndex());
       } while (++circ != circ_end);