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

How to filter PointCloud2 data based on distances?

asked 2019-01-09 19:18:42 -0500

PG_GrantDare gravatar image

updated 2019-01-10 17:40:52 -0500

Hi everyone,

I am using a SICK MRS6124 and experiencing problems where a data point will appear at 0,0. I am trying to filter this point out but don't understand how to use pcl_ros to filter these points based on their distance. I want to do this as the LIDAR has a working range of 0.5m to 200m but clearly 0,0 is not 0.5m away...

If anyone could direct me as to how to filter PointCloud2 data by distance from the sensor then that would be a huge help, thank you in advance to everyone.

edit retag flag offensive close merge delete

Comments

What kind of message are you receiving from the laser? Is your message Pointcloud2 or LaserScan? It is not that common, at least in my opinion, to have a 2D laser messages as Pointcloud2.

dljubic gravatar image dljubic  ( 2019-01-10 02:32:11 -0500 )edit

@dljubic As stated in the question I am looking to filter the PointCloud2 data from my LIDAR.

PG_GrantDare gravatar image PG_GrantDare  ( 2019-01-10 16:14:17 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2019-01-10 04:54:29 -0500

This filtering would be fairly straightforward to implement in your own node, or in a custom pcl-ros::Filter if you want to extend that framework. I've modified the voxel-grid example callback so that it will filter out points within 0.5 units of the origin to get you started.

Hope this helps.

...

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  float depthThreshold = 0.5;
  float threshold2 = depthThreshold*depthThreshold;

  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
  for (int p=0; p<cloud->points.size(); ++p)
  {
    // find the squared distance from the origin.
    float pointDepth2 = (cloud->points[p].x * cloud->points[p].x) +
                       (cloud->points[p].y * cloud->points[p].y) + 
                       (cloud->points[p].z * cloud->points[p].z));

    // remove point if it's within the threshold range
    if (pointDepth2 < threshold2)
    {
      cloud->points[p] = cloud->points[cloud->points.size()-1];
      cloud->points.resize(cloud->points.size()-1);
      --p;
    }
  }

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloudPtr);
  sor.setLeafSize (0.1, 0.1, 0.1);
  sor.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  pcl_conversions::fromPCL(cloud_filtered, output);

  // Publish the data
  pub.publish (output);
}
edit flag offensive delete link more

Comments

Hey @PeteBlackerThe3rd I am having a little trouble with the include tags. are you able to include the include tags to your code block, thank you.Specifically, "'struct pcl::PointCloud2' has no member named 'points'".

PG_GrantDare gravatar image PG_GrantDare  ( 2019-01-10 18:39:24 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2019-01-09 19:18:42 -0500

Seen: 4,480 times

Last updated: Jan 10 '19