Ask Your Question
0

Convert depth image to sensor_msgs::PointCloud2

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

schizzz8 gravatar image

Hi all,

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

https://github.com/xqms/xtion

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:

http://docs.ros.org/jade/api/sensor_m...

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];
            if(id!=0){
                float d=depth_scale*id;
                Eigen::Vector3f image_point(j*d,i*d,d);
                Eigen::Vector3f camera_point=invK*image_point;
                points.push_back(camera_point.x());
                points.push_back(camera_point.y());
                points.push_back(camera_point.z());
            }
        }
    }

    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");
    modifier.resize(n_points);

    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;
    }

    cloud_pub.publish(cloud_msg);
    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?

Thanks

edit retag flag offensive close merge delete

Comments

mind me asking what does the depth_scale variable refer to?

Overseer gravatar imageOverseer ( 2019-04-15 19:42:43 -0500 )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 imageschizzz8 ( 2019-05-02 07:43:47 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

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

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
0

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

gvdhoorn gravatar image

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

edit flag offensive delete link more

Comments

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

huangzonghao gravatar imagehuangzonghao ( 2018-07-25 18:45:09 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

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

Seen: 3,421 times

Last updated: May 15 '17