feat: add FitPlaneRANSAC method to DFPointCloud

Author: DamienGilliard

src/diffCheck/geometry/DFPointCloud.cc

@@ -216,74 +216,21 @@ namespace diffCheck::geometry
             this->Normals.push_back(normal);
     }
 
-    std::vector<Eigen::Vector3d> DFPointCloud::GetPrincipalAxes(int nComponents)
+    Eigen::Vector3d DFPointCloud::FitPlaneRANSAC(
+            double distanceThreshold,
+            int ransacN,
+            int numIterations)
     {
-        std::vector<Eigen::Vector3d> principalAxes;
-
-        if (! this->HasNormals())
-        {
-            DIFFCHECK_WARN("The point cloud has no normals. Normals will be estimated with knn = 20.");
-            this->EstimateNormals(true, 20);
-        }
-
-        // Convert normals to Eigen matrix
-        Eigen::Matrix<double, 3, Eigen::Dynamic> normalMatrix(3, this->Normals.size());
-        for (size_t i = 0; i < this->Normals.size(); ++i)
-        {
-            normalMatrix.col(i) = this->Normals[i].cast<double>();
-        }
-
-        cilantro::KMeans<double, 3> kmeans(normalMatrix); 
-        kmeans.cluster(nComponents);
-
-        const cilantro::VectorSet3d& centroids = kmeans.getClusterCentroids();
-        const std::vector<size_t>& assignments = kmeans.getPointToClusterIndexMap();
-        std::vector<int> clusterSizes(nComponents, 0);
-        for (size_t i = 0; i < assignments.size(); ++i) 
-        {
-            clusterSizes[assignments[i]]++;
-        }
-        // Sort clusters by size
-        std::vector<std::pair<int, Eigen::Vector3d>> sortedClustersBySize(nComponents);
-        for (size_t i = 0; i < nComponents; ++i) 
+        if (this->Points.size() < ransacN)
         {
-            sortedClustersBySize[i] = {clusterSizes[i], centroids.col(i)};
+            DIFFCHECK_ERROR("Not enough points to fit a plane with RANSAC.");
+            return Eigen::Vector3d::Zero();
         }
-        std::sort(sortedClustersBySize.begin(), sortedClustersBySize.end(), [](const auto& a, const auto& b) 
-        {
-            return a.first > b.first;
-        });
-
-        for(size_t i = 0; i < nComponents; ++i) 
-        {
-            if(principalAxes.size() == 0)
-            {
-                principalAxes.push_back(sortedClustersBySize[i].second);
-            }
-            else
-            {
-                bool isAlreadyPresent = false;
-                for (const auto& axis : principalAxes)
-                {
-                    double dotProduct = std::abs(axis.dot(sortedClustersBySize[i].second));
-                    if (std::abs(dotProduct) > 0.7) // Threshold to consider as similar direction
-                    {
-                        isAlreadyPresent = true;
-                        break;
-                    }
-                }
-                if (!isAlreadyPresent)
-                {
-                    principalAxes.push_back(sortedClustersBySize[i].second);
-                }
-            }
-        }
-        if (principalAxes.size() < 2) // Fallback to OBB if k-means fails to provide enough distinct axes
-        {
-            open3d::geometry::OrientedBoundingBox obb = this->Cvt2O3DPointCloud()->GetOrientedBoundingBox();
-            principalAxes = {obb.R_.col(0), obb.R_.col(1), obb.R_.col(2)};
-        }
-        return principalAxes;
+        
+        auto O3DPointCloud = this->Cvt2O3DPointCloud();
+        std::tuple< Eigen::Vector4d, std::vector<size_t>> planeModel = O3DPointCloud->SegmentPlane(distanceThreshold, ransacN, numIterations);
+        Eigen::Vector3d planeParameters = std::get<0>(planeModel).head<3>();
+        return planeParameters;
     }
 
     void DFPointCloud::Crop(const Eigen::Vector3d &minBound, const Eigen::Vector3d &maxBound)

src/diffCheck/geometry/DFPointCloud.hh

@@ -92,12 +92,17 @@ namespace diffCheck::geometry
         void RemoveStatisticalOutliers(int nbNeighbors, double stdRatio);
 
         /**
-         * @brief Get the nCompoments principal axes of the normals of the point cloud
-         * It is used to compute the pose of "boxy" point clouds. It relies on KMeans clustering to find the main axes of the point cloud.
-         *  @param nComponents the number of components to compute (default 6, each of 3 main axes in both directions)
-         * @return std::vector<Eigen::Vector3d> the principal axes of the point cloud ordered by number of normals
+         * @brief Fit a plane to the point cloud using RANSAC
+         * 
+         * @param distanceThreshold the distance threshold to consider a point as an inlier
+         * @param ransacN the number of points to sample for each RANSAC iteration
+         * @param numIterations the number of RANSAC iterations
+         * @return The Normal vector of the fitted plane as an Eigen::Vector3d
          */
-        std::vector<Eigen::Vector3d> GetPrincipalAxes(int nComponents = 6);
+        Eigen::Vector3d FitPlaneRANSAC(
+            double distanceThreshold = 0.01,
+            int ransacN = 3,
+            int numIterations = 100);
 
         /**
          *  @brief Crop the point cloud to a bounding box defined by the min and max bounds