0
$\begingroup$

Rosanswers logo

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_msgs/html/namespacesensor__msgs.html#details

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


Originally posted by schizzz8 on ROS Answers with karma: 183 on 2017-05-15

Post score: 0


Original comments

Comment by Overseer on 2019-04-15:
mind me asking what does the depth_scale variable refer to?

Comment by schizzz8 on 2019-05-02:
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

Comment by Brolin on 2020-09-29:
Hi, can someone tell me what is invK. I am trying to understand the calculation and how the code works.

Comment by schizzz8 on 2020-10-05:
@Brolin invK is the inverse of the camera matrix K: http://ksimek.github.io/2013/08/13/intrinsic/

Comment by Brolin on 2020-10-05:
@schizzz8 so invK is the inverse of the camera matrix right !!

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by thesidjway with karma: 26 on 2018-03-01

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$
0
$\begingroup$

Rosanswers logo

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


Originally posted by gvdhoorn with karma: 86574 on 2017-05-15

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by huangzonghao on 2018-07-25:
are there any tutorials on how to use depth_image_proc? It's really hard to find any useful documentation.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.