feat: add point cloud intersection calculation to DFPointCloud

Author: DamienGilliard

src/diffCheck/geometry/DFPointCloud.cc

@@ -322,6 +322,42 @@ namespace diffCheck::geometry
         }
     }
 
+    diffCheck::geometry::DFPointCloud DFPointCloud::Intersect(const DFPointCloud &pointCloud, double distanceThreshold)
+    {
+        if (this->Points.size() == 0 || pointCloud.Points.size() == 0)
+            throw std::invalid_argument("One of the point clouds is empty.");
+        
+        auto O3DSourcePointCloud = this->Cvt2O3DPointCloud();
+        auto O3DTargetPointCloud = std::make_shared<DFPointCloud>(pointCloud)->Cvt2O3DPointCloud();
+        auto O3DResultPointCloud = std::make_shared<open3d::geometry::PointCloud>();
+
+        open3d::geometry::KDTreeFlann threeDTree;
+        threeDTree.SetGeometry(*O3DTargetPointCloud);
+        std::vector<int> indices;
+        std::vector<double> distances;
+        for (const auto &point : O3DSourcePointCloud->points_)
+        {
+            threeDTree.SearchRadius(point, distanceThreshold, indices, distances);
+            if (!indices.empty())
+            {
+                O3DResultPointCloud->points_.push_back(point);
+                if (O3DSourcePointCloud->HasColors())
+                {
+                    O3DResultPointCloud->colors_.push_back(O3DSourcePointCloud->colors_[&point - &O3DSourcePointCloud->points_[0]]);
+                }
+                if (O3DSourcePointCloud->HasNormals())
+                {
+                    O3DResultPointCloud->normals_.push_back(O3DSourcePointCloud->normals_[&point - &O3DSourcePointCloud->points_[0]]);
+                }
+            }
+        }
+        diffCheck::geometry::DFPointCloud result;
+        result.Points = O3DResultPointCloud->points_;
+        result.Colors = O3DResultPointCloud->colors_;
+        result.Normals = O3DResultPointCloud->normals_;
+        return result;
+    }
+
     void DFPointCloud::ApplyTransformation(const diffCheck::transformation::DFTransformation &transformation)
     {
         auto O3DPointCloud = this->Cvt2O3DPointCloud();

src/diffCheck/geometry/DFPointCloud.hh

@@ -152,7 +152,7 @@ namespace diffCheck::geometry
         */
         std::vector<Eigen::Vector3d> GetTightBoundingBox();
 
-    public:  ///< Point cloud subtraction
+    public:  ///< Point cloud subtraction and intersection
         /**
          * @brief Subtract the points, colors and normals from another point cloud when they are too close to the points of another point cloud.
          * 
@@ -161,6 +161,15 @@ namespace diffCheck::geometry
          */
         void SubtractPoints(const DFPointCloud &pointCloud, double distanceThreshold = 0.01);
 
+        /**
+         * @brief Intersect the points, colors and normals from another point cloud when they are close enough to the points of another point cloud. Is the point cloud interpretation of a boolean intersection.
+         * 
+         * @param pointCloud the other point cloud to intersect with this one
+         * @param distanceThreshold the distance threshold to consider a point as too close. Default is 0.01.
+         * @return diffCheck::geometry::DFPointCloud the intersected point cloud
+         */
+        diffCheck::geometry::DFPointCloud Intersect(const DFPointCloud &pointCloud, double distanceThreshold = 0.01);
+
     public:  ///< Transformers
         /**
          * @brief Apply a transformation to the point cloud