ADD-WIP: Point to point error calculation method added to Registration class

Author: DamienGilliard

src/diffCheck/registration/registration.cc

@@ -2,6 +2,17 @@
 
 namespace diffCheck::registration
 {
+    std::vector<double> Registration::ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
+    {
+        std::vector<double> errors;
+        auto O3DSourcePointCloud = source->Cvt2O3DPointCloud();
+        auto O3DTargetPointCloud = target->Cvt2O3DPointCloud();
+        
+        std::vector<double> distances;
+
+        distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
+        return distances;
+    }
     /*
     Documentation on Fast Point Feature Historigrams: https://pcl.readthedocs.io/projects/tutorials/en/latest/fpfh_estimation.html
     

src/diffCheck/registration/registration.hh

@@ -12,5 +12,7 @@ class Registration
     open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
 
     open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
+
+    std::vector<double> ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
 };
 }

src/diffCheckApp.cc

@@ -14,6 +14,7 @@ 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::DFPointCloud> dfPointCloudPtrGroundTruth = 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\localuser\Downloads\04_pt.ply)";
@@ -23,6 +24,9 @@ int main()
   dfMeshPtr->LoadFromPLY(pathMesh);
   dfPointCloudPtr->LoadFromPLY(pathCloud);
 
+  // populate the mesh with points and store it in dfPointCloudPtrGroundTruth
+  dfPointCloudPtrGroundTruth->Cvt2DFPointCloud(dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(100000));
+
   // 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 world
@@ -33,17 +37,18 @@ int main()
   dfPointCloudPtrAfterTrans->Cvt2DFPointCloud(o3DPointCloudAfterTrans);
 
   std::shared_ptr<diffCheck::registration::Registration> reg = std::make_shared<diffCheck::registration::Registration>();
-  auto result = reg->O3DFastGlobalRegistrationFeatureMatching(dfPointCloudPtrAfterTrans, dfPointCloudPtr);
-  //auto result = reg->O3DFastGlobalRegistrationBasedOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtr);
+  //auto result = reg->O3DFastGlobalRegistrationFeatureMatching(dfPointCloudPtrAfterTrans, dfPointCloudPtr);
+  auto result = reg->O3DFastGlobalRegistrationBasedOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtr);
 
   // apply the transformation to the source point cloud
   Eigen::Matrix<double, 4, 4> transformation = result.transformation_;
   std::shared_ptr<open3d::geometry::PointCloud> o3DPointCloudPtrAfterReg = std::make_shared<open3d::geometry::PointCloud>(dfPointCloudPtrAfterTrans->Cvt2O3DPointCloud()->Transform(transformation));
   dfPointCloudPtrAfterReg->Cvt2DFPointCloud(o3DPointCloudPtrAfterReg);
-
+  std::vector<double> errors = reg->ComputeP2PDistance(dfPointCloudPtrAfterReg, dfPointCloudPtrGroundTruth);
+  std::cout << "Mean distance: " << std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size() << std::endl;
 
   std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
-  vis->AddPointCloud(dfPointCloudPtrAfterTrans);
+  vis->AddPointCloud(dfPointCloudPtrGroundTruth);
   vis->AddPointCloud(dfPointCloudPtrAfterReg);
   vis->AddMesh(dfMeshPtr);
   vis->Run();