WIP-ADD: Fast Global Registration added

Author: DamienGilliard

src/diffCheck.hh

@@ -7,4 +7,5 @@
 #include "diffCheck/geometry/DFPointCloud.hh"
 #include "diffCheck/geometry/DFMesh.hh"
 #include "diffCheck/IOManager.hh"
-#include "diffCheck/visualizer.hh"
+#include "diffCheck/visualizer.hh"
+#include "diffCheck/registration/registration.hh"

src/diffCheck/registration/registration.cc

@@ -0,0 +1,30 @@
+#include "registration.hh"
+
+namespace diffCheck::registration
+{
+     open3d::pipelines::registration::RegistrationResult Registration::o3dFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
+        {
+        auto sourceO3d = source->Cvt2O3DPointCloud();
+        auto targetO3d = target->Cvt2O3DPointCloud();
+
+        sourceO3d->RandomDownSample(0.1);
+        targetO3d->RandomDownSample(0.1);
+
+        std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3d,
+                                                                                                                                           open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
+        std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3d,
+                                                                                                                                           open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
+        std::shared_ptr<open3d::pipelines::registration::FastGlobalRegistrationOption> option = std::make_shared<open3d::pipelines::registration::FastGlobalRegistrationOption>();
+        option->maximum_correspondence_distance_ = 0.05;
+        option->iteration_number_ = 100;
+        option->maximum_tuple_count_ = 500;
+
+        auto result = open3d::pipelines::registration::FastGlobalRegistrationBasedOnFeatureMatching(*sourceO3d,
+                                                                                                    *targetO3d,
+                                                                                                    *sourceFPFHFeatures,
+                                                                                                    *targetFPFHFeatures,
+                                                                                                    *option);
+
+        return result;
+    }
+} // namespace diffCheck::registration

src/diffCheck/registration/registration.hh

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "diffCheck.hh"
+#include <open3d/pipelines/registration/Registration.h>
+
+namespace diffCheck::registration{
+
+class Registration
+{
+    public:
+
+    open3d::pipelines::registration::RegistrationResult o3dFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
+};
+}

src/diffCheckApp.cc

@@ -12,23 +12,39 @@
 int main()
 {
   std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr = std::make_shared<diffCheck::geometry::DFPointCloud>();
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterTrans = std::make_shared<diffCheck::geometry::DFPointCloud>();
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterReg = std::make_shared<diffCheck::geometry::DFPointCloud>();
   std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr = std::make_shared<diffCheck::geometry::DFMesh>();
 
-  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 pathCloud = R"(C:\Users\localuser\Downloads\00_pt.ply)";
+  std::string pathMesh = R"(C:\Users\localuser\Downloads\00_mesh.ply)";
   // std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
 
-  // create a sphere from o3d
-  auto mesh = open3d::geometry::TriangleMesh::CreateSphere(1.0, 4);
   dfMeshPtr->LoadFromPLY(pathMesh);
+  dfPointCloudPtr->LoadFromPLY(pathCloud);
 
+  // create a rigid rotation matrix
+  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+  T.block<3, 3>(0, 0) = Eigen::AngleAxisd(3 , Eigen::Vector3d::UnitZ()).toRotationMatrix(); // Yes, Pi = 3 in this case
+  T(0, 3) = 1;
+  T(1, 3) = 4;
 
-  dfMeshPtr->Cvt2DFMesh(mesh);
+  std::shared_ptr<open3d::geometry::PointCloud> o3DPC = std::make_shared<open3d::geometry::PointCloud>(dfPointCloudPtr->Cvt2O3DPointCloud()->Transform(T));
+  dfPointCloudPtrAfterTrans->Cvt2DFPointCloud(o3DPC);
+
+  std::shared_ptr<diffCheck::registration::Registration> reg = std::make_shared<diffCheck::registration::Registration>();
+  auto result = reg->o3dFastGlobalRegistrationFeatureMatching(dfPointCloudPtrAfterTrans, dfPointCloudPtr);
+
+  // apply the transformation to the source point cloud
+  Eigen::Matrix
+  <double, 4, 4> transformation = result.transformation_;
+  std::shared_ptr<open3d::geometry::PointCloud> o3DPCReg = std::make_shared<open3d::geometry::PointCloud>(dfPointCloudPtrAfterTrans->Cvt2O3DPointCloud()->Transform(transformation));
+  dfPointCloudPtrAfterReg->Cvt2DFPointCloud(o3DPCReg);
 
-  // dfPointCloudPtr->LoadFromPLY(pathCloud);
 
   std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
-  // vis->AddPointCloud(dfPointCloudPtr);
+  //vis->AddPointCloud(dfPointCloudPtrAfterTrans);
+  vis->AddPointCloud(dfPointCloudPtrAfterReg);
   vis->AddMesh(dfMeshPtr);
   vis->Run();