Merge pull request #33 from diffCheckOrg/semantic_segmentation

Backend cpp for model-based semantic segmentation

Author: 9and3

src/diffCheck/geometry/DFMesh.cc

@@ -79,6 +79,22 @@ namespace diffCheck::geometry
         return bboxPts;
     }
 
+    Eigen::Vector3d DFMesh::GetFirstNormal()
+    {
+        if (this->NormalsFace.size() == 0)
+        {
+            std::shared_ptr<open3d::geometry::TriangleMesh> O3DTriangleMesh = this->Cvt2O3DTriangleMesh();
+            O3DTriangleMesh->ComputeTriangleNormals();
+            this->NormalsFace.resize(O3DTriangleMesh->triangle_normals_.size());
+            for (size_t i = 0; i < O3DTriangleMesh->triangle_normals_.size(); i++)
+            {
+                this->NormalsFace[i] = O3DTriangleMesh->triangle_normals_[i];
+            }
+
+        }
+        return this->NormalsFace[0];
+    }
+
     void DFMesh::LoadFromPLY(const std::string &path)
     {
         std::shared_ptr<diffCheck::geometry::DFMesh> tempMesh_ptr = diffCheck::io::ReadPLYMeshFromFile(path);

src/diffCheck/geometry/DFMesh.hh

@@ -71,6 +71,13 @@ namespace diffCheck::geometry
          */
         std::vector<Eigen::Vector3d> GetTightBoundingBox();
 
+        /**
+         * @brief Get the first normal of the mesh. Meant for planar meshes
+         * 
+         * @return Eigen::Vector3d the normal
+         */
+        Eigen::Vector3d GetFirstNormal();
+
     public:  ///< I/O loader
         /**
          * @brief Read a mesh from a file

src/diffCheck/geometry/DFPointCloud.hh

@@ -81,7 +81,7 @@ namespace diffCheck::geometry
          */
         void EstimateNormals(
             bool useCilantroEvaluator = false,
-            std::optional<int> knn = 100,
+            std::optional<int> knn = 50,
             std::optional<double> searchRadius = std::nullopt);
 
         /**

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
 {   
@@ -60,7 +61,6 @@ namespace diffCheck::segmentation
         else
         {
             cilantro::RadiusNeighborhoodSpecification<float> neighborhood(radiusNeighborhoodSize);
-
             cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
             cilantroPointCloud->normals,
             normalThresholdDegree*M_PI/180.0f);
@@ -94,4 +94,257 @@ namespace diffCheck::segmentation
 
         return segments;
     }
+
+    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>();
+
+        // iterate through the mesh faces given as function argument
+        if (referenceMesh.size() == 0)
+        {
+            DIFFCHECK_WARN("No mesh faces to associate with the clusters. Returning the first clusters untouched.");
+            return clusters[0];
+        }
+        for (std::shared_ptr<diffCheck::geometry::DFMesh> face : referenceMesh)
+        {
+            std::shared_ptr<geometry::DFPointCloud> correspondingSegment;
+
+            // Getting the center of the mesh face
+            Eigen::Vector3d faceCenter = face->Cvt2O3DTriangleMesh()->GetCenter();
+
+            // Getting the normal of the mesh face
+            Eigen::Vector3d faceNormal = face->GetFirstNormal();
+            faceNormal.normalize();
+
+            // iterate through the segments to find the closest one compared to the face center taking the normals into account
+            Eigen::Vector3d segmentCenter;
+            Eigen::Vector3d segmentNormal;
+            double faceDistance = std::numeric_limits<double>::max();
+            if (clusters.size() == 0)
+            {
+                DIFFCHECK_WARN("No clusters to associate with the mesh faces. Returning the first mesh face untouched.");
+                return clusters[0];
+            }
+            for (auto segment : clusters)
+            {
+                for (auto point : segment->Points){segmentCenter += point;}
+                segmentCenter /= segment->GetNumPoints();
+
+                for (auto normal : segment->Normals){segmentNormal += normal;}
+                segmentNormal.normalize();
+
+                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(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold  && currentDistance < faceDistance)
+                {
+                    correspondingSegment = segment;
+                    faceDistance = currentDistance;
+                }
+            }
+            
+            // get the triangles of the face.
+            std::vector<Eigen::Vector3i> faceTriangles = face->Faces;
+
+            if (correspondingSegment == nullptr)
+            {
+                DIFFCHECK_WARN("No segment found for the face. Skipping the face.");
+                continue;
+            }
+            for (Eigen::Vector3d point : correspondingSegment->Points)
+            {
+                bool pointInFace = false;
+                if (DFSegmentation::IsPointOnFace(face, point, associationThreshold))
+                {
+                    unifiedPointCloud->Points.push_back(point);
+                    unifiedPointCloud->Normals.push_back(
+                        correspondingSegment->Normals[std::distance(
+                            correspondingSegment->Points.begin(), 
+                            std::find(correspondingSegment->Points.begin(), 
+                            correspondingSegment->Points.end(), 
+                            point))]
+                        );
+                }
+            }
+            // removing points from the segment that are in the face
+            if (unifiedPointCloud->GetNumPoints() == 0)
+            {
+                DIFFCHECK_WARN("No point was associated to this segment. Skipping the segment.");
+                continue;
+            }
+            for(Eigen::Vector3d point : unifiedPointCloud->Points)
+            {
+                correspondingSegment->Points.erase(
+                    std::remove(
+                        correspondingSegment->Points.begin(), 
+                        correspondingSegment->Points.end(), 
+                        point), 
+                    correspondingSegment->Points.end());
+            }
+            // removing the corresponding segment if it is empty after the point transfer
+            if (correspondingSegment->GetNumPoints() == 0)
+            {
+                clusters.erase(
+                    std::remove(
+                        clusters.begin(), 
+                        clusters.end(), 
+                        correspondingSegment), 
+                    clusters.end());
+            }
+        }
+        return unifiedPointCloud;
+    }
+
+    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,
+        double associationThreshold)
+    {
+        if (unassociatedClusters.size() == 0)
+        {
+            DIFFCHECK_WARN("No unassociated clusters. Nothing is done");
+            return;
+        }
+        for (std::shared_ptr<geometry::DFPointCloud> cluster : unassociatedClusters)
+        {
+            Eigen::Vector3d clusterCenter;
+            Eigen::Vector3d clusterNormal;
+
+            if (cluster->GetNumPoints() == 0)
+            {
+                DIFFCHECK_WARN("Empty cluster. Skipping the cluster.");
+                continue;
+            }
+            for (Eigen::Vector3d point : cluster->Points)
+            {
+                clusterCenter += point;
+            }
+            clusterCenter /= cluster->GetNumPoints();
+            for (Eigen::Vector3d normal : cluster->Normals)
+            {
+                clusterNormal += normal;
+            }
+            clusterNormal.normalize();
+
+            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)
+            {
+                DIFFCHECK_WARN("No meshes to associate with the clusters. Skipping the cluster.");
+                continue;
+            }
+            for (std::vector<std::shared_ptr<geometry::DFMesh>> piece : meshes)
+            {
+                if (piece.size() == 0)
+                {
+                    DIFFCHECK_WARN("Empty piece in the meshes vector. Skipping the mesh face vector.");
+                    continue;
+                }
+                for (std::shared_ptr<geometry::DFMesh> meshFace : piece)
+                {
+                    Eigen::Vector3d faceCenter;
+                    Eigen::Vector3d faceNormal;
+
+                    std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace = meshFace->Cvt2O3DTriangleMesh();
+                    
+                    faceNormal = meshFace->GetFirstNormal();
+                    faceNormal.normalize();
+                    faceCenter = o3dFace->GetCenter();
+                    /*
+                    To make sure we select the right meshFace, we add another metric:
+                    Indeed, from experimentation, sometimes the wrong mesh face is selected, because it is parallel to the correct one 
+                    (so the normal don't play a role) and the center of the face is closer to the cluster center than the correct face.
+                    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 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 (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));
+                    }
+                }
+            }
+            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)
+            {
+                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());
+                cluster->Points.pop_back();
+                std::swap(cluster->Normals[*it], cluster->Normals.back());
+                cluster->Normals.pop_back();
+            }
+        }
+    };
+
+    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

@@ -5,7 +5,7 @@ namespace diffCheck::segmentation
 {
     class DFSegmentation
     {
-        public:
+        public: ///< main segmentation methods
         /** @brief Downsamples and segments the point cloud using Cilantro's ConnectedComponentExtraction3f method. It uses the normals' variations to detect different parts in the point cloud.
          * @param pointCloud the point cloud to segment
          * @param normalThresholdDegree the normal threshold in degrees do differentiate segments. The higher the number, the more tolerent the segmentation will be to normal differences
@@ -24,5 +24,44 @@ namespace diffCheck::segmentation
             int knnNeighborhoodSize = 10,
             float radiusNeighborhoodSize = 10.f,
             bool colorClusters = false);
+
+        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 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 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.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 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.1,
+            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