代码目的:
假设有两片点云cloudA、cloudB,若在cloudB中找到cloudA的数据点,则从cloudB中删除该点。
#include <pcl/point_cloud.h> //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件
#include <iostream>
#include <vector>
#include <ctime>
const double eps = 1.0e-6;
int
main(int argc, char** argv)
{
//srand(time(NULL)); //用系统时间初始化随机种子
//创建一个PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
// 随机点云生成
cloudA->width = 20; //此处点云数量
cloudA->height = 1; //表示点云为无序点云
cloudA->points.resize(cloudA->width * cloudA->height);
for (size_t i = 0; i < cloudA->points.size(); ++i) //循环填充点云数据
{
cloudA->points[i].x = i + 1;
cloudA->points[i].y = i + 1;
cloudA->points[i].z = i + 1;
}
cloudB->width = 20; //此处点云数量
cloudB->height = 1; //表示点云为无序点云
cloudB->points.resize(cloudB->width * cloudB->height);
for (size_t i = 0; i < cloudB->points.size(); ++i) //循环填充点云数据
{
cloudB->points[i].x = i + 8;
cloudB->points[i].y = i + 8;
cloudB->points[i].z = i + 8;
}
//cloudBackup->points = cloud->points;
//创建KdTreeFLANN对象,并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
pcl::PointXYZ searchPoint;
int K = 1;
std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引
std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方
std::vector<pcl::PointXYZ> DeleteData;
int num = 0;
for (auto iterA = cloudA->begin(); iterA != cloudA->end(); iterA++)
{
searchPoint.x = iterA->x;
searchPoint.y = iterA->y;
searchPoint.z = iterA->z;
kdtree.setInputCloud(cloudB); //在cloudB中找到对应点后,在cloudB中直接删除该点
num = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
if (num > 0)
{
if (sqrt(pointNKNSquaredDistance[0])<eps)
{
auto iterB = cloudB->begin() + pointIdxNKNSearch[0];
cloudB->erase(iterB);
DeleteData.push_back(searchPoint);
if (cloudB->size()==0)
{
break;
}
searchPoint.x = 0;
searchPoint.y = 0;
searchPoint.z = 0;
num = 0;
pointIdxNKNSearch.clear();
pointNKNSquaredDistance.clear();
}
}
}
for (auto iter = DeleteData.begin(); iter != DeleteData.end(); iter++)
{
std::cout << iter->x << " " << iter->y << " " << iter->z << std::endl;
}
system("pause");
return 0;
}