目的为cloudA中的每个点,在cloudB中找到对应的重叠点(将点与点之间的距离小于某个阈值的点视为重叠点)
主要函数:
int pcl::OrganizedNeighborSearch< PointT >::nearestKSearch ( const PointCloudConstPtr & cloud_arg,
int index_arg,
int k_arg,
std::vector< int > & k_indices_arg,
std::vector< float > & k_sqr_distances_arg
)
Search for k-nearest neighbors at the query point.
Parameters:
cloud_arg :the point cloud data
index_arg :the index in cloud representing the query point
k_arg :the number of neighbors to search for
k_indices_arg :the resultant indices of the neighboring points (must be resized to k a priori!)
k_sqr_distances_arg :the resultant squared distances to the neighboring points (must be resized to k a priori!)
Returns :
number of neighbors found
code:
// get_index_of_overlap.cpp: 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include "stdafx.h"
#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<pcl/kdtree/io.h>
#include<vector>
#include<fstream>
using namespace std;
int main()
{
ofstream overlapA,overlapB,test;
overlapA.open("overlapA.txt");
overlapB.open("overlapB.txt");
test.open("test.txt");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
//读取点云文件
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pointA.pcd", *cloudA) == -1)
{
PCL_ERROR("Couldn't read file cloudA.pcd\n");
return -1;
}
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pointB.pcd", *cloudB) == -1)
{
PCL_ERROR("Couldn't read file cloudB.pcd\n");
return -1;
}
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
kdtree.setInputCloud(cloudB); //在cloudB中进行索引
int K = 1; //索引数目
vector<int>pointIdxNKNSearch(K); //存储索引
vector<float>pointNKNSquaredDistance(K);
//PointA中的每一个点在PointB中找到最近的一个对应点,分别输出对应存在于A,B中的点
for (size_t i = 0; i < cloudA->size(); i++) {
pcl::PointXYZ searchPoint;
searchPoint.x = cloudA->points[i].x;
searchPoint.y = cloudA->points[i].y;
searchPoint.z = cloudA->points[i].z; //可能会存在A中的多个点在B中对应了一个点,导致二者重复点数量不一致
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
if (pointNKNSquaredDistance[0] < 0.04) { //距离平方小于0.04的视为重叠点
//将属于点云A的重叠点输入overlapA文档中
overlapA << searchPoint.x << " " << searchPoint.y << " "
<< searchPoint.z << " " << endl;
//将属于点云B的重叠点输入overlapB文档中
overlapB << cloudB->points[pointIdxNKNSearch[0]].x << " "
<< cloudB->points[pointIdxNKNSearch[0]].y << " "
<< cloudB->points[pointIdxNKNSearch[0]].z << endl;
//想法一:设置点位置,将输出的重叠B点设为离群点,防止被重复计算输出,失败!因为kdtree在索引之前已经被建立
//cloudB->points[pointIdxNKNSearch[0]].x = 0;
//cloudB->points[pointIdxNKNSearch[0]].y = 0;
//cloudB->points[pointIdxNKNSearch[0]].z = 0;
//想法二:每次删除已计算点,重新赋值kdtree,不行!速度太慢
//pcl::PointCloud<pcl::PointXYZ>::iterator index = cloudB->begin();
//删除对应索引位置的点
//cloudB->erase(index+ pointIdxNKNSearch[0]);
//kdtree.setInputCloud(cloudB);
}
}
}
overlapA,overlapB.close();
return 0;
}
结果图:
存在问题:两片重叠点云的数目不匹配!(原因就是可能会存在A中的多个点在B中对应了一个点)