一、距离处理
去除掉非常近的点。
/*
函数功能 :移除 过 近 的 点 可输入 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;
}
}
去除前的效果:
去除后效果: