Rosanswers logo

Hello all,

my colleague and I are developing a small node that colors a point cloud using a rgb image.

One of the bottleneck we found is using openCV to project each pixel back on the image_plane in order to find which pixel corresponds to which 3Dpoint, using camera models.

After browsing the ROS packages for a while, I stumbled upon the package pcl_point_cloud2_image_color which seems to do the exact same thing. Curious, we opened-up the code to see the trick used.

We were surprised to find the following:

  pcl_point_cloud2_image_color::PointCloud2ImageColor::getColorPointCloud2 (const sensor_msgs::PointCloud2ConstPtr &cloud, 
                                                                            const sensor_msgs::ImageConstPtr &image,
                                                                            sensor_msgs::Image &img_out,
                                                                            pcl::PointCloud<pcl::PointXYZRGB> &cloud_out)
  pcl::PointCloud<PointRGB> cloud_color;
  cloud_color.width    = cloud->width;
  cloud_color.height   = cloud->height;
  cloud_color.is_dense = true;
  cloud_color.points.resize (cloud_color.width * cloud_color.height);

  // Convert to a templated type
  pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
  pcl::fromROSMsg (*cloud, cloud_xyz);

  img_out = *image;
  // Cast image to IplImage
  sensor_msgs::CvBridge img_bridge;
  cv::Mat img = img_bridge.imgMsgToCv (image);

  // Assign colors from the rectified image
  int i = 0;
  for (size_t u = 0; u < image->height; ++u)   // rows
    for (size_t v = 0; v < image->width; ++v, ++i)  // cols
      // If the point is invalid
      if (isnan (cloud_xyz.points[i].x) || isnan (cloud_xyz.points[i].y) || isnan (cloud_xyz.points[i].z))
      // Get the color information
      uint8_t g = img.at<uint8_t>(u, v);
      int32_t rgb = (g << 16) | (g << 8) | g;
      cloud_color.points[i].rgb = *(float*)(&rgb);

  // Concatenate the XYZ information with the RGB one
  pcl::concatenateFields (cloud_xyz, cloud_color, cloud_out);

As one can see, this function does the following processing:

  1. Create a dense PointRGB cloud with the same number of points as in the sensor message.
  2. Convert the sensor message into a PointXYZ cloud.
  3. Iterate over each pixel of the image and assign its color to a point of the PointRGB cloud.
  4. Concatenate the PointRGB and PointXYZ clouds.

What I do not get here is how are we sure pixel[u,v] corresponds to point[i]? How come is there no 3D to 2D projection involved in the pixel/point matching?

I feel I must be missing an important optimization trick here. If anyone can explain, I would be very grateful.

Thanks in advance


Originally posted by raphael favier on ROS Answers with karma: 1382 on 2011-05-09

Post score: 5

Original comments

Comment by chukcha2 on 2016-08-23:
hi Raph, I am need to do solve the same problem (colorize point clouds from images / match 3d points and pixels). Do you have any pointers/suggestions for me? Thank you.


2 Answers 2


Rosanswers logo

The underlying assumption seems to be, that the rgb image and the (non-rgb) pointcloud are using the same pixel raster.

Originally posted by Felix Endres with karma: 6468 on 2011-05-09

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by fabbro on 2022-02-01:
what does it mean exactly: "the same pixel raster"?


Rosanswers logo

To expand on what Felix said:

The assumption is that there is a 1 to 1 mapping in the cloud message that came in. So, any projection would have been done beforehand in arranging the incoming message.

Using that assumption then, they have the same number of points and are in the same order, so the same index can be used.

If you are producing the point cloud in the first place and need to pack the message, you can use the camera parameters and libeigen though I don't know if that would be any faster than the opencv method you were using.

Originally posted by Asomerville with karma: 2743 on 2011-05-10

This answer was NOT ACCEPTED on the original site

Post score: 2


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.