距离处理
去除掉非常近的点
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左右的点
把滤掉的点显示出来
确实这些点不咋地
评论(0)
您还未登录,请登录后发表或查看评论