PCL学习笔记——利用kdtree结构找出点云的重叠部分

目的为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中对应了一个点）