Merge pull request #149 from diffCheckOrg/implement_pca

Add axis calculation component for pose estimation

Author: DamienGilliard

doc/_static/example_files/subtractive_gh_v1.gh

@@ -20,7 +20,8 @@ module = [
     "GH_IO.*",
     "clr.*",
     "diffcheck_bindings",
-    "diffCheck.diffcheck_bindings"
+    "diffCheck.diffcheck_bindings",
+    "ghpythonlib.*"
 ]
 ignore_missing_imports = true
 

pyproject.toml

@@ -6,6 +6,9 @@
 #include <loguru.hpp>
 
 #include <cilantro/cilantro.hpp>
+#include <cilantro/clustering/kmeans.hpp>
+
+#include <Eigen/Dense>
 
 // diffCheck includes
 #include "diffCheck/log.hh"

src/diffCheck.hh

@@ -68,4 +68,11 @@ namespace diffCheck::io
         std::filesystem::path pathCloud = pathTestData / "test_pc_for_SOR_101pts_with_1_outlier.ply";
         return pathCloud.string();
     }
+
+    std::string GetTwoConnectedPlanesPlyPath()
+    {
+        std::filesystem::path pathTestData = GetTestDataDir();
+        std::filesystem::path pathCloud = pathTestData / "two_connected_planes_with_normals.ply";
+        return pathCloud.string();
+    }
 } // namespace diffCheck::io

src/diffCheck/IOManager.cc

@@ -42,4 +42,6 @@ namespace diffCheck::io
     std::string GetRoofQuarterPlyPath();
     /// @brief Get the path to the plane point cloud with one outlier
     std::string GetPlanePCWithOneOutliers();
+    /// @brief Get the path to the two connected planes ply test file
+    std::string GetTwoConnectedPlanesPlyPath();
 } // namespace diffCheck::io

src/diffCheck/IOManager.hh

@@ -216,6 +216,76 @@ namespace diffCheck::geometry
             this->Normals.push_back(normal);
     }
 
+    std::vector<Eigen::Vector3d> DFPointCloud::GetPrincipalAxes(int nComponents)
+    {
+        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) 
+        {
+            sortedClustersBySize[i] = {clusterSizes[i], centroids.col(i)};
+        }
+        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;
+    }
+    
     void DFPointCloud::Crop(const Eigen::Vector3d &minBound, const Eigen::Vector3d &maxBound)
     {
         auto O3DPointCloud = this->Cvt2O3DPointCloud();

src/diffCheck/geometry/DFPointCloud.cc

@@ -8,6 +8,8 @@
 
 #include <cilantro/utilities/point_cloud.hpp>
 #include <cilantro/core/nearest_neighbors.hpp>
+#include <cilantro/clustering/kmeans.hpp>
+
 
 namespace diffCheck::geometry
 {
@@ -89,6 +91,14 @@ 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
+         */
+        std::vector<Eigen::Vector3d> GetPrincipalAxes(int nComponents = 6);
+
         /**
          *  @brief Crop the point cloud to a bounding box defined by the min and max bounds
          * 

src/diffCheck/geometry/DFPointCloud.hh

@@ -61,6 +61,8 @@ PYBIND11_MODULE(diffcheck_bindings, m) {
         .def("remove_statistical_outliers", &diffCheck::geometry::DFPointCloud::RemoveStatisticalOutliers, 
             py::arg("nb_neighbors"), py::arg("std_ratio"))
 
+        .def("get_principal_axes", &diffCheck::geometry::DFPointCloud::GetPrincipalAxes,
+            py::arg("n_components") = 6)
         .def("crop",
             (void (diffCheck::geometry::DFPointCloud::*)(const Eigen::Vector3d&, const Eigen::Vector3d&))
             &diffCheck::geometry::DFPointCloud::Crop,

src/diffCheckBindings.cc

@@ -0,0 +1,68 @@
+#! python3
+
+from diffCheck import df_cvt_bindings
+from diffCheck import df_poses
+
+import Rhino
+from Grasshopper.Kernel import GH_RuntimeMessageLevel as RML
+
+from ghpythonlib.componentbase import executingcomponent as component
+import System
+
+
+class DFPoseEstimation(component):
+    def RunScript(self,
+            i_clouds: System.Collections.Generic.List[Rhino.Geometry.PointCloud],
+            i_assembly,
+            i_save: bool,
+            i_reset: bool):
+
+        # ensure assembly has enough beams
+        if len(i_assembly.beams) < len(i_clouds):
+            ghenv.Component.AddRuntimeMessage(RML.Warning, "Assembly has fewer beams than input clouds")  # noqa: F821
+            return None, None
+
+        planes = []
+        all_poses_in_time = df_poses.DFPosesAssembly()
+        if i_reset:
+            all_poses_in_time.reset()
+            return None, None
+
+        all_poses_this_time = []
+        for i, cloud in enumerate(i_clouds):
+            try:
+                df_cloud = df_cvt_bindings.cvt_rhcloud_2_dfcloud(cloud)
+                if df_cloud is None:
+                    return None, None
+                if not df_cloud.has_normals():
+                    ghenv.Component.AddRuntimeMessage(RML.Error, f"Point cloud {i} has no normals. Please compute the normals.")  # noqa: F821
+
+                df_points = df_cloud.get_axis_aligned_bounding_box()
+                df_point = (df_points[0] + df_points[1]) / 2
+                rh_point = Rhino.Geometry.Point3d(df_point[0], df_point[1], df_point[2])
+
+                axes = df_cloud.get_principal_axes(3)
+                vectors = []
+                for axe in axes:
+                    vectors.append(Rhino.Geometry.Vector3d(axe[0], axe[1], axe[2]))
+
+                new_xDirection, new_yDirection = df_poses.select_vectors(vectors, i_assembly.beams[i].plane.XAxis, i_assembly.beams[i].plane.YAxis)
+
+                pose = df_poses.DFPose(
+                    origin = [rh_point.X, rh_point.Y, rh_point.Z],
+                    xDirection = [new_xDirection.X, new_xDirection.Y, new_xDirection.Z],
+                    yDirection = [new_yDirection.X, new_yDirection.Y, new_yDirection.Z])
+                all_poses_this_time.append(pose)
+                plane = Rhino.Geometry.Plane(origin = rh_point, xDirection=new_xDirection, yDirection=new_yDirection)
+                planes.append(plane)
+            except Exception as e:
+                # Any unexpected error on this cloud, skip it and keep going
+                ghenv.Component.AddRuntimeMessage(RML.Error, f"Cloud {i}: processing failed ({e}); skipping.")  # noqa: F821
+                planes.append(None)
+                all_poses_this_time.append(None)
+                continue
+
+        if i_save:
+            all_poses_in_time.add_step(all_poses_this_time)
+
+        return [planes, all_poses_in_time.to_gh_tree()]