Merge pull request #2105 from hrnr/pcd_viewer_color_handling

Fix pcd_viewer color handling when invalid fields are present in pcd
PointCloudLibrary · Nov 30, 2017 · eaa403d · eaa403d
2 parents 47629c1 + 6fb8dc6
commit eaa403d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp
@@ -2980,10 +2980,10 @@ pcl::visualization::PCLVisualizer::updateColorHandlerIndex (const std::string &i
     return (false);
   }
 
-  int color_handler_size = int (am_it->second.color_handlers.size ());
-  if (index >= color_handler_size)
+  size_t color_handler_size = am_it->second.color_handlers.size ();
+  if (!(size_t (index) < color_handler_size))
   {
-    pcl::console::print_warn (stderr, "[updateColorHandlerIndex] Invalid index <%d> given! Maximum range is: 0-%lu.\n", index, static_cast<unsigned long> (am_it->second.color_handlers.size ()));
+    pcl::console::print_warn (stderr, "[updateColorHandlerIndex] Invalid index <%d> given! Index must be less than %d.\n", index, int (color_handler_size));
     return (false);
   }
   // Get the handler

diff --git a/visualization/tools/pcd_viewer.cpp b/visualization/tools/pcd_viewer.cpp
@@ -617,22 +617,26 @@ main (int argc, char** argv)
     {
       int rgb_idx = 0;
       int label_idx = 0;
+      int invalid_fields_count = 0;
       for (size_t f = 0; f < cloud->fields.size (); ++f)
       {
+        if (!isValidFieldName (cloud->fields[f].name))
+        {
+          ++invalid_fields_count;
+          continue;
+        }
         if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
         {
-          rgb_idx = f + 1;
+          rgb_idx = f - invalid_fields_count + 1 /* first is ColorHandlerRandom */;
           color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
         }
         else if (cloud->fields[f].name == "label")
         {
-          label_idx = f + 1;
+          label_idx = f - invalid_fields_count + 1;
           color_handler.reset (new pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2> (cloud, !use_optimal_l_colors));
         }
         else
         {
-          if (!isValidFieldName (cloud->fields[f].name))
-            continue;
           color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
         }
         // Add the cloud to the renderer