ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

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);
        count++;
    }
    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
            //pointcloud_out.data  =0;
        }
    }
    pub_pc2_.publish(pointcloud_out);
    }

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

Comments

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
0

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 (&pointcloud_out.data[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 (&pointcloud_out.data[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 (&pointcloud_out.data[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));
            counter2++;
        }
        counter1++;
    }
edit flag offensive delete link more

Comments

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

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: 2018-04-27 02:48:01 -0600

Seen: 940 times

Last updated: Apr 27 '18