CAP-WIP: working version basic for registration - problem with scaling and parameters adaptation mechanism

Author: 9and3

src/diffCheck/registrations/DFGlobalRegistrations.hh

@@ -20,6 +20,7 @@ namespace diffCheck::registrations
         * 
         * @param source the source diffCheck point cloud
         * @param target the target diffCheck point cloud
+        * @param voxelize whether to voxelize the point clouds before computing the FPFHFeatures. A higher value will result in a more coarse point cloud (less resulting points).
         * @param voxelSize the size of the voxels used to downsample the point clouds. It is expressed relative to the point cloud size (0.01 means voxelSize = 1% of maxSize(pointCloud). A higher value will result in a more coarse point cloud (less resulting points).
         * @param radiusKDTreeSearch  the radius used to search for neighbors in the KDTree.it is expressed relative to the point cloud size (0.01 means radiusKDTreeSearch = 1% of maxSize(pointCloud). It is used for the calculation of FPFHFeatures
         * @param maxNeighborKDTreeSearch the maximum number of neighbors to search for in the KDTree. It is used for the calculation of FPFHFeatures. A higher value will result in heavier computation but potentially more precise.

src/diffCheckApp.cc

@@ -7,60 +7,27 @@
 int main()
 {
   // import clouds
-  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr = std::make_shared<diffCheck::geometry::DFPointCloud>();
-  std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr = std::make_shared<diffCheck::geometry::DFMesh>();
-  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfGroundTruth = std::make_shared<diffCheck::geometry::DFPointCloud>();
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> pcdSrc = std::make_shared<diffCheck::geometry::DFPointCloud>();
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> pcdTgt = std::make_shared<diffCheck::geometry::DFPointCloud>();
 
-  // std::string pathCloud = R"(C:\Users\andre\Downloads\scan_data_normals.ply\scan_data_normals.ply)";
-  // std::string pathMesh = R"(F:\diffCheck\assets\dataset\mesh_fromRh_unfixedLength.ply)";
-  // std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
+  std::string pathPcdSrc = R"(C:\Users\andre\Downloads\04_pt.ply)";
+  std::string pathPcdTgt = R"(C:\Users\andre\Downloads\moved_04.ply)";
 
-  // create a sphere from o3d
+  pcdSrc->LoadFromPLY(pathPcdSrc);
+  pcdTgt->LoadFromPLY(pathPcdTgt);
 
-  std::string pathCloud = R"(C:\Users\localuser\Downloads\04_pt.ply)";
-  std::string pathMesh = R"(C:\Users\localuser\Downloads\04_mesh.ply)";
-  // std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
+  // global registration
+  diffCheck::transformation::DFTransformation xform = 
+    diffCheck::registrations::DFGlobalRegistrations::O3DFastGlobalRegistrationFeatureMatching(
+        pcdSrc, pcdTgt);
 
-  dfMeshPtr->LoadFromPLY(pathMesh);
-  dfPointCloudPtr->LoadFromPLY(pathCloud);
+  pcdTgt->ApplyTransformation(xform);
 
-  dfGroundTruth->Cvt2DFPointCloud(dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(10000));
-  Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
-  transformation(0, 3) = 0.0;
-  transformation(1, 3) = -0.02;
-  transformation(2, 3) = 0.02;
-  Eigen::Matrix3d rotation;
-  rotation = Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitY());
-  transformation.block<3, 3>(0, 0) = rotation * transformation.block<3, 3>(0, 0);
 
-  dfPointCloudPtr->ApplyTransformation(transformation);
-
-  diffCheck::transformation::DFTransformation simpleICPTransformation 
-    = diffCheck::registrations::RefinedRegistration::O3DICP(
-      dfPointCloudPtr, 
-      dfGroundTruth, 
-      0.05);
-
-  diffCheck::transformation::DFTransformation generalizedICPTransformation 
-    = diffCheck::registrations::RefinedRegistration::O3DGeneralizedICP(
-      dfPointCloudPtr,
-      dfGroundTruth,
-      0.05);
-  dfPointCloudPtr->ApplyTransformation(simpleICPTransformation);
-
-  std::stringstream ss;
-  ss << "Simple ICP Transformation:\n";
-  ss << simpleICPTransformation.TransformationMatrix<<"\n";
-  ss << "Generalized ICP Transformation:\n";
-  ss << generalizedICPTransformation.TransformationMatrix;
-  std::string matrixAsString = ss.str();
-  DIFFCHECK_INFO(matrixAsString.c_str());
 
   std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
-  // vis->AddPointCloud(dfPointCloudPtr);
-  vis->AddMesh(dfMeshPtr);
-  vis->AddPointCloud(dfGroundTruth);
-  vis->AddPointCloud(dfPointCloudPtr);
+  vis->AddPointCloud(pcdSrc);
+  vis->AddPointCloud(pcdTgt);
   vis->Run();
   return 0;
 }

src/diffCheckBindings.cc

@@ -20,7 +20,7 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
     submodule_test.def("test", &test, "Simple function testing a vanilla python bindings.");
 
     //#################################################################################################
-    // df_geometry namespace
+    // dfb_geometry namespace
     //#################################################################################################
 
     py::module_ submodule_geometry = m.def_submodule("dfb_geometry", "A submodule for the geometry classes.");
@@ -83,8 +83,50 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
             [](diffCheck::geometry::DFMesh &self, const std::vector<Eigen::Vector3d>& value) { self.ColorsFace = value; });
 
     //#################################################################################################
-    // df_registration namespace
+    // dfb_transformation namespace
     //#################################################################################################
 
-    // py::module_ submodule_geometry = m.def_submodule("df_registration", "A submodule for the registration methods.");
+    py::module_ submodule_transformation = m.def_submodule("dfb_transformation", "A submodule for the transformation classes.");
+
+    py::class_<diffCheck::transformation::DFTransformation>(submodule_transformation, "DFTransformation")
+        .def(py::init<>())
+        .def(py::init<const Eigen::Matrix4d&>())
+        .def(py::init<const Eigen::Matrix3d&, const Eigen::Vector3d&>())
+
+        .def_readwrite("transformation_matrix", &diffCheck::transformation::DFTransformation::TransformationMatrix)
+        .def_readwrite("rotation_matrix", &diffCheck::transformation::DFTransformation::RotationMatrix)
+        .def_readwrite("translation_vector", &diffCheck::transformation::DFTransformation::TranslationVector);
+        
+
+    //#################################################################################################
+    // dfb_registrations namespace
+    //#################################################################################################
+
+    py::module_ submodule_registrations = m.def_submodule("dfb_registrations", "A submodule for the registration methods.");
+
+    py::class_<diffCheck::registrations::DFGlobalRegistrations>(submodule_registrations, "DFGlobalRegistrations")
+        .def_static("O3DFastGlobalRegistrationFeatureMatching", &diffCheck::registrations::DFGlobalRegistrations::O3DFastGlobalRegistrationFeatureMatching,
+            py::arg("source"),
+            py::arg("target"),
+            py::arg("voxelize") = true,
+            py::arg("voxel_size") = 0.005,
+            py::arg("radius_kd_tree_search") = 0.1,
+            py::arg("max_neighbor_kd_tree_search") = 50,
+            py::arg("max_correspondence_distance") = 0.05,
+            py::arg("iteration_number") = 128,
+            py::arg("max_tuple_count") = 1000)
+        .def_static("O3DRansacOnFeatureMatching", &diffCheck::registrations::DFGlobalRegistrations::O3DRansacOnFeatureMatching,
+            py::arg("source"),
+            py::arg("target"),
+            py::arg("voxelize") = true,
+            py::arg("voxel_size") = 0.005,
+            py::arg("radius_kd_tree_search") = 0.1,
+            py::arg("max_neighbor_kd_tree_search") = 50,
+            py::arg("max_correspondence_distance") = 0.05,
+            py::arg("is_t_estimate_pt2pt") = false,
+            py::arg("ransac_n") = 3,
+            py::arg("correspondence_checker_distance") = 0.05,
+            py::arg("similarity_threshold") = 0.9,
+            py::arg("ransac_max_iteration") = 100000,
+            py::arg("ransac_confidence_threshold") = 0.999);
 }

src/gh/components/DF_global_registration/code.py

@@ -12,34 +12,74 @@
 
 import diffCheck
 from diffCheck import diffcheck_bindings
-import diffCheck.df_cvt_bindings
+from diffCheck import df_cvt_bindings
 
 
 class DFGlobalRegistration(component):
     def RunScript(self,
-            i_mesh: rg.Mesh,
-            i_points: int) -> rg.PointCloud:
+        i_cloud_source: rg.PointCloud = None,
+        i_cloud_target: rg.PointCloud = None
+    ) -> rg.Transform:
         """
-            Convert a Rhino mesh to a cloud.
+            The global registration component aligns two point clouds in a rough way.
 
-            :param i_mesh: mesh to convert
-            :param i_points: number of points to sample
+            :param i_cloud_source: source point cloud
+            :param i_cloud_target: target point cloud to align to
 
-            :return o_cloud: rhino cloud
+            :return o_x_form : transformation matrix
         """
 
-        diffCheck.df_cvt_bindings.test()
+        # if i_cloud_source is None or i_cloud_target is None:
+        #     ghenv.Component.AddRuntimeMessage(RML.Error, "Please provide two point clouds to align")
+        #     return o_x_form
 
-        df_mesh = diffCheck.df_cvt_bindings.cvt_rhmesh_2_dfmesh(i_mesh)
-        df_cloud = df_mesh.sample_points_uniformly(i_points)
+        print(type(i_cloud_source))
 
-        # convert the df_cloud to a rhino cloud
-        rgpoints = [rg.Point3d(pt[0], pt[1], pt[2]) for pt in df_cloud.points]
-        rh_cloud = rg.PointCloud(rgpoints)
+        df_cloud_source = df_cvt_bindings.cvt_rhcloud_2_dfcloud(i_cloud_source)
+        df_cloud_target = df_cvt_bindings.cvt_rhcloud_2_dfcloud(i_cloud_target)
 
-        return [rh_cloud]
+        print(type(df_cloud_source))    
+        # # print all the available registration methods
+        # print(dir(diffcheck_bindings.dfb_registrations))
+        # registrations = diffcheck_bindings.dfb_registrations.DFGlobalRegistrations()
+
+
+
+        df_xform = diffcheck_bindings.dfb_registrations.DFGlobalRegistrations.O3DFastGlobalRegistrationFeatureMatching(
+            source=df_cloud_source,
+            target=df_cloud_target,
+            voxelize=True,
+            voxel_size=0.1,
+            radius_kd_tree_search=0.1,
+            max_neighbor_kd_tree_search=50,
+            max_correspondence_distance=0.05,
+            iteration_number=128,
+            max_tuple_count=1000
+        )
+        print(type(df_xform))
+
+        print(df_xform.transformation_matrix)
+        print("-------------------")
+        print(df_xform.rotation_matrix)
+        print("-------------------")
+        print(df_xform.translation_vector)
+
+        df_xform_matrix = df_xform.transformation_matrix
+
+
+        rh_form = rg.Transform()
+        for i in range(4):
+            for j in range(4):
+                rh_form[i, j] = df_xform_matrix[i, j]
+
+        print(rh_form)
+
+        o_x_form = rh_form
+
+
+        return o_x_form
 
 
 if __name__ == "__main__":
     com = DFGlobalRegistration()
-    com.RunScript(i_mesh, i_points)
+    o_x_form = com.RunScript(i_cloud_source, i_cloud_target)

src/gh/diffCheck/diffCheck/__pycache__/__init__.cpython-39.pyc

@@ -164,4 +164,26 @@ def cvt_rhmesh_2_dfmesh(rh_mesh: rg.Mesh) -> diffcheck_bindings.dfb_geometry.DFM
             colors_vertex.append([color.R, color.G, color.B])
         df_mesh.colors_vertex = colors_vertex
 
-    return df_mesh
+    return df_mesh
+
+def cvt_dfxform_2_rhxform(df_xform : diffcheck_bindings.dfb_transformation.DFTransformation) -> rg.Transform:
+    """
+        Convert a diffCheck transformation to a Rhino transformation.
+
+        :param df_xform: diffCheck transformation
+        :return rh_xform: rhino transformation
+    """
+    if not isinstance(df_xform, diffcheck_bindings.dfb_transformation.DFTransformation):
+        raise ValueError("df_xform should be a DFTransformation")
+
+    rh_xform = rg.Transform()
+
+    # translation
+    translation = df_xform.translation_vector
+    rh_xform = rg.Transform.Translation(translation[0], translation[1], translation[2])
+
+    # rotation
+    rotation = df_xform.rotation_matrix
+    rh_xform = rh_xform * rg.Transform.Rotation(rotation[0], rg.Vector3d(rotation[1], rotation[2], rotation[3]), rg.Point3d(0, 0, 0))
+
+    return rh_xform