I am trying to filter out a specific area using pcl::CropBox
and only show the points within an established box, but something is not totally correct. The point clouds are coming from a lidar
sensor. The lidar is in a fixed position as shown below and all points in the box that I would like to keep are in the yellow box (coordinates are also shown). All points outside of the box should not be visible or erased. The sensor is in a (0,0,0)
position and represents the origin for XYZ
in red below.
Below and in the box the area I am trying to keep:
Below the code I am using to do the filtering and the steps are:
1) Acquiring point clouds - it works
2) Filtering and downsampling: I filter only the point on 180 degree facing upward and filtering in all XYZ
direction establishing limits - it works
3) Establishing a fixed box using pcl::CropBox
and keep only the points inside the box above - it does not work as I still see all the points outside the box
processlidar.h
class SubscribeProcessPublish
{
public:
SubscribeProcessPublish();
pcl::PointCloud<pcl::PointXYZ> filterPointCloudData(const pcl::PCLPointCloud2ConstPtr& cloud);
void processLidarMeasurementCallBack(const pcl::PCLPointCloud2ConstPtr& cloud);
void removePointsOutsideBox();
}
processlidar.cpp
void SubscribeProcessPublish::removePointsOutsideBox()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_toRemove (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_removed (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloud_xyz_toRemove, *cloud_xyz_removed);
pcl::CropBox<pcl::PointXYZ> boxFilter;
// Establishing the size of the CropBox
float x_min = 0.0, y_min = -25.0, z_min = -1.5;
float x_max = 50.0, y_max = +25.0, z_max = +1.5;
boxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));
boxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
boxFilter.setInputCloud(cloud_xyz_toRemove);
boxFilter.filter(*cloud_xyz_removed);
std::cout<<"Box established"<<std::endl;
std::cout<<"Constantly Keeping only points inside the box"<<std::endl;
}
pcl::PointCloud<pcl::PointXYZ> SubscribeProcessPublish::filterPointCloudData(const pcl::PCLPointCloud2ConstPtr& cloud)
{
// Before establishing the XYZ limits we already have the box defined above
removePointsOutsideBox()
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloud, *cloud_xyz_filtered);
// define a PassThrough
pcl::PassThrough<pcl::PointXYZ> pass;
// set input to cloudVoxel
pass.setInputCloud(cloud_xyz_filtered);
// filter along z-axis
pass.setFilterFieldName("z");
pass.setFilterLimits(z_min, z_max);
pass.filter(*cloud_xyz_filtered);
// filter along y-axis
pass.setFilterFieldName("y");
pass.setFilterLimits(y_min, y_max);
pass.filter(*cloud_xyz_filtered);
// filter along x-axis
pass.setFilterFieldName("x");
pass.setFilterLimits(x_min, x_max);
pass.filter(*cloud_xyz_filtered);
// cascade the floor removal filter and define a container for floorRemoved
pcl::PCLPointCloud2::Ptr filtered_cloud (new pcl::PCLPointCloud2 ());
// copy the contents of the cloud_xyz_filtered to filtered_cloud
pcl::toPCLPointCloud2(*cloud_xyz_filtered, *filtered_cloud);
// define a voxelgrid
pcl::VoxelGrid<pcl::PCLPointCloud2> voxelGrid;
// set input to cloud
voxelGrid.setInputCloud(filtered_cloud);
// set the leaf size (x, y, z)
voxelGrid.setLeafSize(0.1, 0.1, 0.1);
// apply the filter to dereferenced cloudVoxel
voxelGrid.filter(*filtered_cloud);
pcl::PointCloud<pcl::PointXYZ> pclXYZ;
pcl::fromPCLPointCloud2(*filtered_cloud, pclXYZ);
this->publisher.publish (*filtered_cloud);
return pclXYZ;
}
What I have done so far:
Before arriving to the code I posted above I had to do a lot of research on how to use the filtering functions of pcl
. At first I did the tutorial related to the filtering part and creating the voxels and extablishing the XYZ
limits I wanted. Very useful posts were this one, also this one.
I went through the official documentation of pcl
and consulted CropBox API but also went ahead and consulted this.
The following source was also useful but could still not figure out why it is not working.
However there is something missing that is preventing me from seeing only the points in the box. Please point to the right direction for solving this problem.
Aucun commentaire:
Enregistrer un commentaire