CAP: Association functions implemented, defensive checks added, and general cleanup

Author: DamienGilliard

src/diffCheck/segmentation/DFSegmentation.cc

@@ -3,6 +3,7 @@
 #include <cilantro/utilities/point_cloud.hpp>
 #include <cilantro/core/nearest_neighbors.hpp>
 #include <cilantro/clustering/connected_component_extraction.hpp>
+#include <cmath>
 
 namespace diffCheck::segmentation
 {   
@@ -101,7 +102,6 @@ namespace diffCheck::segmentation
         double associationThreshold)
     {
         std::shared_ptr<geometry::DFPointCloud> unifiedPointCloud = std::make_shared<geometry::DFPointCloud>();
-        std::vector<std::shared_ptr<geometry::DFPointCloud>> segmentsRemainder;
 
         // iterate through the mesh faces given as function argument
         if (referenceMesh.size() == 0)
@@ -139,7 +139,7 @@ namespace diffCheck::segmentation
 
                 double currentDistance = (faceCenter - segmentCenter).norm() / std::abs(segmentNormal.dot(faceNormal));
                 // if the distance is smaller than the previous one, update the distance and the corresponding segment
-                if (std::abs(faceNormal.dot(segmentNormal)) > angleThreshold  && currentDistance < faceDistance)
+                if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold  && currentDistance < faceDistance)
                 {
                     correspondingSegment = segment;
                     faceDistance = currentDistance;
@@ -231,9 +231,8 @@ namespace diffCheck::segmentation
             }
             clusterNormal.normalize();
 
-            std::shared_ptr<diffCheck::geometry::DFMesh> testMesh;
-            int meshIndex;
-            int faceIndex ;
+            int meshIndex = std::numeric_limits<int>::max();
+            int faceIndex = std::numeric_limits<int>::max() ;
             double distance = std::numeric_limits<double>::max();
 
             if (meshes.size() == 0)
@@ -270,17 +269,21 @@ namespace diffCheck::segmentation
 
                     double clusterNormalToJunctionLineAngle = std::abs(std::acos(clusterNormal.dot((clusterCenter - faceCenter).normalized())));
 
-                    double currentDistance = (clusterCenter - faceCenter).norm()   * std::pow(std::cos(clusterNormalToJunctionLineAngle), 2) / std::pow(clusterNormal.dot(faceNormal), 2);
-                    if (std::abs(faceNormal.dot(clusterNormal)) > angleThreshold && currentDistance < distance)
+                    double currentDistance = (clusterCenter - faceCenter).norm() * std::pow(std::cos(clusterNormalToJunctionLineAngle), 2) / std::pow(clusterNormal.dot(faceNormal), 2);
+                    if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold && currentDistance < distance)
                     {
                         distance = currentDistance;
                         meshIndex = std::distance(meshes.begin(), std::find(meshes.begin(), meshes.end(), piece));
                         faceIndex = std::distance(piece.begin(), std::find(piece.begin(), piece.end(), meshFace));
-                        testMesh = meshFace;
                     }
                 }
             }
-
+            if (meshIndex >= meshes.size() || faceIndex >= meshes[meshIndex].size())
+            {
+                // this one generates a lot of warnings
+                DIFFCHECK_WARN("No mesh face found for the cluster. Skipping the cluster.");
+                continue;
+            }
             std::shared_ptr<geometry::DFPointCloud> completed_segment = existingPointCloudSegments[meshIndex];
             for (Eigen::Vector3d point : cluster->Points)
             {

src/diffCheck/segmentation/DFSegmentation.hh

@@ -29,28 +29,28 @@ namespace diffCheck::segmentation
         /** @brief Associates point cloud segments to mesh faces and merges them. It uses the center of mass of the segments and the mesh faces to find correspondances. For each mesh face it then iteratively associate the points of the segment that are actually on the mesh face.
          * @param referenceMesh the vector of mesh faces to associate with the segments
          * @param clusters the vector of clusters from cilantro to associate with the mesh faces of the reference mesh
-         * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum cosine of the angles. A value of 0.99 requires perfect alignment, while a value of 0.8 is more permissive. A value of 0 allows any angle between the normal of the cluster and the normal of the mesh face.
+         * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum sine of the angles. A value of 0 requires perfect alignment (angle = 0), while a value of 0.1 allows an angle of 5.7 degrees.
          * @param associationThreshold the threshold to consider the points of a segment and a mesh face as associable. It is the ratio between the surface of the closest mesh triangle and the sum of the areas of the three triangles that form the rest of the pyramid described by the mesh triangle and the point we want to associate or not. The lower the number, the more strict the association will be and some poinnts on the mesh face might be wrongfully excluded.
          * @return std::shared_ptr<geometry::DFPointCloud> The unified segments
          */
         static std::shared_ptr<geometry::DFPointCloud> DFSegmentation::AssociateClustersToMeshes(
             std::vector<std::shared_ptr<geometry::DFMesh>> referenceMesh,
             std::vector<std::shared_ptr<geometry::DFPointCloud>> &clusters,
-            double angleThreshold = 0.95,
+            double angleThreshold = 0.1,
             double associationThreshold = 0.1);
 
         /** @brief Iterated through clusters and finds the corresponding mesh face. It then associates the points of the cluster that are on the mesh face to the segment already associated with the mesh face.
          * @param unassociatedClusters the clusters from the normal-based segmentatinon that haven't been associated yet.
          * @param existingPointCloudSegments the already associated segments
          * @param Meshes the mesh faces for all the model. This is used to associate the clusters to the mesh faces.
-         * * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum cosine of the angles. A value of 0.99 requires perfect alignment, while a value of 0.8 is more permissive. A value of 0 allows any angle between the normal of the cluster and the normal of the mesh face.
+         * * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum sine of the angles. A value of 0 requires perfect alignment (angle = 0), while a value of 0.1 allows an angle of 5.7 degrees. 
          * @param associationThreshold the threshold to consider the points of a segment and a mesh face as associable. It is the ratio between the surface of the closest mesh triangle and the sum of the areas of the three triangles that form the rest of the pyramid described by the mesh triangle and the point we want to associate or not. The lower the number, the more strict the association will be and some poinnts on the mesh face might be wrongfully excluded.   
          */
         static void DFSegmentation::CleanUnassociatedClusters(
             std::vector<std::shared_ptr<geometry::DFPointCloud>> &unassociatedClusters,
             std::vector<std::shared_ptr<geometry::DFPointCloud>> &existingPointCloudSegments,
             std::vector<std::vector<std::shared_ptr<geometry::DFMesh>>> Meshes,
-            double angleThreshold = 0.95,
+            double angleThreshold = 0.1,
             double associationThreshold = 0.1);
 
         private: ///< helper methods

src/diffCheckApp.cc

@@ -16,36 +16,47 @@ int main()
   std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>> segments;
   std::vector<std::string> meshPaths;
 
-  std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\again_other_meshes_for_diffCheck\6\)";
+  std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\9\)";
 
-  for (int i = 1; i <= 4; i++)
+  for (int i = 1; i <= 7; i++)
   {
     std::string meshPath = meshesFolderPath + std::to_string(i) + ".ply";
     std::shared_ptr<diffCheck::geometry::DFMesh> mesh = std::make_shared<diffCheck::geometry::DFMesh>();
     mesh->LoadFromPLY(meshPath);
     meshSrc.push_back(mesh);
   }
 
-  std::string pathPcdSrc = R"(C:\Users\localuser\Desktop\again_other_meshes_for_diffCheck\source_pc.ply)";
+  std::string pathPcdSrc = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\source_pc_2.ply)";
 
   pcdSrc->LoadFromPLY(pathPcdSrc);
 
-  pcdSrc->EstimateNormals(false, 40);
+  pcdSrc->EstimateNormals(false, 100);
+  pcdSrc->VoxelDownsample(0.01);
   auto intermediateTime = std::chrono::high_resolution_clock::now();
   segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(
     pcdSrc,
-    2.0f,
-    200,
+    6.0f,
+    25,
     true,
-    100,
+    20,
     0.5f,
     false);
   std::cout << "number of segments:" << segments.size()<< std::endl;
 
   std::shared_ptr<diffCheck::geometry::DFPointCloud> unifiedSegments = 
-    diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes(meshSrc, segments, .15);
+    diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes(
+      meshSrc, 
+      segments, 
+      .1, 
+      .9);
 
-  diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters(segments, std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>{unifiedSegments}, std::vector<std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>>{meshSrc}, .15);
+  std::cout << "Association done. refinement in progress" << std::endl;
+
+  diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters(segments, 
+    std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>{unifiedSegments}, 
+    std::vector<std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>>{meshSrc},
+    .1, 
+    .9);
 
   std::cout << "number of points in unified segments:" << unifiedSegments->Points.size() << std::endl;
 
@@ -67,7 +78,7 @@ int main()
   }
   for(auto mesh : meshSrc)
   {
-    vis.AddMesh(mesh);
+    //vis.AddMesh(mesh);
   }
 
   for (int i = 0; i < unifiedSegments->Points.size(); i++)

src/diffCheckBindings.cc

@@ -182,10 +182,12 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
         .def_static("associate_clusters", &diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes,
             py::arg("reference_mesh"),
             py::arg("clusters"),
+            py::arg("angle_threshold") = 0.95,
             py::arg("association_threshold") = 0.1)
         .def_static("clean_unassociated_clusters", &diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters,
             py::arg("unassociated_clusters"),
             py::arg("existing_point_cloud_segments"),
             py::arg("meshes"),
+            py::arg("angle_threshold") = 0.95,
             py::arg("association_threshold") = 0.1);
 }