激光SLAM前端数据处理总结（过滤坏点）

---

一、距离处理

去除掉非常近的点。

/* 
函数功能 ：移除 过 近  的 点   可输入 cloud_in = cloud_out 这样就是处理一个点云，自身发生处理

参数1  输入点云
参数2  输出点云
参数3  阈值：距离  

已通过测试

time 2020.6.28

jone
*/
template <typename PointT>  //模板点云类型   可以适应各类pcl的点云类型
void removeClosedPointCloud( const pcl::PointCloud<PointT> &cloud_in,//参数1  输入点云
                                pcl::PointCloud<PointT> &cloud_out,     //参数2  输出点云
                                float thres                             //参数3  阈值：距离  
                            )           
{
    /*如何参数 点云输入 和 输出 不是一个 则需要 先赋值 输出点云的 header（不变），重新定义大小*/
    if ( &cloud_in != &cloud_out )
        {
            cloud_out.header = cloud_in.header;
            cloud_out.points.resize( cloud_in.points.size() );
        }

    size_t j = 0;//输出点云的大小 

    for ( size_t i = 0; i < cloud_in.points.size(); ++i )//遍历输入点云
    {
        //判断 就是简单的计算
        if ( cloud_in.points[ i ].x * cloud_in.points[ i ].x + cloud_in.points[ i ].y * cloud_in.points[ i ].y + cloud_in.points[ i ].z * cloud_in.points[ i ].z < thres * thres )
            continue;//如果距离过小 则 不进行下面的赋值

        //点的距离满足要求  进行输出点云的赋值
        cloud_out.points[ j ] = cloud_in.points[ i ];
        j++;//输出点云个数 增加 1
    }

    /*最后输出点云的数量减少了*/
    if ( j != cloud_in.points.size() )
    {
            cloud_out.points.resize( j );//重新定义输出点云个数
    }

    /*根据点云的输出 设置点云 宽度*/
    cloud_out.height = 1;
    cloud_out.width = static_cast<uint32_t>( j );
    cloud_out.is_dense = true;//先设置为dense
}

测试

创建一个4个点的点云进行测试。设置距离为5，那么按照计算 下面的点云 会保留最后两个点云。

void test_removeClosedPointCloud(void)
{   
    /*创建一个 4个点 的点云  进行 测试*/
    pcl::PointCloud<pcl::PointXYZI> laserCloudIn;

    laserCloudIn.points.resize(4);

    laserCloudIn.points[0].x = 1;laserCloudIn.points[0].y = 1;laserCloudIn.points[0].z = 1;

    laserCloudIn.points[1].x = 2;laserCloudIn.points[1].y = 2;laserCloudIn.points[1].z = 2;

    laserCloudIn.points[2].x = 3;laserCloudIn.points[2].y = 3;laserCloudIn.points[2].z = 3;

    laserCloudIn.points[3].x = 4;laserCloudIn.points[3].y = 4;laserCloudIn.points[3].z = 4;

    /*打印去除前的点的数量*/
    std::cout<<"before "<<laserCloudIn.points.size()<<std::endl;

    /*调用去除非常近 的 点  的 函数 */
    removeClosedPointCloud(laserCloudIn,laserCloudIn,5);


     /*打印去除后的点的数量  和 各点 的 坐标*/
    std::cout<<"after "<<laserCloudIn.points.size()<<std::endl;

    for(size_t i;i<laserCloudIn.points.size();++i)
    {
        std::cout<<laserCloudIn.points[i].x<<'\t'<<laserCloudIn.points[i].y<<'\t'<<laserCloudIn.points[i].z<<std::endl;
    }

}

去除前的效果：
去除后效果：

参考链接