Changed the Glasbey Color Lookup, such that label ids get consisten c…

…olor.

This is especially important, if labels have a semantic meaning (class) (say 1=chair, 2=door, 3=floor, 4=wall ).
If a chair is missing in a pointcloud (label 1 is absent) than the colors of all objects would change.
Additionally, it is much faster, since we do not iterate through clouds multiple times as well as saving time on various map lookups.

visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp

@@ -522,24 +522,12 @@ pcl::visualization::PointCloudColorHandlerLabelField<PointT>::getColor (vtkSmart
   reinterpret_cast<vtkUnsignedCharArray*> (&(*scalars))->SetNumberOfTuples (nr_points);
   unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*> (&(*scalars))->GetPointer (0);
 
-  std::set<uint32_t> labels;
-  std::map<uint32_t, pcl::RGB> colormap;
-
-  // First pass: find unique labels
-  for (vtkIdType i = 0; i < nr_points; ++i)
-    labels.insert (cloud_->points[i].label);
-
-  // Assign Glasbey colors in ascending order of labels
-  size_t color = 0;
-  for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
-    colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
-
   int j = 0;
   for (vtkIdType cp = 0; cp < nr_points; ++cp)
   {
     if (pcl::isFinite (cloud_->points[cp]))
     {
-      const pcl::RGB& color = colormap[cloud_->points[cp].label];
+      const pcl::RGB& color = GlasbeyLUT::at (cloud_->points[cp].label % GlasbeyLUT::size ());
       colors[j    ] = color.r;
       colors[j + 1] = color.g;
       colors[j + 2] = color.b;

visualization/src/point_cloud_handlers.cpp

@@ -615,22 +615,6 @@ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::getCo
   int point_offset = cloud_->fields[field_idx_].offset;
   const int field_size = pcl::getFieldSize (cloud_->fields[field_idx_].datatype);
 
-  std::set<uint32_t> labels;
-  std::map<uint32_t, pcl::RGB> colormap;
-
-  // First pass: find unique labels
-  for (vtkIdType i = 0; i < nr_points; ++i, point_offset += cloud_->point_step)
-  {
-    uint32_t label;
-    memcpy (&label, &cloud_->data[point_offset], field_size);
-    labels.insert (label);
-  }
-
-  // Assign Glasbey colors in ascending order of labels
-  size_t color = 0;
-  for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
-    colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
-
   // If XYZ present, check if the points are invalid
   int x_idx = pcl::getFieldIndex (*cloud_, "x");
   point_offset = cloud_->fields[field_idx_].offset;
@@ -654,7 +638,7 @@ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::getCo
       if (!pcl_isfinite (x_data) || !pcl_isfinite (y_data) || !pcl_isfinite (z_data))
         continue;
 
-      const pcl::RGB& color = colormap[label];
+      const pcl::RGB& color = GlasbeyLUT::at (label % GlasbeyLUT::size ());
       colors[j    ] = color.r;
       colors[j + 1] = color.g;
       colors[j + 2] = color.b;
@@ -670,7 +654,7 @@ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::getCo
       uint32_t label;
       memcpy (&label, &cloud_->data[point_offset], field_size);
 
-      const pcl::RGB& color = colormap[label];
+      const pcl::RGB& color = GlasbeyLUT::at (label % GlasbeyLUT::size ());
       colors[j    ] = color.r;
       colors[j + 1] = color.g;
       colors[j + 2] = color.b;