WIP: ApplyTransformation method

Author: DamienGilliard

src/diffCheck/geometry/DFPointCloud.cc

@@ -49,6 +49,9 @@ namespace diffCheck::geometry
     {
         auto O3DPointCloud = this->Cvt2O3DPointCloud();
         O3DPointCloud->Transform(transformation.TransformationMatrix);
+        this->Points.clear();
+        this->Colors.clear();
+        this->Normals.clear();
         this->Cvt2DFPointCloud(O3DPointCloud);
     }
 

src/diffCheck/registrations/DFGlobalRegistrations.hh

@@ -71,10 +71,10 @@ namespace diffCheck::registrations
             int maxNeighborKDTreeSearch  = 50,
             double maxCorrespondenceDistance = 0.05,
             open3d::pipelines::registration::TransformationEstimationPointToPoint transformationEstimation = open3d::pipelines::registration::TransformationEstimationPointToPoint(false),
-            int ransacN = 3,
+            int ransacN = 5,
             double correspondenceCheckerDistance = 0.05,
             int ransacMaxIteration = 1000,
-            double ransacConfidenceThreshold = 0.999);
+            double ransacConfidenceThreshold = 0.99);
 
     private: ///< o3d utilities to evaluate registration errors
         /**
@@ -86,7 +86,6 @@ namespace diffCheck::registrations
          * @param transform The vector of transformation matrix we want to evaluate. they are applied to the source point cloud.
          * @return std::vector<double> A vector of mean distances, one for each transform.
         */
-
         static std::vector<double> EvaluateRegistrations(std::shared_ptr<geometry::DFPointCloud> source, 
                                                                     std::shared_ptr<geometry::DFPointCloud> target,
                                                                     std::vector<Eigen::Matrix<double, 4, 4>> transforms);

src/diffCheckApp.cc

@@ -9,57 +9,54 @@ int main()
 {
   // import clouds
   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> dfPointCloudPtrGroundTruthNoNormals = 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\andre\Downloads\00_pt.ply)";
-  std::string pathMesh = R"(C:\Users\andre\Downloads\00_mesh.ply)";
+  std::string pathCloud = R"(C:\Users\localuser\Downloads\04_pt.ply)";
+  std::string pathMesh = R"(C:\Users\localuser\Downloads\04_mesh.ply)";
 
   dfMeshPtr->LoadFromPLY(pathMesh);
   dfPointCloudPtr->LoadFromPLY(pathCloud);
-
   // add noise to dfPointCloudPtr
   for (int i = 0; i < dfPointCloudPtr->Points.size(); i++)
   {
-    dfPointCloudPtr->Points[i] += Eigen::Vector3d::Random() * 0.1 ;
+    dfPointCloudPtr->Points[i] += Eigen::Vector3d::Random() * 0.01 ;
   }
 
   // populate the mesh with points and store it in dfPointCloudPtrGroundTruth
-  dfPointCloudPtrGroundTruthNoNormals->Cvt2DFPointCloud(dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(10000));
+  dfPointCloudPtrGroundTruthNoNormals->Cvt2DFPointCloud(dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(1000));
   std::shared_ptr<open3d::geometry::PointCloud> pcd = dfPointCloudPtrGroundTruthNoNormals->Cvt2O3DPointCloud();
   pcd->EstimateNormals();
   dfPointCloudPtrGroundTruth->Cvt2DFPointCloud(pcd);
-  
+
   // create a rigid rotation matrix
   Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
-  T.block<3, 3>(0, 0) = Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitX()).toRotationMatrix();
-  T(0, 3) = 5;
-  T(1, 3) = -10;
-  T(2, 3) = 10;
+  T.block<3, 3>(0, 0) = Eigen::AngleAxisd(3, Eigen::Vector3d::UnitX()).toRotationMatrix();
+  T(0, 3) = 50;
+  T(1, 3) = -100;
+  T(2, 3) = 100;
   dfPointCloudPtr->ApplyTransformation(diffCheck::transformation::DFTransformation(T));
-  dfPointCloudPtrAfterTrans = dfPointCloudPtr;
+  std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrCopy = std::make_shared<diffCheck::geometry::DFPointCloud>(*dfPointCloudPtr);
 
   // test global registrations Fast and RANSAC-based
   std::vector<diffCheck::transformation::DFTransformation> registrationResults;
-
   diffCheck::transformation::DFTransformation transformationA =
-    diffCheck::registrations::DFGlobalRegistrations::O3DFastGlobalRegistrationFeatureMatching(dfPointCloudPtr, dfPointCloudPtrAfterTrans);
-  std::cout << "Fast transformation: " << transformationA.TransformationMatrix << std::endl;
-  dfPointCloudPtrAfterTrans->ApplyTransformation(transformationA);
+    diffCheck::registrations::DFGlobalRegistrations::O3DFastGlobalRegistrationFeatureMatching(dfPointCloudPtr, dfPointCloudPtrGroundTruth, true, 0.01, 1, 50, 1, 500, 500);
+  std::cout << "Fast transformation: " << transformationA.TransformationMatrix << std::flush;
+  dfPointCloudPtr->ApplyTransformation(transformationA);
   registrationResults.push_back(transformationA);
-
   diffCheck::transformation::DFTransformation transformationB =
-    diffCheck::registrations::DFGlobalRegistrations::O3DRansacOnFeatureMatching(dfPointCloudPtr, dfPointCloudPtrAfterTrans);
+    diffCheck::registrations::DFGlobalRegistrations::O3DRansacOnFeatureMatching(dfPointCloudPtrCopy, dfPointCloudPtrGroundTruth);
   std::cout << "Ransac transformation: " << transformationB.TransformationMatrix << std::endl;
-  dfPointCloudPtrAfterTrans->ApplyTransformation(transformationB);
+  dfPointCloudPtrCopy->ApplyTransformation(transformationB);
   registrationResults.push_back(transformationB);
 
   // visualize cloud
   std::shared_ptr<diffCheck::visualizer::Visualizer> visualizer = std::make_shared<diffCheck::visualizer::Visualizer>();
   visualizer->AddPointCloud(dfPointCloudPtr);
-  visualizer->AddPointCloud(dfPointCloudPtrAfterTrans);
+  visualizer->AddPointCloud(dfPointCloudPtrCopy);
+  visualizer->AddMesh(dfMeshPtr);
   visualizer->Run();
 
   return 0;