Publishing colored point clouds

[DJBongoJohn]

Hello,

I have a hard time publishing colored point clouds.
As the PointCloud2 message does not contain color information, I have chosen to publish the depth and rgb info as two separate streams and combining them in an external ROS node, as suggested here.

I tried to publish the depth as a PointCloud2 message (shown here), but i got the error:

read__sensor_msgs__PointCloud2: field data: expected uint8 array (simROS.publish @ 'RosInterface' plugin)

using the new ros Interface. Also, filling the message was quite slow (even when I'm handling the sensor explicitly). Therefore, I've chosen to publish the depth data as a std_msgs::Float32MultiArray. RGB is published as a sensor_msgs::Image.

The problem I have is with registering the color to the points, as seen on the photo (1st being the point cloud in rviz; 2nd being the scene in V-Rep):



I use a single sensor with the following setup:




I handle the sensor in a non-threaded child script:

Code: Select all

if (sim_call_type==sim.syscb_init) then
    sensorHandle=sim.getObjectHandle("sensor")
    depth_pub = simROS.advertise('/vrep/depth', 'std_msgs/Float32MultiArray')
    rgb_pub = simROS.advertise('/vrep/rgb', 'sensor_msgs/Image')
    simROS.publisherTreatUInt8ArrayAsString(rgb_pub)
end

if (sim_call_type==sim.syscb_actuation) then
    sim.handleVisionSensor(sensorHandle)
    r,t1,u1=sim.readVisionSensor(sensorHandle)
    tb_depth={}
    tb_depth['data']=u1
    simROS.publish(depth_pub, tb_depth)

    t=sim.getSystemTime()
    image=sim.getVisionSensorImage(sensorHandle,0,0,0,0,1)
    tb_rgb={}
    tb_rgb['header']={stamp=t, frame_id="camera_depth_frame"}
    tb_rgb['height']=480
    tb_rgb['width']=640
    tb_rgb['encoding']='rgb8'
    tb_rgb['step']=640 * 3
    tb_rgb['data']=image
    simROS.publish(rgb_pub, tb_rgb)
end

I fuse the depth and rgb information in a separate ROS node with the function:

Code: Select all

using Point = pcl::PointXYZRGB;
using Cloud = pcl::PointCloud<Point>;
using MultiArray = std_msgs::Float32MultiArray;

Cloud::Ptr img2cloud(cv::Mat rgb, MultiArray depth_msg)
{
    std::vector<float> depth = depth_msg.data;
    if (rgb.cols != depth[0] or rgb.rows != depth[1]) {
        std::cout << "rgb and depth image size mismatch" << '\n';
        exit(0);
    }
    Cloud::Ptr cloud(new Cloud);
    for (size_t u = 0; u < rgb.rows; u++) {
        for (size_t v = 0; v < rgb.cols; v++) {
            auto rgb_ints = rgb.at<cv::Vec3b>(u, v);
            int w = 2 + 4 * (rgb.cols * u + v);
            Point p;
            p.x = depth[w], p.y = depth[w + 1], p.z = depth[w + 2];
            p.r = (int) rgb_ints[0]; p.g = (int) rgb_ints[1]; p.b = (int) rgb_ints[2];
            cloud->points.push_back(p);
        }
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    return cloud;
}

The RGB and depth information have the same dimensions, but it looks like the color is somehow shifted to the center. I.e. depth[u,v] does not correspond with RGB[u,v]. This might indicate that the RGB image is shot at a wider angle, but this should not be the case since it is the same sensor?

All I need is a correctly registered point cloud, and I don't really care how I accomplish it, so if you can't find an error I've made, I would like to be pointed in a different direction.

Thank you

[DJBongoJohn]

So, by using 'intensity scale work image' instead of 'extract coordinates from work image' I get a perfect overlay between the depth and rgb channels




I guess extract coordinates doesn't just calculate a x, y and d value for each z in the depth buffer?
Might be an issue in the scaling calculation in the resolution of the filter.

[DJBongoJohn]

I got it working using the 'getVisionSensorDepthBuffer' function.
I still publish the depth as a Float32MultiArray and the RGB as an Image, and combine it to a PointCloud2 in an external ROS node.

The code for publishing colored point clouds with the Kinect:

Code: Select all

if (sim_call_type==sim.syscb_init) then
    depth_pub = simROS.advertise('/vrep/depth', 'std_msgs/Float32MultiArray')
    rgb_pub = simROS.advertise('/vrep/rgb', 'sensor_msgs/Image')
    simROS.publisherTreatUInt8ArrayAsString(rgb_pub)
    depthCam=sim.getObjectHandle('depth_sensor')
    colorCam=sim.getObjectHandle('rgb_sensor')
end

if (sim_call_type==sim.syscb_actuation) then
    tb_depth={}
    tb_depth['data']=sim.getVisionSensorDepthBuffer(depthCam)
    simROS.publish(depth_pub, tb_depth)

    t=simROS.getTime()
    image=sim.getVisionSensorImage(colorCam,0,0,0,0,1)
    tb_rgb={}
    tb_rgb['header']={stamp=t, frame_id="camera_depth_frame"}
    tb_rgb['height']=480
    tb_rgb['width']=640
    tb_rgb['encoding']='rgb8'
    tb_rgb['step']=640 * 3
    tb_rgb['data']=image
    simROS.publish(rgb_pub, tb_rgb)
end

Code: Select all

using Point = pcl::PointXYZRGB;
using Cloud = pcl::PointCloud<Point>;
using MultiArray = std_msgs::Float32MultiArray;

Cloud::Ptr img2cloud(cv::Mat rgb, MultiArray depth_msg)
{
    std::vector<float> depth = depth_msg.data;
    if (rgb.cols != v_res or rgb.rows != u_res or depth.size() != u_res * v_res) {
        std::cout << "rgb and depth image size mismatch" << '\n';
        exit(0);
    }
    Cloud::Ptr cloud(new Cloud);
    for (size_t u = 0; u < u_res; u++) {
        for (size_t v = 0; v < v_res; v++) {
            auto rgb_ints = rgb.at<cv::Vec3b>(u, v);
            int w = v_res * u + v;
            float z = far_clip * depth[w] + near_clip;
            float y = (u - u_res / 2) * z / focal_length;
            float x = (v - v_res / 2) * z / focal_length;
            Point p;
            p.x = x, p.y = y, p.z = z;
            p.r = (int) rgb_ints[0]; p.g = (int) rgb_ints[1]; p.b = (int) rgb_ints[2];
            cloud->points.push_back(p);
        }
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    return cloud;
}

Here u_res and v_res is the resolution of the depth and rgb sensors (has to match).
near_clip and far_clip is the near and far clipping planes (used to scale z to meters, as getVisionSensorDepthBuffer returns values between 0-1)
focal_length can be calculated as f = max(u_res, v_res) / tan(FoV/2), where FoV is the perspective angle (from the vision sensor properties)

[fabianvargas]

Hello, thank you very much for this post, please confirm if the image is generated by a depth sensor such as kinect or realsense camera, is it possible to visualize colored point cloud in real time? I am new to this topic, I appreciate if you can share simple examples or tutorials.
Thank you