距离处理

去除掉非常近的点

Code

/* 
函数功能 :移除 过 近  的 点   可输入 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;
    }

}

结果正确

模拟数据
在这里插入图片描述

实际数据。用的livox的一帧输出的数据经过 去除后,平均一帧能构去掉8000左右的点。
在这里插入图片描述
++++++++++++++++++++++++++++++++++++++++++++
去除后的效果
在这里插入图片描述

去除掉的点
在这里插入图片描述
就是lidar 的运动的轨迹,因为离lidar很近

++++++++++++++++++++++++++++++++++++++++++++
去除前的效果
在这里插入图片描述
++++++++++++++++++++++++++++++++++++++++++++

结果分析

从实际测试的数据上来看,livox雷达,每帧 数据有24000,减到16000.

从三维建模上看 虽然并没有什么效果

但是每帧减少的8000个点会 加快算法速度。减少无意义的计算

去除掉非常近的点

Code

/* 
函数功能 :移除 过 远  的 点   可输入 cloud_in = cloud_out 这样就是处理一个点云,自身发生处理
参数1  输入点云;   参数2  输出点云;   参数3  阈值:距离  
已通过测试
time 2020.6.28
jone
*/
template <typename PointT>  //模板点云类型   可以适应各类pcl的点云类型
void removeFarPointCloud( 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;
    }

}

结果正确

模拟数据
在这里插入图片描述

可以通过这个函数看下上面去掉的是哪些点
依然设置第三个参数为 0.1

removeFarPointCloud(laserCloudIn,laserCloudIn,0.1);

结果如下
在这里插入图片描述
是lidar的运动轨迹。以为离lidar很近嘛。这些点完全没有用可以舍掉

设置参数3 距离为 50 ,即去掉大于50m的点。

removeFarPointCloud(laserCloudIn,laserCloudIn,50);

在这里插入图片描述
有的场景会去掉500个左右

这个主要是看应用场景了

去除NaN的点

Code

//去出NaN的点    pcl的库,直接用就可以 不写函数了
pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
std::vector<int> indices;
// 移除 NaN 的 点  pcl  的 库
pcl::removeNaNFromPointCloud( laserCloudIn, laserCloudIn, indices );

直接调用pcl库的 方法。

经过测试livox的输出结果很少有NaN的点

反射率处理

去除掉反射率很小的点

Code

/*
函数功能 : 去除点的强度较小的点  sigma= I/[(y*y+z*z)/x*x]   可输入 cloud_in = cloud_out 这样就是处理一个点云,自身发生处理
参数1  输入点云;   参数2  输出点云;   参数3  阈值:强度   livox mid 设置一般为7e-4
已通过测试
time 2020.6.29
jone
*/
template <typename PointT>  //模板点云类型   可以适应各类pcl的点云类型 但是要有强度值
void removeLowReflectivityPointCloud (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 )//遍历输入点云
    { 
        //计算反射率  sigma= I/[(y*y+z*z)/x*x]
        float sigma = cloud_in.points[i].intensity/dis2_xy(cloud_in.points[i].y / cloud_in.points[i].x,cloud_in.points[i].z / cloud_in.points[i].x);
        //判断 就是简单的计算
        if (sigma < 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

}

测试

void test_removeLowReflectivityPointCloud(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[0].intensity=7e-5;

    laserCloudIn.points[1].x = 2;laserCloudIn.points[1].y = 2;laserCloudIn.points[1].z = 2;laserCloudIn.points[1].intensity=7e-1;

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

    laserCloudIn.points[3].x = 4;laserCloudIn.points[3].y = 4;laserCloudIn.points[3].z = 4;laserCloudIn.points[3].intensity=7e-7;

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

    /*调用去除非常近 的 点  的 函数 */
    removeLowReflectivityPointCloud(laserCloudIn,laserCloudIn,7e-4);


     /*打印去除后的点的数量  和 各点 的 坐标*/
    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<<'\t'<<laserCloudIn.points[i].intensity<<std::endl;
    }

}

结果

模拟数据
在这里插入图片描述
实际数据每帧会少几百至1千个
在这里插入图片描述

去除入射角较小的点

入射角,是雷达射线和测量点周围的平面的夹角

其中 a点和c点是b点相邻的两个,关系如下图所示
在这里插入图片描述
计算公式如下
在这里插入图片描述
公式的含义就是通过向量点乘的方式,计算向量ca和ob的夹角。
在这里插入图片描述
筛选:入射角接近π或者0,去除。
Livox MID40的范围为5°-175°,像图中的f点,因为这样角度入射的激光点会被拉长。

Code

/*
函数功能 : 去除掉 入射角 小的 点  实质是求两个向量的夹角  可输入 cloud_in = cloud_out 这样就是处理一个点云,自身发生处理
参数1  输入点云;   参数2  输出点云;   参数3  阈值:最小入射角   livox mid 设置一般为  5
已通过测试
time 2020.6.29
jone
*/
template <typename PointT>  //模板点云类型   可以适应各类pcl的点云类型 但是要有强度值
void removeMinimumViewAnglePointCloud (const pcl::PointCloud<PointT> &cloud_in,      //参数1  输入点云
                                     pcl::PointCloud<PointT> &cloud_out,     //参数2  输出点云
                                     float thres                             //参数3  阈值:最小入射角 
                              )
{

     size_t neighbor_ssd_size = 2;//判断点 与 前后 相邻的 此变量 的 点 进行判断

    /*如何参数 点云输入 和 输出 不是一个 则需要 先赋值 输出点云的 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 = neighbor_ssd_size; i < cloud_in.points.size()-neighbor_ssd_size; ++i )//遍历输入点云
    { 
        /* ****计算入射角*****  */
        Eigen::Matrix< float, 3, 1 > vec_a( cloud_in.points[ i ].x, cloud_in.points[ i ].y, cloud_in.points[ i ].z );
        Eigen::Matrix< float, 3, 1 > vec_b( cloud_in.points[ i + neighbor_ssd_size ].x - cloud_in.points[ i - neighbor_ssd_size ].x,
                                            cloud_in.points[ i + neighbor_ssd_size ].y - cloud_in.points[ i - neighbor_ssd_size ].y,
                                            cloud_in.points[ i + neighbor_ssd_size ].z - cloud_in.points[ i - neighbor_ssd_size ].z );

        float view_angle = Eigen_math::vector_angle( vec_a  , vec_b, 1 ) * 57.3;//计算入射角
        std::cout<<"view_angle "<<view_angle<<std::endl; 
        //判断 就是简单的计算
        if (view_angle < 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

}

测试

直接用实际点云测试的

每帧会少个 1w左右的点
在这里插入图片描述
把滤掉的点显示出来
在这里插入图片描述
确实这些点不咋地