WIP: creation of the aggregatino function that associates point cloud segments to mesh faces

Author: DamienGilliard

src/diffCheck/segmentation/DFSegmentation.cc

@@ -97,4 +97,90 @@ 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 (auto face : meshFaces)
+        {
+            std::shared_ptr<geometry::DFPointCloud> correspondingSegment;
+            
+            // Getting the center of the mesh face
+            Eigen::Vector3d faceCenter = Eigen::Vector3d::Zero();
+            for (auto vertex : face->Vertices){faceCenter += vertex;}
+            faceCenter /= face->GetNumVertices();
+
+            if (face->NormalsFace.size() == 0)
+            {
+                std::cout << "face has no normals, computing normals" << std::endl;
+                std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace = face->Cvt2O3DTriangleMesh();
+                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::Zero();
+            Eigen::Vector3d segmentNormal = Eigen::Vector3d::Zero();
+            double faceDistance = (segments[0]->Points[0] - faceCenter).norm() / std::pow(std::abs(segments[0]->Normals[0].dot(faceNormal)), 2);
+            int segmentIndex = 0;
+            for (auto segment : segments)
+            {
+                for (auto point : segment->Points)
+                {
+                    segmentCenter += point;
+                }
+                segmentCenter /= segment->GetNumPoints();
+
+                if (segment->Normals.size() == 0)
+                {
+                    std::shared_ptr<open3d::geometry::PointCloud> o3dSegment = segment->Cvt2O3DPointCloud();
+                    o3dSegment->EstimateNormals();
+                    segment->Normals.clear();
+                    for (auto normal : o3dSegment->normals_)
+                    {
+                        segment->Normals.push_back(normal.cast<double>());
+                    }
+                }
+                for (auto normal : segment->Normals)
+                {
+                    segmentNormal += normal;
+                }
+                segmentNormal.normalize();
+
+                double currentDistance = (faceCenter - segmentCenter).norm() / std::pow(std::abs(segmentNormal.dot(faceNormal)), 2);
+                // if the distance is smaller than the previous one, update the distance and the corresponding segment
+                if (currentDistance < faceDistance)
+                {
+                    correspondingSegment = segment;
+                    // storing previous segment in the remainder vector
+                    faceDistance = currentDistance;
+                }
+                segmentIndex++;
+            }
+            // remove the segment fron the segments vector
+            segments.erase(std::remove(segments.begin(), segments.end(), correspondingSegment), segments.end());
+            
+            // Add the closest points of the corresponding segment to the unified point cloud
+            for (auto point : correspondingSegment->Points)
+            {
+                unifiedPointCloud->Points.push_back(point);
+                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

@@ -7,30 +7,52 @@
 int main()
 {
   std::shared_ptr<diffCheck::geometry::DFPointCloud> pcdSrc = std::make_shared<diffCheck::geometry::DFPointCloud>();
-  std::shared_ptr<diffCheck::geometry::DFMesh> meshSrc = std::make_shared<diffCheck::geometry::DFMesh>();
+  std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>> meshSrc = std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>();
   std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>> segments;
-  std::string pathMeshSrc = R"(C:\Users\localuser\Downloads\02_mesh.ply)";
+  std::vector<std::string> meshPaths;
+
+  std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\other_meshes_for_diffCheck\)";
+
+  for (int i = 1; i <= 14; 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\Downloads\02_points_with_errors_1e6_pts.ply)";
 
   pcdSrc->LoadFromPLY(pathPcdSrc);
-  meshSrc->LoadFromPLY(pathMeshSrc);
 
-  segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(*pcdSrc, 0.01, 1, 30, true, 50, 30);
+  segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(*pcdSrc, 5, 1, 20, true, 50, 200);
   std::cout << "number of segments:" << segments.size()<< std::endl;
 
+  std::tuple<std::shared_ptr<diffCheck::geometry::DFPointCloud>, std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>> unifiedSegments = 
+    diffCheck::segmentation::DFSegmentation::AssociateSegments(meshSrc, segments, 50);
+  
+
   diffCheck::visualizer::Visualizer vis;
-  vis.AddMesh(meshSrc);
-  for (auto segment : segments)
+  for (int i = 0; i < 1; i++)
   {
     // colorize the segments with random colors
     double r = static_cast<double>(rand()) / RAND_MAX;
     double g = static_cast<double>(rand()) / RAND_MAX;
     double b = static_cast<double>(rand()) / RAND_MAX;    
-    for (int i = 0; i < segment->Points.size(); i++)
+    for (int i = 0; i < std::get<0>(unifiedSegments)->Points.size(); i++)
+    {
+      std::get<0>(unifiedSegments)->Colors.push_back(Eigen::Vector3d(r, g, b));
+    }
+    vis.AddPointCloud(std::get<0>(unifiedSegments));
+
+    for (auto segemt : std::get<1>(unifiedSegments))
     {
-      segment->Colors.push_back(Eigen::Vector3d(r, g, b));
+      for (int i = 0; i < segemt->Points.size(); i++)
+      {
+        segemt->Colors.push_back(Eigen::Vector3d(0, 0, 1));
+      }
+      vis.AddPointCloud(segemt);
     }
-    vis.AddPointCloud(segment);
 
   }
   vis.Run();