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 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 iter_x(*pointcloud_in, "x");
sensor_msgs::PointCloud2ConstIterator 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 iter_xx(*pointcloud_in, "x");
sensor_msgs::PointCloud2ConstIterator 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!
↧