TEMP: working on colored cluster + visualizer

Author: 9and3

src/diffCheck.hh

@@ -3,6 +3,8 @@
 #include <open3d/Open3D.h>
 #include <loguru.hpp>
 
+#include <cilantro/cilantro.hpp>
+
 // diffCheck includes
 #include "diffCheck/log.hh"
 const diffCheck::Log LOG = diffCheck::Log();

src/diffCheck/geometry/DFPointCloud.cc

@@ -8,6 +8,10 @@ namespace diffCheck::geometry
 {
     void DFPointCloud::Cvt2DFPointCloud(const std::shared_ptr<open3d::geometry::PointCloud> &O3DPointCloud)
     {
+        this->Points.clear();
+        this->Colors.clear();
+        this->Normals.clear();
+
         if (O3DPointCloud->points_.size() != 0)
             for (auto &point : O3DPointCloud->points_)
                 this->Points.push_back(point);
@@ -19,6 +23,44 @@ namespace diffCheck::geometry
                 this->Normals.push_back(normal);
     }
 
+    void DFPointCloud::Cvt2DFPointCloud(const std::shared_ptr<cilantro::PointCloud3f> &cilantroPointCloud)
+    {
+        this->Points.clear();
+        this->Colors.clear();
+        this->Normals.clear();
+        
+        auto ptt = cilantroPointCloud->points;
+        int n_pt = (int)ptt.cols();
+        auto col = cilantroPointCloud->colors;
+        auto nor = cilantroPointCloud->normals;
+
+        if (n_pt == 0)
+            throw std::invalid_argument("The point cloud is empty.");
+        for (int i = 0; i < n_pt; i++)
+        {
+            Eigen::Vector3d pt_d = ptt.col(i).cast<double>();
+            this->Points.push_back(pt_d);
+        }
+
+        if (cilantroPointCloud->hasColors())
+        {
+            for (int i = 0; i < n_pt; i++)
+            {
+                Eigen::Vector3d cl_d = col.col(i).cast <double>();
+                this->Colors.push_back(cl_d);
+            }
+        }
+
+        if (cilantroPointCloud->hasNormals())
+        {
+            for (int i = 0; i < n_pt; i++)
+            {
+                Eigen::Vector3d no_d = nor.col(i).cast <double>();
+                this->Normals.push_back(no_d);
+            }
+        }
+    }
+
     std::shared_ptr<open3d::geometry::PointCloud> DFPointCloud::Cvt2O3DPointCloud()
     {
         std::shared_ptr<open3d::geometry::PointCloud> O3DPointCloud(new open3d::geometry::PointCloud());
@@ -34,6 +76,46 @@ namespace diffCheck::geometry
         return O3DPointCloud;
     }
 
+    std::shared_ptr<cilantro::PointCloud3f> DFPointCloud::Cvt2CilantroPointCloud()
+    {
+        std::shared_ptr<cilantro::PointCloud3f> cilantroPointCloud = std::make_shared<cilantro::PointCloud3f>();
+
+        cilantro::VectorSet3f points;
+        for (auto& pt : this->Points)
+        {
+            Eigen::Vector3f pt_f = pt.cast <float>();
+            points.conservativeResize(points.rows(), points.cols() + 1);
+            points.col(points.cols() - 1) = pt_f;
+        }
+        cilantroPointCloud->points = points;
+
+        cilantro::VectorSet3f colors;
+        if (this->HasColors())
+        {
+            for (auto& color : this->Colors)
+            {
+                Eigen::Vector3f color_f = color.cast <float>();
+                colors.conservativeResize(colors.rows(), colors.cols() + 1);
+                colors.col(colors.cols() - 1) = color_f;
+            }
+        }
+        cilantroPointCloud->colors = colors;
+
+        cilantro::VectorSet3f normals;
+        if (this->HasNormals())
+        {
+            for (auto& normal : this->Normals)
+            {
+                Eigen::Vector3f normal_f = normal.cast <float>();
+                normals.conservativeResize(normals.rows(), normals.cols() + 1);
+                normals.col(normals.cols() - 1) = normal_f;
+            }
+        }
+        cilantroPointCloud->normals = normals;
+
+        return cilantroPointCloud;
+    }
+
     std::vector<double> DFPointCloud::ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> target)
     {
         std::vector<double> errors;
@@ -57,34 +139,49 @@ namespace diffCheck::geometry
     }
 
     void DFPointCloud::EstimateNormals(
+        bool useCilantroEvaluator,
         std::optional<int> knn,
         std::optional<double> searchRadius
     )
     {
-        auto O3DPointCloud = this->Cvt2O3DPointCloud();
-        O3DPointCloud->EstimateNormals();
-
-        if (knn.value() != 30 && searchRadius.has_value() == false)
-        {
-            open3d::geometry::KDTreeSearchParamKNN knnSearchParam(knn.value());
-            O3DPointCloud->EstimateNormals(knnSearchParam);
-            DIFFCHECK_INFO(("Estimating normals with knn = " + std::to_string(knn.value())).c_str());
-        }
-        else if (searchRadius.has_value())
+        if (!useCilantroEvaluator)
         {
-            open3d::geometry::KDTreeSearchParamHybrid hybridSearchParam(searchRadius.value(), knn.value());
-            O3DPointCloud->EstimateNormals(hybridSearchParam);
-            DIFFCHECK_INFO(("Estimating normals with hybrid search radius = " + std::to_string(searchRadius.value()) + "and knn = " + std::to_string(knn.value())).c_str());
+            auto O3DPointCloud = this->Cvt2O3DPointCloud();
+
+            if (knn.value() != 30 && searchRadius.has_value() == false)
+            {
+                open3d::geometry::KDTreeSearchParamKNN knnSearchParam(knn.value());
+                O3DPointCloud->EstimateNormals(knnSearchParam);
+                DIFFCHECK_INFO(("Estimating normals with knn = " + std::to_string(knn.value())).c_str());
+            }
+            else if (searchRadius.has_value())
+            {
+                open3d::geometry::KDTreeSearchParamHybrid hybridSearchParam(searchRadius.value(), knn.value());
+                O3DPointCloud->EstimateNormals(hybridSearchParam);
+                DIFFCHECK_INFO(("Estimating normals with hybrid search radius = " + std::to_string(searchRadius.value()) + "and knn = " + std::to_string(knn.value())).c_str());
+            }
+            else
+            {
+                O3DPointCloud->EstimateNormals();
+                DIFFCHECK_INFO("Default estimation of normals with knn = 30");
+            }
+
+            this->Normals.clear();
+            for (auto &normal : O3DPointCloud->normals_)
+                this->Normals.push_back(normal);
         }
         else
         {
-            O3DPointCloud->EstimateNormals();
-            DIFFCHECK_INFO("Default estimation of normals with knn = 30");
+            std::shared_ptr<cilantro::PointCloud3f> cilantroPointCloud = this->Cvt2CilantroPointCloud();
+            cilantro::KNNNeighborhoodSpecification<int> neighborhood(knn.value());
+            cilantroPointCloud->estimateNormals(neighborhood, false);
+
+            this->Normals.clear();
+            for (int i = 0; i < cilantroPointCloud->normals.cols(); i++)
+                this->Normals.push_back(cilantroPointCloud->normals.col(i).cast<double>());
+            DIFFCHECK_INFO(("Estimating normals with cilantro evaluator with knn = " + std::to_string(knn.value())).c_str());
         }
 
-        this->Normals.clear();
-        for (auto &normal : O3DPointCloud->normals_)
-            this->Normals.push_back(normal);
     }
 
     void DFPointCloud::VoxelDownsample(double voxelSize)

src/diffCheck/geometry/DFPointCloud.hh

@@ -6,6 +6,8 @@
 
 #include <diffCheck/transformation/DFTransformation.hh>
 
+#include <cilantro/utilities/point_cloud.hpp>
+#include <cilantro/core/nearest_neighbors.hpp>
 
 namespace diffCheck::geometry
 {
@@ -29,14 +31,22 @@ namespace diffCheck::geometry
          * @param pointCloud the open3d point cloud
          */
         void Cvt2DFPointCloud(const std::shared_ptr<open3d::geometry::PointCloud> &O3DPointCloud);
-        
+        void Cvt2DFPointCloud(const std::shared_ptr<cilantro::PointCloud3f> &cilantroPointCloud);
+
         /**
          * @brief Convert the DFPointCloud to open3d point cloud
          * 
          * @return std::shared_ptr<open3d::geometry::PointCloud> the open3d point cloud
          */
         std::shared_ptr<open3d::geometry::PointCloud> Cvt2O3DPointCloud();
 
+        /**
+         * @brief Convert DFPointCloud to cilantro point cloud
+         * 
+         * @return std::shared_ptr<cilantro::PointCloud3f> the cilantro point cloud
+         */
+        std::shared_ptr<cilantro::PointCloud3f> Cvt2CilantroPointCloud();
+
     public:  ///< Utilities
         /** 
         * @brief Compute the "point to point" distance between this and another point clouds. 
@@ -64,11 +74,14 @@ namespace diffCheck::geometry
          * is provided by hybrid search.
          * 
          * <a href=https://www.open3d.org/html/cpp_api/classopen3d_1_1t_1_1geometry_1_1_point_cloud.html#a4937528c4b6194092631f002bccc44d0> Reference from Open3d</a>.
+         *
+         * @param useCilantroEvaluator if true, the cilantro evaluator will be used, otherwise the open3d one
          * @param knn the number of nearest neighbors to consider (by default 30)
-         * @param searchRadius the radius of the search, by default deactivated
+         * @param searchRadius the radius of the search, by default deactivated (only if useCilantroEvaluator is false)
          */
         void EstimateNormals(
-            std::optional<int> knn = 30,
+            bool useCilantroEvaluator = false,
+            std::optional<int> knn = 10,
             std::optional<double> searchRadius = std::nullopt);
 
     public:  ///< Downsamplers

src/diffCheck/segmentation/DFSegmentation.cc

@@ -7,54 +7,55 @@
 namespace diffCheck::segmentation
 {   
     std::vector<std::shared_ptr<geometry::DFPointCloud>> DFSegmentation::NormalBasedSegmentation(
-        geometry::DFPointCloud &pointCloud,
-        float voxelSize,
+        std::shared_ptr<geometry::DFPointCloud> &pointCloud,
         float normalThresholdDegree,
         int minClusterSize,
         bool useKnnNeighborhood,
         int knnNeighborhoodSize,
-        int radiusNeighborhoodSize)
+        int radiusNeighborhoodSize,
+        bool colorClusters)
     {
-        std::vector<std::shared_ptr<geometry::DFPointCloud>> segments;
-        cilantro::PointCloud3f cilantroPointCloud;
-        
-        // Convert the point cloud to cilantro point cloud
-        for (int i = 0; i < pointCloud.Points.size();  i++)
-        {
-            cilantroPointCloud.points.conservativeResize(3, cilantroPointCloud.points.cols() + 1);
-            cilantroPointCloud.points.rightCols(1) = pointCloud.Points[i].cast<float>();
-        }
+        std::shared_ptr<cilantro::PointCloud3f> cilantroPointCloud = pointCloud->Cvt2CilantroPointCloud();
 
-        // segment the point cloud using knn or radius neighborhood
+        std::vector<std::shared_ptr<geometry::DFPointCloud>> segments;
         if (useKnnNeighborhood)
         {
             cilantro::KNNNeighborhoodSpecification<int> neighborhood(knnNeighborhoodSize);
 
-            // conpute the normals and downsample
-            cilantroPointCloud.estimateNormals(neighborhood, false);
-            cilantroPointCloud.gridDownsample(voxelSize);
+            cilantroPointCloud->estimateNormals(neighborhood);
 
-            // Similarity evaluator
             cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
-            cilantroPointCloud.normals, 
+            cilantroPointCloud->normals, 
             normalThresholdDegree*M_PI/180.0f);
 
-            // Segment the point cloud
-            cilantro::ConnectedComponentExtraction3f<> segmenter(cilantroPointCloud.points);
+            cilantro::ConnectedComponentExtraction3f<> segmenter(cilantroPointCloud->points);
             segmenter.segment(neighborhood, similarityEvaluator, minClusterSize);
             auto clusterToPointMap = segmenter.getClusterToPointIndicesMap();
             int nSegments = segmenter.getNumberOfClusters();
 
-            // Get the segments
+            // FIXME: painting color not working (see paint_uniform?)
             for (int indice = 0; indice<nSegments; indice++)
             {
                 std::shared_ptr<geometry::DFPointCloud> segment = std::make_shared<geometry::DFPointCloud>();
+                
+                // random rgb color in format Eigen::Vector3d from 0 to 1
+                Eigen::Vector3d color = Eigen::Vector3d::Random();
                 for (auto pointIndice : clusterToPointMap[indice])
                 {
-                    Eigen::Vector3d point = cilantroPointCloud.points.col(pointIndice).cast<double>();
-                    Eigen::Vector3d normal = cilantroPointCloud.normals.col(pointIndice).cast<double>();
+                    Eigen::Vector3d point = cilantroPointCloud->points.col(pointIndice).cast<double>();
+                    Eigen::Vector3d normal = cilantroPointCloud->normals.col(pointIndice).cast<double>();
                     segment->Points.push_back(point);
                     segment->Normals.push_back(normal);
+                    if (colorClusters)
+                        segment->Colors.push_back(color);
+                    else
+                    {
+                        if (cilantroPointCloud->colors.cols() > 0)
+                        {
+                            Eigen::Vector3d color = cilantroPointCloud->colors.col(pointIndice).cast<double>();
+                            segment->Colors.push_back(color);
+                        }
+                    }
                 }
                 segments.push_back(segment);
             }
@@ -63,37 +64,43 @@ namespace diffCheck::segmentation
         {
             cilantro::RadiusNeighborhoodSpecification<float> neighborhood(radiusNeighborhoodSize);
 
-            // conpute the normals and downsample
-            cilantroPointCloud.estimateNormals(neighborhood, false);
-            cilantroPointCloud.gridDownsample(voxelSize);
-            
-            // Similarity evaluator
+            cilantroPointCloud->estimateNormals(neighborhood);
+
             cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
-            cilantroPointCloud.normals, 
+            cilantroPointCloud->normals,
             normalThresholdDegree*M_PI/180.0f);
 
-            // Segment the point cloud
-            cilantro::ConnectedComponentExtraction3f<> segmenter(cilantroPointCloud.points);
+            cilantro::ConnectedComponentExtraction3f<> segmenter(cilantroPointCloud->points);
             segmenter.segment(neighborhood, similarityEvaluator, minClusterSize);
 
             auto clusterToPointMap = segmenter.getClusterToPointIndicesMap();
             int nSegments = segmenter.getNumberOfClusters();
 
-            // Get the segments
+            // FIXME: painting color not working (see paint_uniform?)
             for (int indice = 0; indice<nSegments; indice++)
             {
                 std::shared_ptr<geometry::DFPointCloud> segment = std::make_shared<geometry::DFPointCloud>();
+                Eigen::Vector3d color = Eigen::Vector3d::Random();
                 for (auto pointIndice : clusterToPointMap[indice])
                 {
-                    Eigen::Vector3d point = cilantroPointCloud.points.col(pointIndice).cast<double>();
-                    Eigen::Vector3d normal = cilantroPointCloud.normals.col(pointIndice).cast<double>();
+                    Eigen::Vector3d point = cilantroPointCloud->points.col(pointIndice).cast<double>();
+                    Eigen::Vector3d normal = cilantroPointCloud->normals.col(pointIndice).cast<double>();
                     segment->Points.push_back(point);
                     segment->Normals.push_back(normal);
+                    if (colorClusters)
+                        segment->Colors.push_back(color);
+                    else
+                    {
+                        if (cilantroPointCloud->colors.cols() > 0)
+                        {
+                            Eigen::Vector3d color = cilantroPointCloud->colors.col(pointIndice).cast<double>();
+                            segment->Colors.push_back(color);
+                        }
+                    }
                 }
                 segments.push_back(segment);
             }
         }
-        
 
         return segments;
     }

src/diffCheck/segmentation/DFSegmentation.hh

@@ -8,21 +8,21 @@ namespace diffCheck::segmentation
         public:
         /** @brief Downsamples and segments the point cloud using Cilantro's ConnectedComponentExtraction3f method. It uses the normals' variations to detect different parts in the point cloud.
          * @param pointCloud the point cloud to segment
-         * @param voxelSize the voxel size for the downsampling of the point cloud. The point cloud is downsampled after the normal calculation. A lower number will result in a denser point cloud
          * @param normalThresholdDegree the normal threshold in degrees do differentiate segments. The higher the number, the more tolerent the segmentation will be to normal differences
          * @param minClusterSize the minimum cluster size to consider a segment. A lower number will discard smaller segments
          * @param useKnnNeighborhood if true, the neighborhood search will be done using the knnNeighborhoodSize, otherwise it will be done using radiusNeighborhoodSize
          * @param knnNeighborhoodSize the k nearest neighbors size for the "neighborhood search". This is used when useKnnNeighborhood is true. a higher number will result in smoother segmentation, but at the cost of computation time
          * @param radiusNeighborhoodSize the radius of the neighborhood size for the "radius search". This is used when useKnnNeighborhood is false. A higher number will result in smoother segmentation, but at the cost of computation time.
+         * @param colorClusters if true, the clusters will be colored with random colors
          * @return std::vector<std::shared_ptr<geometry::DFPointCloud>> the segmented point clouds
          */
         static std::vector<std::shared_ptr<geometry::DFPointCloud>> NormalBasedSegmentation(
-            geometry::DFPointCloud &pointCloud,
-            float voxelSize = 0.2,
+            std::shared_ptr<geometry::DFPointCloud> &pointCloud,
             float normalThresholdDegree = 20,
             int minClusterSize = 10,
             bool useKnnNeighborhood = true,
             int knnNeighborhoodSize = 10,
-            int radiusNeighborhoodSize = 10);
+            int radiusNeighborhoodSize = 10,
+            bool colorClusters = false);
     };
 } // namespace diffCheck::segmentation