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

Cutting off a pointcloud2 at a certain range

asked 2018-04-27 02:48:01 -0600

Peter1 gravatar image

updated 2018-04-27 02:51:48 -0600

I am working on a mapping problem with a laser scanner. I want to cut off the 20% of points that are furthest away from the robot. So this distance should be variable for each time steps. Depending on how close we are to walls in certain areas. I came up with the following approach:

void PcFilter::pcCallback(const sensor_msgs::PointCloud2ConstPtr &pointcloud_in)
    unsigned int num_points = pointcloud_in->width;
    std::vector<float> ranges(num_points);
    sensor_msgs::PointCloud2 pointcloud_out;
    int cut_off_idx = int(num_points*0.8);
    pointcloud_out.header = pointcloud_in->header;
    pointcloud_out.fields = pointcloud_in->fields;
    pointcloud_out.height = pointcloud_in->height;
    pointcloud_out.width = cut_off_idx;//+1
    pointcloud_out.point_step = pointcloud_in->point_step;
    pointcloud_out.row_step = pointcloud_out.width*pointcloud_out.point_step;
    pointcloud_out.is_bigendian = pointcloud_in->is_bigendian;
    pointcloud_out.is_dense = pointcloud_in->is_dense;
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*pointcloud_in, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*pointcloud_in, "y");
    int count = 0;
    //iterate over pointcloud an save the distance from the points in the xy plane
    for(;iter_x != iter_x.end();
        ++iter_x, ++iter_y)
        ranges[count] = hypot(*iter_x, *iter_y);
    std::sort(std::begin(ranges), std::end(ranges));
    float cut_off_dist = ranges[cut_off_idx];
    float point_dist;
    sensor_msgs::PointCloud2ConstIterator<float> iter_xx(*pointcloud_in, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_yy(*pointcloud_in, "y");
    //iterate over pointcloud again and fill data in new pointcloud
    for(;iter_xx != iter_xx.end(); ++iter_xx, ++iter_yy)
        point_dist = hypot(*iter_x, *iter_y);
        if (point_dist <= cut_off_dist)

            //todo fill in data
            //  =0;

What I cant seem to accomplish is actually filling the data field with the corresponding points. I saw sth about memcpy, but I could find an extensive tutorial. Appreciate your help!

edit retag flag offensive close merge delete


If I understand you correctly, I would actually recommend to use the CropBox nodelet filter in pcl_ros. That does exactly what you're after if I'm not mistaken.

gvdhoorn gravatar image gvdhoorn  ( 2018-04-27 07:14:29 -0600 )edit

Is that going to work with the ros pointcloud2 format. I have never worked with the native pcl. Any advise on the use?

Peter1 gravatar image Peter1  ( 2018-04-27 08:46:36 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-04-27 05:01:29 -0600

Peter1 gravatar image

updated 2018-04-27 05:02:10 -0600

Got it to work with the following loop

for(;iter_xx != iter_xx.end(); ++iter_xx, ++iter_yy)
        point_dist = hypot(*iter_xx, *iter_yy);
        if (point_dist <= cut_off_dist)
            memcpy (&[counter2 * pointcloud_out.point_step + pointcloud_out.fields[0].offset],
                    &pointcloud_in->data[counter1 * pointcloud_in->point_step + pointcloud_in->fields[0].offset], sizeof (float));
            memcpy (&[counter2 * pointcloud_out.point_step + pointcloud_out.fields[1].offset],
                    &pointcloud_in->data[counter1 * pointcloud_in->point_step + pointcloud_in->fields[1].offset], sizeof (float));
            memcpy (&[counter2 * pointcloud_out.point_step + pointcloud_out.fields[2].offset],
                    &pointcloud_in->data[counter1 * pointcloud_in->point_step + pointcloud_in->fields[2].offset], sizeof (float));
edit flag offensive delete link more


Glad you've got this working. I was going to comment that this will be a very computationally expensive processes especially for large point clouds. Have you considered filtering either the farthest 20% of the depth range, or a threshold based on standard deviation? They could be much faster.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-04-27 05:08:05 -0600 )edit

It works so far, but of course its not a cheap calculation. I dont have any depth information. Which standard deviation do you mean?

Peter1 gravatar image Peter1  ( 2018-04-27 08:47:53 -0600 )edit

Question Tools


Asked: 2018-04-27 02:48:01 -0600

Seen: 1,086 times

Last updated: Apr 27 '18