Skip to content

Instantly share code, notes, and snippets.

@safijari
Last active December 21, 2023 10:10
Show Gist options
  • Save safijari/0b3f839926c66238530c5a86438fb232 to your computer and use it in GitHub Desktop.
Save safijari/0b3f839926c66238530c5a86438fb232 to your computer and use it in GitHub Desktop.
The code I used to further denoise the R200 point cloud derived from the filtered cloud(see here https://gist.github.com/safijari/fca7fa75649dbaba4b0be54f5dd742ec). Note that this will need at least PCL 1.7.2 (for Ubuntu, it's in the official PPA)
/*
Purpose of node: subscribe to a point cloud, use a VoxelGrid filter on it with a setting that
clobbers voxels with fewer than a threshold of points.
*/
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <pcl/filters/voxel_grid.h>
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef pcl::PointXYZ PointXYZ;
class FilterAndPublish
{
public:
FilterAndPublish()
{
printf("Made object\n");
pub = nh.advertise<PointCloud> ("/points_filtered", 1);
sub = nh.subscribe<PointCloud>("/points", 1, &FilterAndPublish::callback, this);
this->thresh = 15; // This is the minimum number of points that have to occupy a voxel in order for it to survive the downsample.
}
void callback(const PointCloud::ConstPtr& msg)
{
PointCloud::Ptr cloud (new PointCloud);
PointCloud::Ptr cloud_filtered (new PointCloud);
*cloud = *msg;
// What to do here:
// 1. Take cloud and put it in a voxel grid while restricting the bounding box
// 2. Go through the voxels and remove all points in a voxel that has less than this.thresh points
// 3. Publish resulting cloud
pcl::VoxelGrid<PointXYZ> vox;
vox.setInputCloud(cloud);
// The leaf size is the size of voxels pretty much. Note that this value affects what a good threshold value would be.
vox.setLeafSize(0.05f, 0.05f, 0.05f);
// I limit the overall volume being considered so lots of "far away" data that is just terrible doesn't even have to be considered.
vox.setFilterLimits(-1.0, 1.0);
// The line below is perhaps the most important as it reduces ghost points.
vox.setMinimumPointsNumberPerVoxel(this->thresh);
vox.filter(*cloud_filtered);
pub.publish (cloud_filtered);
}
private:
ros::NodeHandle nh;
ros::Publisher pub;
ros::Subscriber sub;
int thresh;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "pcl_filtering");
FilterAndPublish f = FilterAndPublish();
ros::spin();
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment