diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp index 22c0aea2d5d..0108f5c9b71 100644 --- 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 color_handler (cloud); + if (pcl::traits::has_color()) + { + PointCloudColorHandlerRGBField color_handler_rgb_field (cloud); + return (fromHandlersToScreen (geometry_handler, color_handler_rgb_field, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); + } PointCloudColorHandlerCustom 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 index 62725d55164..81212ab9db5 100644 --- 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::getColor (vtkSmartPo { if (!capable_ || !cloud_) return (false); + + // Get the RGB field index + std::vector 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::New (); @@ -151,6 +158,7 @@ pcl::visualization::PointCloudColorHandlerRGBField::getColor (vtkSmartPo if (fields_[d].name == "x") x_idx = static_cast (d); + pcl::RGB rgb; if (x_idx != -1) { // Color every point @@ -162,9 +170,10 @@ pcl::visualization::PointCloudColorHandlerRGBField::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 (&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::getColor (vtkSmartPo for (vtkIdType cp = 0; cp < nr_points; ++cp) { int idx = static_cast (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 (&cloud_->points[cp])) + rgba_index, sizeof (pcl::RGB)); + colors[j ] = rgb.r; + colors[j + 1] = rgb.g; + colors[j + 2] = rgb.b; } } return (true);