UPDATE-WIP: add IsPointOnFace function, and angle threshold parameter for cluster to mesh face association

Author: DamienGilliard

src/diffCheck/segmentation/DFSegmentation.cc

@@ -97,6 +97,7 @@ namespace diffCheck::segmentation
     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,
         double associationThreshold)
     {
         std::shared_ptr<geometry::DFPointCloud> unifiedPointCloud = std::make_shared<geometry::DFPointCloud>();
@@ -111,12 +112,12 @@ namespace diffCheck::segmentation
 
             // Getting the center of the mesh face
             Eigen::Vector3d faceCenter;
-            // open3d::geometry::OrientedBoundingBox orientedBoundingBox = o3dFace->GetMinimalOrientedBoundingBox();
             faceCenter = o3dFace->GetCenter();
 
             // Getting the normal of the mesh face
 
             Eigen::Vector3d faceNormal = face->GetFirstNormal();
+            faceNormal.normalize();
 
             // iterate through the segments to find the closest ones compared to the face center taking the normals into account
             Eigen::Vector3d segmentCenter;
@@ -138,54 +139,20 @@ 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 (currentDistance < faceDistance)
+                if (std::abs(faceNormal.dot(segmentNormal)) > angleThreshold  && currentDistance < faceDistance)
                 {
                     correspondingSegment = segment;
                     faceDistance = currentDistance;
                 }
             }
 
             // get the triangles of the face.
-            std::vector<Eigen::Vector3i> faceTriangles = o3dFace->triangles_;
+            std::vector<Eigen::Vector3i> faceTriangles = face->Faces;
 
             for (Eigen::Vector3d point : correspondingSegment->Points)
             {
                 bool pointInFace = false;
-                /*
-                To check if the point is in the face, we take into account all the triangles forming the face.
-                We calculate the area of each triangle, then check if the sum of the areas of the tree triangles 
-                formed by two of the points of the referencr triangle and our point is equal to the reference triangle area 
-                (within a user-defined margin). If it is the case, the triangle is in the face.
-                */
-                for (Eigen::Vector3i triangle : faceTriangles)
-                {
-                    // reference triangle
-                    Eigen::Vector3d v0 = o3dFace->vertices_[triangle[0]];
-                    Eigen::Vector3d v1 = o3dFace->vertices_[triangle[1]];
-                    Eigen::Vector3d v2 = o3dFace->vertices_[triangle[2]];
-                    Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
-                    double referenceTriangleArea = n.norm()*0.5;
-                    
-                    // triangle 1
-                    Eigen::Vector3d n1 = (v1 - v0).cross(point - v0);
-                    double area1 = n1.norm()*0.5;
-
-                    // triangle 2
-                    Eigen::Vector3d n2 = (v2 - v1).cross(point - v1);
-                    double area2 = n2.norm()*0.5;
-
-                    // triangle 3
-                    Eigen::Vector3d n3 = (v0 - v2).cross(point - v2);
-                    double area3 = n3.norm()*0.5;
-
-                    double res = ( area1 + area2 + area3 - referenceTriangleArea) / referenceTriangleArea;
-                    if (res < associationThreshold)
-                    {
-                        pointInFace = true;
-                        break;
-                    }
-                }
-                if (pointInFace)
+                if (DFSegmentation::IsPointOnFace(face, point, associationThreshold))
                 {
                     unifiedPointCloud->Points.push_back(point);
                     unifiedPointCloud->Normals.push_back(
@@ -224,6 +191,7 @@ namespace diffCheck::segmentation
         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,
         double associationThreshold)
     {
         for (std::shared_ptr<geometry::DFPointCloud> cluster : unassociatedClusters)
@@ -239,7 +207,7 @@ namespace diffCheck::segmentation
             {
                 clusterNormal += normal;
             }
-            clusterNormal /= cluster->GetNumPoints();
+            clusterNormal.normalize();
 
             std::shared_ptr<diffCheck::geometry::DFMesh> testMesh;
             int meshIndex;
@@ -254,9 +222,8 @@ namespace diffCheck::segmentation
 
                     std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace = meshFace->Cvt2O3DTriangleMesh();
 
-                    Eigen::Vector3d faceNormal = meshFace->GetFirstNormal();
-
-                    // std::shared_ptr<open3d::geometry::OrientedBoundingBox> orientedBoundingBox(new open3d::geometry::OrientedBoundingBox(o3dFace->GetMinimalOrientedBoundingBox()));
+                    faceNormal = meshFace->GetFirstNormal();
+                    faceNormal.normalize();
                     faceCenter = o3dFace->GetCenter();
                     /*
                     To make sure we select the right meshFace, we add another metric:
@@ -265,13 +232,13 @@ namespace diffCheck::segmentation
                     To prevent this, we take into the account the angle between the line linking the center of the meshFace considered 
                     and the center of the point cloud cluster and the normal of the cluster. This value should be close to pi/2
 
-                    The following two lines are not super optimized but more readable thatn the optimized version
+                    The following two lines are not super optimized but more readable than the optimized version
                     */
 
                     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 (currentDistance < distance)
+                    if (std::abs(faceNormal.dot(clusterNormal)) > angleThreshold && currentDistance < distance)
                     {
                         distance = currentDistance;
                         meshIndex = std::distance(meshes.begin(), std::find(meshes.begin(), meshes.end(), piece));
@@ -280,51 +247,26 @@ namespace diffCheck::segmentation
                     }
                 }
             }
-            // diffCheck::visualizer::Visualizer vis;
-            // vis.AddPointCloud(cluster);
-            // vis.AddMesh(meshes[meshIndex][faceIndex]);
-            // vis.Run();
+
             std::shared_ptr<geometry::DFPointCloud> completed_segment = existingPointCloudSegments[meshIndex];
             for (Eigen::Vector3d point : cluster->Points)
             {
-                bool pointInFace = false;
-                std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace = meshes[meshIndex][faceIndex]->Cvt2O3DTriangleMesh();
-                std::vector<Eigen::Vector3i> faceTriangles = o3dFace->triangles_;
-                for (Eigen::Vector3i triangle : faceTriangles)
-                {
-                    Eigen::Vector3d v0 = o3dFace->vertices_[triangle[0]];
-                    Eigen::Vector3d v1 = o3dFace->vertices_[triangle[1]];
-                    Eigen::Vector3d v2 = o3dFace->vertices_[triangle[2]];
-                    Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
-                    double referenceTriangleArea = n.norm()*0.5;
-                    Eigen::Vector3d n1 = (v1 - v0).cross(point - v0);
-                    double area1 = n1.norm()*0.5;
-                    Eigen::Vector3d n2 = (v2 - v1).cross(point - v1);
-                    double area2 = n2.norm()*0.5;
-                    Eigen::Vector3d n3 = (v0 - v2).cross(point - v2);
-                    double area3 = n3.norm()*0.5;
-                    double res = ( area1 + area2 + area3 - referenceTriangleArea) / referenceTriangleArea;
-                    if (res < associationThreshold)
-                    {
-                        pointInFace = true;
-                        break;
-                    }
-                }
-                if (pointInFace)
+                std::vector<Eigen::Vector3i> faceTriangles = meshes[meshIndex][faceIndex]->Faces;
+                if (IsPointOnFace(meshes[meshIndex][faceIndex], point, associationThreshold))
                 {
                     completed_segment->Points.push_back(point);
                     completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
                 }
             }
             std::vector<int> indicesToRemove;
+            
             for (int i = 0; i < cluster->Points.size(); ++i) 
             {
                 if (std::find(completed_segment->Points.begin(), completed_segment->Points.end(), cluster->Points[i]) != completed_segment->Points.end()) 
                 {
                     indicesToRemove.push_back(i);
                 }
             }
-
             for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it) 
             {
                 std::swap(cluster->Points[*it], cluster->Points.back());
@@ -334,4 +276,39 @@ namespace diffCheck::segmentation
             }
         }
     };
+
+    bool DFSegmentation::IsPointOnFace(
+        std::shared_ptr<diffCheck::geometry::DFMesh> face,
+        Eigen::Vector3d point,
+        double associationThreshold)
+    {
+        /*
+        To check if the point is in the face, we take into account all the triangles forming the face.
+        We calculate the area of each triangle, then check if the sum of the areas of the tree triangles 
+        formed by two of the points of the referencr triangle and our point is equal to the reference triangle area 
+        (within a user-defined margin). If it is the case, the triangle is in the face.
+        */
+        std::vector<Eigen::Vector3i> faceTriangles = face->Faces;
+        for (Eigen::Vector3i triangle : faceTriangles)
+        {
+            Eigen::Vector3d v0 = face->Vertices[triangle[0]];
+            Eigen::Vector3d v1 = face->Vertices[triangle[1]];
+            Eigen::Vector3d v2 = face->Vertices[triangle[2]];
+            Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
+            double referenceTriangleArea = n.norm()*0.5;
+            Eigen::Vector3d n1 = (v1 - v0).cross(point - v0);
+            double area1 = n1.norm()*0.5;
+            Eigen::Vector3d n2 = (v2 - v1).cross(point - v1);
+            double area2 = n2.norm()*0.5;
+            Eigen::Vector3d n3 = (v0 - v2).cross(point - v2);
+            double area3 = n3.norm()*0.5;
+            double res = ( area1 + area2 + area3 - referenceTriangleArea) / referenceTriangleArea;
+            if (res < associationThreshold)
+            {
+                return true;
+                break;
+            }
+        }
+        return false;
+    }
 } // namespace diffCheck::segmentation

src/diffCheck/segmentation/DFSegmentation.hh

@@ -29,24 +29,39 @@ 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 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 associationThreshold);
+            double angleThreshold = 0.95,
+            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 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 associationThreshold = 0.1);
+        
+        private: ///< helper methods
+        /** @brief private  method to check if a point is on a face of a triangle mesh triangle, within a certain  association threshold. This takes into account the fact that, in 3D, a point can be "above" a triangle of a triangle mesh but still considered as being on the mesh face.
+         * @param face the triangle mesh face to check the point against
+         * @param point the point to check
+         * @param associationThreshold the threshold to consider the point associable to the mesh. 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 bool DFSegmentation::IsPointOnFace(
+            std::shared_ptr<diffCheck::geometry::DFMesh> face,
+            Eigen::Vector3d point,
             double associationThreshold);
     };
 } // namespace diffCheck::segmentation