commit before rebase

Author: DamienGilliard

src/diffCheck/segmentation/DFSegmentation.cc

@@ -94,4 +94,119 @@ namespace diffCheck::segmentation
 
         return segments;
     }
+
+    std::tuple<std::shared_ptr<geometry::DFPointCloud>, std::vector<std::shared_ptr<geometry::DFPointCloud>>> DFSegmentation::AssociateSegments(
+        std::vector<std::shared_ptr<geometry::DFMesh>> meshFaces,
+        std::vector<std::shared_ptr<geometry::DFPointCloud>> segments,
+        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
+        for (std::shared_ptr<diffCheck::geometry::DFMesh> face : meshFaces)
+        {
+            std::shared_ptr<geometry::DFPointCloud> correspondingSegment;
+            std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace;
+            o3dFace = face->Cvt2O3DTriangleMesh();
+
+            // Getting the center of the mesh face
+            Eigen::Vector3d faceCenter;
+            open3d::geometry::OrientedBoundingBox orientedBoundingBox = o3dFace->GetMinimalOrientedBoundingBox();
+            faceCenter = orientedBoundingBox.GetCenter();
+
+            if (face->NormalsFace.size() == 0)
+            {
+                o3dFace->ComputeTriangleNormals();
+                face->NormalsFace.clear();
+                for (auto normal : o3dFace->triangle_normals_)
+                {
+                    face->NormalsFace.push_back(normal.cast<double>());
+                }
+            }
+            
+            // Getting the normal of the mesh face
+            Eigen::Vector3d faceNormal = face->NormalsFace[0];
+
+            // iterate through the segments to find the closest ones compared to the face center taking the normals into account
+            Eigen::Vector3d segmentCenter;
+            Eigen::Vector3d segmentNormal;
+            double faceDistance = std::numeric_limits<double>::max();
+            for (auto segment : segments)
+            {
+                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 (currentDistance < faceDistance)
+                {
+                    correspondingSegment = segment;
+                    faceDistance = currentDistance;
+                }
+            }
+            
+            // get the triangles of the face. This is to check if the point is in the face
+            std::vector<Eigen::Vector3i> faceTriangles = o3dFace->triangles_;
+
+            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)
+                {
+                    unifiedPointCloud->Points.push_back(point);
+                }
+            }
+            // removing points from the segment that are in the face
+            for(Eigen::Vector3d point : unifiedPointCloud->Points)
+            {
+                correspondingSegment->Points.erase(std::remove(correspondingSegment->Points.begin(), correspondingSegment->Points.end(), point), correspondingSegment->Points.end());
+            }
+        }
+        return std::make_tuple(unifiedPointCloud, segments);
+    }
+
 } // namespace diffCheck::segmentation

src/diffCheckApp.cc

@@ -27,12 +27,32 @@ int main()
     true);
   std::cout << "number of segments:" << segments.size()<< std::endl;
 
-  std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
-  vis->RenderPcdColorNormals = false;
-  
+  std::tuple<std::shared_ptr<diffCheck::geometry::DFPointCloud>, std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>> unifiedSegments = 
+    diffCheck::segmentation::DFSegmentation::AssociateSegments(meshSrc, segments, 0.15);
+
+  diffCheck::visualizer::Visualizer vis;
+  vis.AddMesh(meshSrc);
   for (auto segment : segments)
     vis->AddPointCloud(segment);
 
-  vis->Run();
+  }
+  }
+  for(auto mesh : meshSrc)
+  {
+    vis.AddMesh(mesh);
+  }
+
+  auto unified = std::get<0>(unifiedSegments);
+  for (int i = 0; i < unified->Points.size(); i++)
+  {
+    unified->Colors.push_back(Eigen::Vector3d(0, 0, 0));
+  }
+  vis.AddPointCloud(unified);
+
+  auto endTime = std::chrono::high_resolution_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - initTime);
+  std::cout << "Computation time:" << duration.count() << std::endl;
+
+  vis.Run();
   return 0;
 }