WIP: adding coloring for cloud and clustering

Author: 9and3

src/diffCheck/geometry/DFPointCloud.cc

@@ -201,6 +201,18 @@ namespace diffCheck::geometry
             this->Normals.push_back(normal);
     }
 
+    void DFPointCloud::ApplyColor(const Eigen::Vector3d &color)
+    {
+        this->Colors.clear();
+        for (auto &point : this->Points)
+            this->Colors.push_back(color);
+    }
+    void DFPointCloud::ApplyColor(int r, int g, int b)
+    {
+        Eigen::Vector3d color = Eigen::Vector3d(r / 255.0, g / 255.0, b / 255.0);
+        this->ApplyColor(color);
+    }
+
     void DFPointCloud::UniformDownsample(int everyKPoints)
     {
         auto O3DPointCloud = this->Cvt2O3DPointCloud();

src/diffCheck/geometry/DFPointCloud.hh

@@ -84,6 +84,14 @@ namespace diffCheck::geometry
             std::optional<int> knn = 10,
             std::optional<double> searchRadius = std::nullopt);
 
+        /**
+         * @brief Paint the point cloud with a uniform color
+         * 
+         * @param color the color to paint the point cloud
+         */
+        void ApplyColor(const Eigen::Vector3d &color);
+        void ApplyColor(int r, int g, int b);
+
     public:  ///< Downsamplers
         /**
          * @brief Downsample the point cloud with voxel grid

src/diffCheck/segmentation/DFSegmentation.cc

@@ -22,6 +22,7 @@ namespace diffCheck::segmentation
         {
             cilantro::KNNNeighborhoodSpecification<int> neighborhood(knnNeighborhoodSize);
 
+            // FIXME: not clear why this is needed all the time
             cilantroPointCloud->estimateNormals(neighborhood);
 
             cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
@@ -33,38 +34,31 @@ namespace diffCheck::segmentation
             auto clusterToPointMap = segmenter.getClusterToPointIndicesMap();
             int nSegments = segmenter.getNumberOfClusters();
 
-            // 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>();
                     segment->Points.push_back(point);
                     segment->Normals.push_back(normal);
-                    if (colorClusters)
-                        segment->Colors.push_back(color);
-                    else
+                    if (cilantroPointCloud->hasColors())
                     {
-                        if (cilantroPointCloud->colors.cols() > 0)
-                        {
-                            Eigen::Vector3d color = cilantroPointCloud->colors.col(pointIndice).cast<double>();
-                            segment->Colors.push_back(color);
-                        }
+                        Eigen::Vector3d color = cilantroPointCloud->colors.col(pointIndice).cast<double>();
+                        segment->Colors.push_back(color);
                     }
                 }
+                if (colorClusters)
+                    segment->ApplyColor(Eigen::Vector3d::Random());
                 segments.push_back(segment);
             }
         }
         else
         {
             cilantro::RadiusNeighborhoodSpecification<float> neighborhood(radiusNeighborhoodSize);
 
-            cilantroPointCloud->estimateNormals(neighborhood);
+            // cilantroPointCloud->estimateNormals(neighborhood);
 
             cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
             cilantroPointCloud->normals,
@@ -76,27 +70,15 @@ namespace diffCheck::segmentation
             auto clusterToPointMap = segmenter.getClusterToPointIndicesMap();
             int nSegments = segmenter.getNumberOfClusters();
 
-            // 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>();
                     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);
             }

src/diffCheckApp.cc

@@ -15,38 +15,27 @@ int main()
 
   pcdSrc->LoadFromPLY(pathPcdSrc);
 
+  // pcdSrc->EstimateNormals(false, 50);
+
+  // pcdSrc->ApplyColor(255, 0 , 0);
+
+
   segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(
     pcdSrc,
-    10.f,
+    20.f,
     10,
     true,
     50,
     30,
     true);
   std::cout << "number of segments:" << segments.size()<< std::endl;
-  // print the last 5 colors
-  for (auto segment : segments)
-  {
-    for (int i = 0; i < 5; i++)
-    {
-      std::cout << segment->Colors[i].transpose() << std::endl;
-    }
-  }
 
   diffCheck::visualizer::Visualizer vis;
-  // vis.AddPointCloud(pcdSrc);
+  
   for (auto segment : segments)
-  {
-    // colorize the segments with random colors
-    double r = static_cast<double>(rand()) / RAND_MAX;
-    double g = static_cast<double>(rand()) / RAND_MAX;
-    double b = static_cast<double>(rand()) / RAND_MAX;    
-    for (int i = 0; i < segment->Points.size(); i++)
-    {
-      segment->Colors.push_back(Eigen::Vector3d(r, g, b));
-    }
     vis.AddPointCloud(segment);
-  }
+
+  // vis.AddPointCloud(pcdSrc);
   vis.Run();
   return 0;
 }