Coloring point clouds from images - how to match 3Dpoint/pixel efficiently?
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:
void
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))
continue;
// 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:
- Create a dense PointRGB cloud with the same number of points as in the sensor message.
- Convert the sensor message into a PointXYZ cloud.
- Iterate over each pixel of the image and assign its color to a point of the PointRGB cloud.
- 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
Raph
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.