Robotics StackExchange | Archived questions

How to filter PointCloud2 data based on distances?

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.

Asked by PG_GrantDare on 2019-01-09 20:18:42 UTC

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.

Asked by dljubic on 2019-01-10 03:32:11 UTC

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

Asked by PG_GrantDare on 2019-01-10 17:14:17 UTC

Answers

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);
}

Asked by PeteBlackerThe3rd on 2019-01-10 05:54:29 UTC

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'".

Asked by PG_GrantDare on 2019-01-10 19:39:24 UTC