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

Convert depth image to sensor_msgs::PointCloud2

asked 2017-05-15 05:29:18 -0600

schizzz8 gravatar image

Hi all,

I have an Asus Xtion Pro RGB-D camera, I'm using this node to acquire images:

which has tremendous performance improvements w.r.t. OpenNI2.

To convert the depth image into a point cloud I'm following the draft tutorial in:

So far, that's what I wrote:

void frameCallback(const sensor_msgs::ImageConstPtr& msg) {

    if (!gotInfo) return;

    vector<float> points;

    cv_bridge::CvImageConstPtr image =  cv_bridge::toCvShare(msg);
    for(int i =0;i<image->image.rows;i++){
        const ushort* row_ptr = image->image.ptr<ushort>(i);
        for(int j=0;j<image->image.cols;j++){
            ushort id=row_ptr[j];
                float d=depth_scale*id;
                Eigen::Vector3f image_point(j*d,i*d,d);
                Eigen::Vector3f camera_point=invK*image_point;

    int n_points = points.size();

    // Create a PointCloud2
    sensor_msgs::PointCloud2 cloud_msg;
    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::PointField::FLOAT32,
                                  "y", 1, sensor_msgs::PointField::FLOAT32,
                                  "z", 1, sensor_msgs::PointField::FLOAT32);
    modifier.setPointCloud2FieldsByString(1, "xyz");

    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");

    cloud_msg.height = 1;
    cloud_msg.width = n_points;
    cloud_msg.header.frame_id = msg->header.frame_id;
    cloud_msg.header.seq = msg->header.seq;
    cloud_msg.header.stamp = msg->header.stamp;

    for(size_t i=0; i<n_points; ++i, ++iter_x, ++iter_y, ++iter_z){
        *iter_x = points[3*i+0];
        *iter_y = points[3*i+1];
        *iter_z = points[3*i+2];

        cerr << *iter_x << " " << *iter_y << " " << *iter_z << endl;

    std::cerr << ".";

but as expected in RVIZ I see three point clouds, and if I print the points created I see that they are triplicated. I'm quite sure that the bug is in the use of the sensor_msgs::PointCloud2Iterator, but since the tutorial I mentioned doesn't give better clues on how to use them I'm stuck.

Does anyone know the correct way to populate the sensor_msgs::PointCloud2?


edit retag flag offensive close merge delete


mind me asking what does the depth_scale variable refer to?

Overseer gravatar image Overseer  ( 2019-04-15 19:42:43 -0600 )edit

It's a conversion factor, it's needed when depth images are stored as CV_16U opencv matrices. It should be depth_value=1e-3

schizzz8 gravatar image schizzz8  ( 2019-05-02 07:43:47 -0600 )edit

Hi, can someone tell me what is invK. I am trying to understand the calculation and how the code works.

Brolin gravatar image Brolin  ( 2020-09-29 06:16:24 -0600 )edit

@Brolin invK is the inverse of the camera matrix K:

schizzz8 gravatar image schizzz8  ( 2020-10-05 08:16:08 -0600 )edit

@schizzz8 so invK is the inverse of the camera matrix right !!

Brolin gravatar image Brolin  ( 2020-10-05 08:23:38 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2018-03-01 17:32:34 -0600

thesidjway gravatar image

I know this is really late and I'm sure you would have figured this out, but maybe this will benefit someone else in the future.

The error in your code is where you iterate over the points vector. Now since n_points = points.size(), you should only iterate until i < n_points/3 not until i < n_points since you are later multiplying i with 3 during the access.

edit flag offensive delete link more

answered 2017-05-15 06:06:27 -0600

gvdhoorn gravatar image

Not an answer, but have you considered using depth_image_proc/point_cloud_xyz?

edit flag offensive delete link more


are there any tutorials on how to use depth_image_proc? It's really hard to find any useful documentation.

huangzonghao gravatar image huangzonghao  ( 2018-07-25 18:45:09 -0600 )edit

Question Tools


Asked: 2017-05-15 05:29:18 -0600

Seen: 8,224 times

Last updated: May 15 '17