Made it so that addPointCloud<MyCustomPointType>(cloud,label) uses th…

…e RGB field if it is present in the custom point type without the definition of a custom color handler.
PointCloudLibrary · Aug 28, 2015 · 17c80c0 · 17c80c0
1 parent cad6224
commit 17c80c0
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 7 deletions.
diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp
@@ -88,7 +88,11 @@ pcl::visualization::PCLVisualizer::addPointCloud (
     return (false);
   }
 
-  //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
+  if (pcl::traits::has_color<PointT>())
+  {
+    PointCloudColorHandlerRGBField<PointT> color_handler_rgb_field (cloud);
+    return (fromHandlersToScreen (geometry_handler, color_handler_rgb_field, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
+  }
   PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
 }

diff --git a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp
@@ -135,6 +135,13 @@ pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPo
 {
   if (!capable_ || !cloud_)
     return (false);
+
+   // Get the RGB field index
+  std::vector<pcl::PCLPointField> fields;
+  int rgba_index = -1;
+  rgba_index = pcl::getFieldIndex (*cloud_, "rgb", fields);
+  if (rgba_index == -1)
+    rgba_index = pcl::getFieldIndex (*cloud_, "rgba", fields);
 
   if (!scalars)
     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
@@ -151,6 +158,7 @@ pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPo
     if (fields_[d].name == "x")
       x_idx = static_cast<int> (d);
 
+  pcl::RGB rgb;
   if (x_idx != -1)
   {
     // Color every point
@@ -162,9 +170,10 @@ pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPo
           !pcl_isfinite (cloud_->points[cp].z))
         continue;
 
-      colors[j    ] = cloud_->points[cp].r;
-      colors[j + 1] = cloud_->points[cp].g;
-      colors[j + 2] = cloud_->points[cp].b;
+      memcpy (&rgb, (reinterpret_cast<const char *> (&cloud_->points[cp])) + rgba_index, sizeof (pcl::RGB));
+      colors[j    ] = rgb.r;
+      colors[j + 1] = rgb.g;
+      colors[j + 2] = rgb.b;
       j += 3;
     }
   }
@@ -174,9 +183,10 @@ pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPo
     for (vtkIdType cp = 0; cp < nr_points; ++cp)
     {
       int idx = static_cast<int> (cp) * 3;
-      colors[idx    ] = cloud_->points[cp].r;
-      colors[idx + 1] = cloud_->points[cp].g;
-      colors[idx + 2] = cloud_->points[cp].b;
+      memcpy (&rgb, (reinterpret_cast<const char *> (&cloud_->points[cp])) + rgba_index, sizeof (pcl::RGB));
+      colors[j    ] = rgb.r;
+      colors[j + 1] = rgb.g;
+      colors[j + 2] = rgb.b;
     }
   }
   return (true);