vendredi 29 mai 2020

pcl::CropBox does not erase points outside of the box

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:

cropbox

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