Refactor NormalsFromMeshFacesModifier (#334)

* Refactor NormalsFromMeshFacesModifier

* Apply Clang Format

* Simplified; used existing methods for extracting face normals

---------

Co-authored-by: Michael Ripperger <[email protected]>

Author: DavidMerzJr

noether_tpp/include/noether_tpp/mesh_modifiers/normals_from_mesh_faces_modifier.h

@@ -8,7 +8,11 @@ namespace noether
 /**
  * @ingroup mesh_modifiers
  * @brief MeshModifier that assigns vertex normals using mesh faces
- * @details
+ * @details The normal of each vertex is calculated as the average of the normals of all faces
+ * which include that vertex. This function handles supra-triangular polygonal faces, but ignores
+ * 'faces' that include less than three vertices. Well-behaved on meshes with holes. Note that a
+ * mesh with inconsistent face normals will generate inconsistent vertex normals. Note that a
+ * non-manifold mesh may produce numerically unstable results.
  */
 class NormalsFromMeshFacesMeshModifier : public MeshModifier
 {

noether_tpp/src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp

@@ -1,92 +1,49 @@
 #include <noether_tpp/mesh_modifiers/normals_from_mesh_faces_modifier.h>
 #include <noether_tpp/utils.h>
 
-#include <pcl/point_types.h>
+#include <Eigen/Dense>
 #include <pcl/common/io.h>
 #include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+#include <vector>
 #include <yaml-cpp/yaml.h>
 
 namespace noether
 {
-Eigen::Vector3f computeFaceNormal(const pcl::PolygonMesh& mesh,
-                                  const TriangleMesh& tri_mesh,
-                                  const TriangleMesh::FaceIndex& face_idx)
-{
-  using VAFC = TriangleMesh::VertexAroundFaceCirculator;
-
-  // Get the vertices of this triangle
-  VAFC v_circ = tri_mesh.getVertexAroundFaceCirculator(face_idx);
-
-  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;
-  const Eigen::Vector3f edge_13 = v3 - v1;
-
-  // Compute the face normal as the cross product between edge v1 -> v2 and edge v1 -> v3
-  return edge_12.cross(edge_13).normalized();
-}
-
 std::vector<pcl::PolygonMesh> NormalsFromMeshFacesMeshModifier::modify(const pcl::PolygonMesh& mesh) const
 {
-  // Create a triangle mesh representation of the input mesh
-  TriangleMesh tri_mesh = createTriangleMesh(mesh);
-
-  pcl::PointCloud<pcl::Normal> normals_cloud;
-  normals_cloud.reserve(mesh.cloud.height * mesh.cloud.width);
+  // Create a point cloud for the normals and extract its Eigen matrix mapping
+  pcl::PointCloud<pcl::Normal> normals_cloud(mesh.cloud.width, mesh.cloud.height);
+  auto normals_map = normals_cloud.getMatrixXfMap();
 
-  // For each vertex, circulate the faces around and average the face normals
-  for (std::size_t i = 0; i < tri_mesh.getVertexDataCloud().size(); ++i)
+  // Iterate across the polygons, calculating their normals and accumulating the normals onto all adjacent vertices
+  for (const auto& p : mesh.polygons)
   {
-    Eigen::Vector3f avg_face_normal = Eigen::Vector3f::Zero();
-    int n_faces = 0;
+    // Compute the normal
+    const Eigen::Vector3f normal = getFaceNormal(mesh, p);
 
-    using FAVC = TriangleMesh::FaceAroundVertexCirculator;
-    FAVC circ = tri_mesh.getFaceAroundVertexCirculator(TriangleMesh::VertexIndex(i));
-
-    FAVC circ_end = circ;
-    do
-    {
-      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.");
+    // Add this normal to the accumulators for this face's valid vertices
+    for (const std::size_t v_idx : p.vertices)
+      normals_map.col(v_idx).head<3>() += normal;
+  }
 
-      if (circ.isValid())
-      {
-        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);
+  // Normalize the normals
+  normals_map.colwise().normalize();
 
-    pcl::Normal normal;
-    normal.getNormalVector3fMap() = (avg_face_normal / n_faces).normalized();
-    normals_cloud.push_back(normal);
-  }
+  // Replace nan values with 0
+  normals_map.unaryExpr([](float v) { return std::isfinite(v) ? v : 0.0; });
 
   // Convert to message type
-  pcl::PCLPointCloud2 normals;
-  pcl::toPCLPointCloud2(normals_cloud, normals);
+  pcl::PCLPointCloud2 normals_pc2;
+  pcl::toPCLPointCloud2(normals_cloud, normals_pc2);
 
-  // Copy the mesh
+  // Create output mesh
   pcl::PolygonMesh output = mesh;
 
   // Concatenate the normals with the nominal mesh information
-  if (!pcl::concatenateFields(mesh.cloud, normals, output.cloud))
+  if (!pcl::concatenateFields(mesh.cloud, normals_pc2, output.cloud))
     throw std::runtime_error("Failed to concatenate normals into mesh vertex cloud");
 
   return { output };