Merge pull request #40 from diffCheckOrg/substractive_refinement

Substractive registration refinement

Author: 9and3

src/diffCheck/geometry/DFMesh.cc

@@ -125,7 +125,7 @@ namespace diffCheck::geometry
             double res = (area1 + area2 + area3 - referenceTriangleArea) / referenceTriangleArea;
 
             // arbitrary value to avoid false positives (points that, when projected on the triangle, are in it, but that are actually located too far from the mesh to actually belong to it)
-            double maxProjectionDistance = std::min({(v1 - v0).norm(), (v2 - v1).norm(), (v0 - v2).norm()});
+            double maxProjectionDistance = std::min({(v1 - v0).norm(), (v2 - v1).norm(), (v0 - v2).norm()}) / 2;
 
             if (std::abs(res) < associationThreshold && (projectedPoint - point).norm() < maxProjectionDistance)
             {

src/diffCheck/geometry/DFPointCloud.cc

@@ -273,6 +273,13 @@ namespace diffCheck::geometry
         this->Normals = cloud->Normals;
     }
 
+    void DFPointCloud::AddPoints(const DFPointCloud &pointCloud)
+    {
+        this->Points.insert(this->Points.end(), pointCloud.Points.begin(), pointCloud.Points.end());
+        this->Colors.insert(this->Colors.end(), pointCloud.Colors.begin(), pointCloud.Colors.end());
+        this->Normals.insert(this->Normals.end(), pointCloud.Normals.begin(), pointCloud.Normals.end());
+    }
+
     Eigen::Vector3d DFPointCloud::GetCenterPoint()
     {
         if (this->Points.size() == 0)

src/diffCheck/geometry/DFPointCloud.hh

@@ -156,6 +156,13 @@ namespace diffCheck::geometry
          */
         void LoadFromPLY(const std::string &path);
 
+        /**
+         * @brief adds the points, colors and normals from another point cloud
+         * 
+         * @param pointCloud the other point cloud
+         */
+        void AddPoints(const DFPointCloud &pointCloud);
+
     public:  ///< Getters
         /// @brief Number of points in the point cloud
         int GetNumPoints() const { return this->Points.size(); }

src/diffCheckApp.cc

@@ -12,54 +12,94 @@ int main()
   auto initTime = std::chrono::high_resolution_clock::now();
 
   std::shared_ptr<diffCheck::geometry::DFPointCloud> pcdSrc = std::make_shared<diffCheck::geometry::DFPointCloud>();
-  std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>> meshSrc = std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>();
+  std::vector<std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>> meshSrc = std::vector<std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>>();
   std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>> segments;
   std::vector<std::string> meshPaths;
 
-  std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\9\)";
+  std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\joints\)";
 
-  for (int i = 1; i <= 7; i++)
+  for (int i = 1; i <= 3; 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::vector<std::shared_ptr<diffCheck::geometry::DFMesh>> fullJoint;
+    for (int j = 1; j <= 3; j++)
+    {
+      std::string meshPath = meshesFolderPath + std::to_string(i) + "/" + std::to_string(j) + ".ply";
+      std::shared_ptr<diffCheck::geometry::DFMesh> mesh = std::make_shared<diffCheck::geometry::DFMesh>();
+      mesh->LoadFromPLY(meshPath);
+      fullJoint.push_back(mesh);
+    }
+    meshSrc.push_back(fullJoint);
   }
 
-  std::string pathPcdSrc = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\source_pc_2.ply)";
+  std::string pathPcdSrc = R"(C:\Users\localuser\Desktop\meshes_for_diffCheck\joints\full_beam.ply)";
 
   pcdSrc->LoadFromPLY(pathPcdSrc);
 
   pcdSrc->EstimateNormals(false, 100);
-  pcdSrc->VoxelDownsample(0.01);
+  pcdSrc->VoxelDownsample(0.007);
   auto intermediateTime = std::chrono::high_resolution_clock::now();
   segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(
     pcdSrc,
-    6.0f,
-    25,
+    15.0f,
+    15,
     true,
-    20,
+    50,
     0.5f,
     false);
   std::cout << "number of segments:" << segments.size()<< std::endl;
 
-  std::shared_ptr<diffCheck::geometry::DFPointCloud> unifiedSegments = 
-    diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes(
-      meshSrc, 
+  std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>> unifiedSegments;
+  for (int i = 0; i < meshSrc.size(); i++)
+  {
+    std::shared_ptr<diffCheck::geometry::DFPointCloud> unifiedSegment = std::make_shared<diffCheck::geometry::DFPointCloud>();
+    unifiedSegment = diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes(
+      meshSrc[i], 
       segments, 
-      .1, 
-      .9);
-  
-  std::cout << "Association done. refinement in progress" << std::endl;
+      .2, 
+      .05);
+    unifiedSegments.push_back(unifiedSegment);
+  }
 
   diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters(segments, 
-    std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>{unifiedSegments}, 
-    std::vector<std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>>{meshSrc},
-    .1, 
-    .9);
-  
-  std::cout << "number of points in unified segments:" << unifiedSegments->Points.size() << std::endl;
-  
+    unifiedSegments, 
+    meshSrc,
+    .2, 
+    .05);
+
+  // Perform a registration per joint
+  for (int i = 0; i < meshSrc.size(); i++)
+  {
+    std::shared_ptr<diffCheck::geometry::DFPointCloud> referencePointCloud = std::make_shared<diffCheck::geometry::DFPointCloud>();
+    for (auto jointFace : meshSrc[i])
+    {
+      std::shared_ptr<diffCheck::geometry::DFPointCloud> facePointCloud = jointFace->SampleCloudUniform(1000);
+      referencePointCloud->Points.insert(referencePointCloud->Points.end(), facePointCloud->Points.begin(), facePointCloud->Points.end());
+    }
+    referencePointCloud->EstimateNormals(false, 100);
+
+    diffCheck::transformation::DFTransformation transformation = diffCheck::registrations::DFRefinedRegistration::O3DICP(
+      unifiedSegments[i],
+      referencePointCloud);
+
+    std::cout << "Transformation matrix:" << std::endl;
+    std::cout << transformation.TransformationMatrix << std::endl;
+
+    diffCheck::visualizer::Visualizer deVisu = diffCheck::visualizer::Visualizer("DiffCheckApp", 1000, 800, 50, 50, false, true, false);
+    for (int i = 0; i < segments.size(); i++)
+    {
+      segments[i]->ApplyTransformation(transformation);
+      deVisu.AddPointCloud(segments[i]);
+    }
+    for (auto joint : meshSrc)
+    {
+      for (auto face : joint)
+      {
+        deVisu.AddMesh(face);
+      }
+    }
+    deVisu.Run();
+  }
+    
   diffCheck::visualizer::Visualizer vis(std::string("DiffCheckApp"), 1000, 800, 50, 50, false, true, false);
   for (auto segment : segments)
   {
@@ -74,18 +114,25 @@ int main()
       segment->Colors.push_back(Eigen::Vector3d(0, 0, 0));
     }
   vis.AddPointCloud(segment);
-
   }
-  for(auto mesh : meshSrc)
+  for(auto joint : meshSrc)
   {
-    //vis.AddMesh(mesh);
+    for(auto mesh : joint){vis.AddMesh(mesh);}
   }
 
-  for (int i = 0; i < unifiedSegments->Points.size(); i++)
+  int numSegments = unifiedSegments.size();
+
+  for (int i = 0; i < numSegments; i++)
   {
-    unifiedSegments->Colors.push_back(Eigen::Vector3d(0, 0, 1));
+    for (int j = 0; j < unifiedSegments[i]->Points.size(); j++)
+    {
+      unifiedSegments[i]->Colors.push_back(Eigen::Vector3d((double(numSegments) - double(i))/double(numSegments), 1, double(i) / double(numSegments)));
+    }
+  }
+  for (auto seg : unifiedSegments)
+  {
+    vis.AddPointCloud(seg);
   }
-  vis.AddPointCloud(unifiedSegments);
 
   auto endTime = std::chrono::high_resolution_clock::now();
   auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - initTime);
@@ -95,6 +142,5 @@ int main()
 
   vis.Run();
 
-
   return 0;
 }

src/diffCheckBindings.cc

@@ -48,6 +48,7 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
             py::arg("r"), py::arg("g"), py::arg("b"))
 
         .def("load_from_PLY", &diffCheck::geometry::DFPointCloud::LoadFromPLY)
+        .def("add_points", &diffCheck::geometry::DFPointCloud::AddPoints)  
 
         .def("get_tight_bounding_box", &diffCheck::geometry::DFPointCloud::GetTightBoundingBox)
 

src/gh/components/DF_joint_segmentator/code.py

@@ -0,0 +1,90 @@
+#! python3
+
+import Rhino
+
+import diffCheck
+from diffCheck import diffcheck_bindings
+from diffCheck import df_cvt_bindings as df_cvt
+import diffCheck.df_util
+
+from ghpythonlib.componentbase import executingcomponent as component
+
+ABSTOL = Rhino.RhinoDoc.ActiveDoc.ModelAbsoluteTolerance
+
+class DFJointSegmentator(component):
+    def RunScript(self, 
+                  i_clusters: Rhino.Geometry.PointCloud, 
+                  i_joints: Rhino.Geometry.Mesh, 
+                  i_joint_ids: int,
+                  i_angle_threshold: float,
+                  i_distance_threshold: float):
+        """
+        Amongst clusters, associates the clusters to the individual joints, 
+        creates a reference point cloud for each joint, 
+        and returns the joint segments, the reference point clouds, and the ICP transformations from the first to the second.
+
+        :param i_clusters: The clusters to be associated to the joints.
+        :param i_joints: The joints to which the clusters will be associated. These are meshes.
+        :param i_joint_ids: The joint ids of the joints.
+        :param i_angle_threshold: The angle threshold for the association of the clusters to the joints.
+        :param i_distance_threshold: The distance threshold for the association of the clusters to the joints.
+        """
+        if i_angle_threshold is None : i_angle_threshold = 0.1
+        if i_distance_threshold is None : i_distance_threshold = 0.1
+
+        if len(i_joints) != len(i_joint_ids):
+            raise ValueError("The number of joints and joint ids must be the same.")
+        
+        if len(i_clusters) == 0:
+            raise ValueError("No clusters given.")
+
+        if not isinstance(i_clusters[0], Rhino.Geometry.PointCloud):
+            raise ValueError("The input clusters must be PointClouds.")
+        
+        if not isinstance(i_joints[0], Rhino.Geometry.Mesh):
+            raise ValueError("The input joints must be convertible to Meshes.")
+            
+
+        # prepping the reference meshes
+        n_joints = max(i_joint_ids) + 1
+        joints = [ [] for i in range(n_joints) ]
+        for face, id in zip(i_joints, i_joint_ids):
+            face.Subdivide()
+            face.Faces.ConvertQuadsToTriangles()
+            joints[id].append(df_cvt.cvt_rhmesh_2_dfmesh(face))
+
+        joint_clouds = []
+        registrations = []
+        joint_segments = []
+        df_clouds = [df_cvt.cvt_rhcloud_2_dfcloud(cluster) for cluster in i_clusters]
+
+        # for each joint, find the corresponding clusters and merge them, generate a reference point cloud, and register the merged clusters to the reference point cloud
+        for joint in joints:
+
+            # create the reference point cloud
+            joint_cloud = diffcheck_bindings.dfb_geometry.DFPointCloud()
+
+            for face in joint:
+                face_cloud = face.sample_points_uniformly(1000)
+                joint_cloud.add_points(face_cloud)
+
+            joint_clouds.append(df_cvt.cvt_dfcloud_2_rhcloud(joint_cloud))
+
+            # find the corresponding clusters and merge them
+            segment = diffcheck_bindings.dfb_segmentation.DFSegmentation.associate_clusters(joint, df_clouds, i_angle_threshold, i_distance_threshold)
+            diffcheck_bindings.dfb_segmentation.DFSegmentation.clean_unassociated_clusters(df_clouds, [segment], [joint], i_angle_threshold ,i_distance_threshold)
+
+            # register the merged clusters to the reference point cloud
+            registration = diffcheck_bindings.dfb_registrations.DFRefinedRegistration.O3DICP(segment, joint_cloud)
+            res = registration.transformation_matrix
+
+            registrations.append(df_cvt.cvt_ndarray_2_rh_transform(res))
+            joint_segments.append(df_cvt.cvt_dfcloud_2_rhcloud(segment))
+
+        return joint_segments, registrations, joint_clouds
+
+# if __name__ == "__main__":
+#     o_joint_segments, o_reference_point_clouds, o_transforms = main(i_clusters, i_joints, i_joint_ids)
+
+#     for i in range(len(o_joint_segments)):
+#         o_joint_segments[i].Transform(o_transforms[i])