ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to convert sensor_msgs/Image to PointCloud<pcl::RGB>??

asked 2016-06-20 11:24:41 -0500

Anshul Paigwar gravatar image

I am trying to use PCL implementation of generating pointcloud and disparity from stereo images. I am using pcl::StereoMatching Class and stereo.compute() function to get disparity image. But compute() function requires left and right camera images in PointCloud<pcl::rgb> format. While I am using ros and my images are in the form of sensor_msgs/Image. Is there any way to do this conversion??

PCL stereo implementation guide: here

I am able to convert sensor_msgs/Image to PCLImage format using function pcl_conversions::toPCL(), but apparently PCLImage format is not same as PointCloud<pcl::rgb> format. please someone help me.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-06-21 10:48:05 -0500

Fabiobreo gravatar image

You can use OpenCV to do this task. You can get your disparity map using StereoSGBM, then your depth map using reprojectImageTo3D. Once you have your depth map, you can reconstruct your 3D scene using something like this:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

//Reconstruct PointCloud with the depthmap points
for (int i = 0; i < depth.rows; ++i)
{
    for (int j = 0; j < depth.cols; ++j)
    {
        pcl::PointXYZRGB p;

        //The coordinate of the point is taken from the depth map
        //Y and Z  taken negative to immediately visualize the cloud in the right way
        p.x = depth.at<Vec3f>(i,j)[0];
        p.y = -(depth.at<Vec3f>(i,j)[1]);
        p.z = -(depth.at<Vec3f>(i,j)[2]);

        //Coloring the point with the corrispondent point in the rectified image
        p.r = static_cast<uint8_t>(color.at<Vec3b>(i,j)[2]);
        p.g = static_cast<uint8_t>(color.at<Vec3b>(i,j)[1]);
        p.b = static_cast<uint8_t>(color.at<Vec3b>(i,j)[0]);

        //Insert point in the cloud, cutting the points that are too distant
        if(( abs( p.x ) < 500 )&&( abs( p.y ) < 200 )&&( abs( p.z ) < 500 ))
            cloud->points.push_back(p);
    }
}
cloud->width = (int) cloud->points.size();
cloud->height = 1;
edit flag offensive delete link more

Comments

1

Hi there, Is there any faster method other than looping through all the pixels? I need to do something similar in real time. Any suggestions? Thanks in Advance

Sooryakiran gravatar image Sooryakiran  ( 2018-05-07 07:21:45 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2016-06-20 11:24:41 -0500

Seen: 2,936 times

Last updated: Jun 20 '16