Merge pull request #174 from diffCheckOrg/feature/improve_CAD_segmentation

Add ICP registration as step of CAD segmentation

Author: eleniv3d

src/diffCheck/segmentation/DFSegmentation.cc

@@ -97,11 +97,12 @@ namespace diffCheck::segmentation
 
     std::vector<std::shared_ptr<geometry::DFPointCloud>> DFSegmentation::AssociateClustersToMeshes(
         bool isCylinder,
+        bool discriminatePoints,
         std::vector<std::shared_ptr<geometry::DFMesh>> referenceMesh,
         std::vector<std::shared_ptr<geometry::DFPointCloud>> &clusters,
         double angleThreshold,
         double associationThreshold,
-        double angleAssociationThreshold)
+        double maximumFaceSegmentDistance)
     {
         std::vector<std::shared_ptr<geometry::DFPointCloud>> faceSegments = std::vector<std::shared_ptr<geometry::DFPointCloud>>();
 
@@ -270,12 +271,12 @@ namespace diffCheck::segmentation
                     for (auto normal : segment->Normals){segmentNormal += normal;}
                     segmentNormal.normalize();
                     double currentDistance = (faceCenter - segmentCenter).norm();
+                    double currentDitanceOrthogonalToFace = std::abs((faceCenter - segmentCenter).dot(faceNormal));
                     double currentAngle = std::abs(sin(acos(faceNormal.dot(faceCenter - segmentCenter))));
-                    // if the distance is smaller than the previous one, update the distance and the corresponding segment
-                    if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold  && currentDistance * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - segmentCenter) / (faceCenter - segmentCenter).norm()))) < faceDistance)
+                    if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold && currentDitanceOrthogonalToFace < maximumFaceSegmentDistance  && currentDitanceOrthogonalToFace < faceDistance)
                     {
                         correspondingSegment = segment;
-                        faceDistance = currentDistance * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - segmentCenter) / (faceCenter - segmentCenter).norm())));
+                        faceDistance = currentDitanceOrthogonalToFace;
                     }
                 }
 
@@ -289,8 +290,32 @@ namespace diffCheck::segmentation
 
                 for (Eigen::Vector3d point : correspondingSegment->Points)
                 {
-                    bool pointInFace = false;
-                    if (face->IsPointOnFace(point, associationThreshold))
+                    if (discriminatePoints)
+                    {
+                        bool pointInFace = false;
+                        if (face->IsPointOnFace(point, associationThreshold))
+                        {
+                            facePoints->Points.push_back(point);
+                            facePoints->Normals.push_back(
+                                correspondingSegment->Normals[std::distance(
+                                    correspondingSegment->Points.begin(), 
+                                    std::find(correspondingSegment->Points.begin(), 
+                                    correspondingSegment->Points.end(), 
+                                    point))]
+                                );
+                            if (hasColors)
+                            {
+                                facePoints->Colors.push_back(
+                                    correspondingSegment->Colors[std::distance(
+                                        correspondingSegment->Points.begin(), 
+                                        std::find(correspondingSegment->Points.begin(), 
+                                        correspondingSegment->Points.end(), 
+                                        point))]
+                                    );
+                            }
+                        }
+                    }
+                    else
                     {
                         facePoints->Points.push_back(point);
                         facePoints->Normals.push_back(
@@ -330,12 +355,13 @@ namespace diffCheck::segmentation
 
     void DFSegmentation::CleanUnassociatedClusters(
         bool isCylinder,
+        bool discriminatePoints,
         std::vector<std::shared_ptr<geometry::DFPointCloud>> &unassociatedClusters,
         std::vector<std::vector<std::shared_ptr<geometry::DFPointCloud>>> &existingPointCloudSegments,
         std::vector<std::vector<std::shared_ptr<geometry::DFMesh>>> meshes,
         double angleThreshold,
         double associationThreshold,
-        double angleAssociationThreshold)
+        double maximumFaceSegmentDistance)
     {
         if (unassociatedClusters.size() == 0)
         {
@@ -443,7 +469,7 @@ namespace diffCheck::segmentation
 
                             double currentDistance = (clusterCenter - faceCenter).norm() * std::abs(std::cos(clusterNormalToJunctionLineAngle))
                             / std::min(std::abs(clusterNormal.dot(faceNormal)), 0.05) ;
-                            if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold  && currentDistance * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance)
+                            if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold && currentDistance < maximumFaceSegmentDistance && currentDistance * (std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance)
                             {
                                 goodMeshIndex = meshIndex;
                                 goodFaceIndex = faceIndex;
@@ -477,12 +503,23 @@ namespace diffCheck::segmentation
                         completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
                     }
                     else
-                        if (correspondingMeshFace->IsPointOnFace(point, associationThreshold))
+                    {
+                        if (discriminatePoints)
+                        {
+                            if (correspondingMeshFace->IsPointOnFace(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))]);
+                                completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
+                            }
+                        }
+                        else
                         {
                             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))]);
                             completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
                         }
+                    }
                 }
                 std::vector<int> indicesToRemove;
 

src/diffCheck/segmentation/DFSegmentation.hh

@@ -28,38 +28,42 @@ namespace diffCheck::segmentation
         public: ///< segmentation refinement methods
         /** @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 isCylinder a boolean to indicate if the model is a cylinder. If true, the method will use the GetCenterAndAxis method of the mesh to find the center and axis of the mesh. based on that, we only want points that have normals more or less perpendicular to the cylinder axis.
+         * @param discriminatePoints a boolean to indicate if we want to discriminate points based on their normal direction when associating clusters to mesh faces. If true, only points that have normals more or less aligned with the face normal will be considered for association.
          * @param referenceMesh the vector of mesh faces to associate with the segments. It is a representation of a beam and its faces.
          * @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 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.
-         * @param angleAssociationThreshold a number to indicate how much distance in the plane of the face should be favored, compared to distance orthogonal to the face normal. If set to 0, any face in the same plane as the face will be considered as having a distance of 0. If set to a high value (e.g. 1000000), no difference will be made between distance in the plane of the face and orthogonal to it. Default is 0.5
+         * @param maximumFaceSegmentDistance the maximum distance a segment's center of mass can be perpendicularly to a mesh face
          * @return std::shared_ptr<geometry::DFPointCloud> The unified segments
          */
         static std::vector<std::shared_ptr<geometry::DFPointCloud>> DFSegmentation::AssociateClustersToMeshes(
             bool isCylinder,
+            bool discriminatePoints,
             std::vector<std::shared_ptr<geometry::DFMesh>> referenceMesh,
             std::vector<std::shared_ptr<geometry::DFPointCloud>> &clusters,
             double angleThreshold = 0.1,
             double associationThreshold = 0.1,
-            double angleAssociationThreshold = 0.5);
+            double maximumFaceSegmentDistance = 0.05);
 
         /** @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 isCylinder a boolean to indicate if the model is a cylinder. If true, the method will use the GetCenterAndAxis method of the mesh to find the center and axis of the mesh. based on that, we only want points that have normals more or less perpendicular to the cylinder axis.
+         * @param discriminatePoints a boolean to indicate if we want to discriminate points based on their normal direction when associating clusters to mesh faces. If true, only points that have normals more or less aligned with the face normal will be considered for association.
          * @param unassociatedClusters the clusters from the normal-based segmentatinon that haven't been associated yet.
          * @param existingPointCloudSegments the already associated segments per mesh face.
          * @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 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.   
-         * @param angleAssociationThreshold a number to indicate how much distance in the plane of the face should be favored, compared to distance orthogonal to the face normal. If set to 0, any face in the same plane as the face will be considered as having a distance of 0. If set to a high value (e.g. 1000000), no difference will be made between distance in the plane of the face and orthogonal to it. Default is 0.5
+         * @param maximumFaceSegmentDistance the maximum distance a segment's center of mass can be perpendicularly to a mesh face
          * @return void
          */
         static void DFSegmentation::CleanUnassociatedClusters(
             bool isCylinder,
+            bool discriminatePoints,
             std::vector<std::shared_ptr<geometry::DFPointCloud>> &unassociatedClusters,
             std::vector<std::vector<std::shared_ptr<geometry::DFPointCloud>>> &existingPointCloudSegments,
             std::vector<std::vector<std::shared_ptr<geometry::DFMesh>>> meshes,
             double angleThreshold = 0.1,
             double associationThreshold = 0.1,
-            double angleAssociationThreshold = 0.5);
+            double maximumFaceSegmentDistance = 0.05);
     };
 } // namespace diffCheck::segmentation

src/diffCheckBindings.cc

@@ -226,18 +226,20 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
 
         .def_static("associate_clusters", &diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes,
             py::arg("is_roundwood"),
+            py::arg("discriminate_points"),
             py::arg("reference_mesh"),
             py::arg("unassociated_clusters"),
             py::arg("angle_threshold") = 0.1,
             py::arg("association_threshold") = 0.1,
-            py::arg("angle_association_threshold") = 0.5)
+            py::arg("maximum_face_segment_distance") = 0.05)
 
         .def_static("clean_unassociated_clusters", &diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters,
             py::arg("is_roundwood"),
+            py::arg("discriminate_points"),
             py::arg("unassociated_clusters"),
             py::arg("associated_clusters"),
             py::arg("reference_mesh"),
             py::arg("angle_threshold") = 0.1,
             py::arg("association_threshold") = 0.1,
-            py::arg("angle_association_threshold") = 0.5);
+            py::arg("maximum_face_segment_distance") = 0.05);
 }