【PCL模块解析 05 之KDTree】01 KDTree原理及代码解析

雨点打透心脏的1/2处 2021-06-11 15:14 1007阅读 1赞

**目录**

一、前言

二、KDTree简介

1、基本概念

2、基本原理

三、代码及解析

1、全部代码

2、代码分段解析

1.头文件

2.文件加载

3.kd树变量与查找点

4.可视化

5.创建字符流

6.文件加载

7.近邻查找算法

8.可视化

--------------------

**一、前言**

最近在学习点云库，由于刚刚入门，很多东西也不是很了解，如果大家有什么问题，都可以跟我沟通交流。除了通过博客交流外，欢迎你加入我的QQ群（326866692），一起交流有关于机器学习、深度学习、计算机视觉的内容。目前我并未确定具体的研究方向，所以现在处于广泛涉猎阶段，希望我们能够一起沟通。下图是我的群二维码：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_2FFF3F_t_70][]

今天要分享的是**PCL**中的**KDTree。**

为什么是模块05呢？这个是按照官网中模块顺序写的。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_FF86FF_t_70][]

**二、KDTree简介**

## 1、基本概念 ##

kd树（k维树）是计算机科学中用于在具有k维度的空间中组织一些点的数据结构。它是一个二进制搜索树，其上施加了其他约束。

**Kd树对范围和最近邻居搜索非常有用**。出于我们的目的，我们通常只处理三维点云，因此我们所有的kd树都是三维的。kd树的每个级别使用垂直于相应轴的超平面沿着特定维度分割所有子级。

## 2、基本原理 ##

在树的根部，所有子节点将根据第一维分割（即，如果第一个维度坐标小于它将在左子树中的根，如果它大于根，它显然将在正确的子树）。树中的每个级别在下一个维度上划分，一旦所有其他维度都耗尽，则返回到第一个维度。它们构建kd树的最有效方法是使用分区方法，例如Quick Sort用于将中间点放置在根处，并且一切具有较小的一维值，左侧较大，右侧较大。然后，在左侧和右侧子树上重复此过程，直到您要分区的最后一棵树仅由一个元素组成。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F7548F_t_30][] 二维KD树示例

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F7548F_t_30 1][] 最近邻搜索演示

**三、代码及解析**

**1、全部代码**

#include "stdafx.h"
    #include <pcl/point_cloud.h>
    #include <pcl/kdtree/kdtree_flann.h>
    #include <pcl/io/pcd_io.h>
    #include <pcl/visualization/cloud_viewer.h>
    
    #include <iostream>
    #include <vector>
    #include <ctime>
    #include <string.h>
    
    int main(int argc, char** argv)
    {
    	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    	pcl::PCDReader reader;   //读取PCD
    	//读取文件 
    	reader.read("D:/Plane2D.pcd", *cloud);
    	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl;
    
    	//创建kdtree对象，并将读取到的点云设置为输入。
    	pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    	kdtree.setInputCloud(cloud);
    
    	pcl::PointXYZRGBA searchPoint;
    	searchPoint.x = 0;
    	searchPoint.y = 0;
    	searchPoint.z = 0;
    
    	//可视化
    	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_plane2D(new pcl::visualization::PCLVisualizer("2D Viewer plane"));
    	viewer_plane2D->addPointCloud<pcl::PointXYZRGBA>(cloud, "sample cloud - plane ");
    	std::string lineId = "line";
    	std::stringstream ss;
    	
    	 //K nearest neighbor search
    	int K = 10;
    	std::vector<int> pointIdxNKNSearch(K);
    	std::vector<float> pointNKNSquaredDistance(K);
    	std::cout << "K nearest neighbor search at (" << searchPoint.x
    		<< " " << searchPoint.y	<< " " << searchPoint.z	<< ") with K=" << K << std::endl;
    	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    	{
    		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
    		{
    			ss << i;
    			lineId += ss.str();
    			viewer_plane2D->addLine<pcl::PointXYZRGBA>(cloud->points[pointIdxNKNSearch[i]], searchPoint, 1, 58, 82, lineId);
    			std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y
    				<< " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    		}
    	}
    
    	// Neighbors within radius search
    	//创建2个向量来存储有关近邻的信息。
    	std::vector<int> pointIdxRadiusSearch;
    	std::vector<float> pointRadiusSquaredDistance;
    	float radius = 0.05;//近邻半径为0.05m
    	
    	std::cout << "Neighbors within radius search at (" << searchPoint.x	<< " " << searchPoint.y
    		<< " " << searchPoint.z	<< ") with radius=" << radius << std::endl;
    	if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    	{
    		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
    			ss << i;
    			lineId += ss.str();
    			viewer_plane2D->addLine<pcl::PointXYZRGBA>(cloud->points[pointIdxRadiusSearch[i]], searchPoint, 1, 58, 82, lineId);
    			std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y
    				<< " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    		}
    	}
    	while (!viewer_plane2D->wasStopped())
    	{
    		viewer_plane2D->spinOnce(100);
    		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    	}
    
    	system("pause");
    	return 0;
    }

**注：需要提前下载好对应的pcd文件。也可以自己利用随机数创建PCD文件。**

**2、代码分段解析**

### 1.头文件 ###

#include <pcl/point_cloud.h>  //点云使用
    #include <pcl/kdtree/kdtree_flann.h> //kd树使用
    #include <pcl/io/pcd_io.h> //读入文件使用
    #include <pcl/visualization/cloud_viewer.h> //可视化使用
    
    #include <iostream>
    #include <vector>
    #include <string.h>

### 2.文件加载 ###

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    	pcl::PCDReader reader;   //读取PCD
    	//读取文件 
    	reader.read("D:/Plane2D.pcd", *cloud);
    	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl;//输出pcd文件包含点的个数

### 3.kd树变量与查找点 ###

要用kd树找最近的几个点，要先创建变量，并且指定，要找哪个点的最近几个点。

**查找点的坐标是自己设定的，但是一定要在自己的点云文件范围之内。**

//创建kdtree对象，并将读取到的点云设置为输入。
    	pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    	kdtree.setInputCloud(cloud);
    
    	pcl::PointXYZRGBA searchPoint;
    	searchPoint.x = 0;
    	searchPoint.y = 0;
    	searchPoint.z = 0;

### 4.可视化 ###

可视化的目的是为了让大家能够更好的看到效果。

**注：这里的可视化是创建可视化变量，并开辟一个窗口来展示图像。在后面的代码才会显示点云文件。**

//可视化
    	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_plane2D(new pcl::visualization::PCLVisualizer("2D Viewer plane"));
    	viewer_plane2D->addPointCloud<pcl::PointXYZRGBA>(cloud, "sample cloud - plane ");

### 5.创建字符流 ###

为了让大家能更直观的看到最近邻点，将所有的点与查找点连线，每条连线都需要有自己独立的ID，不能有重复，所以需要定义字符流来创建不同的ID。

std::string lineId = "line";
    	std::stringstream ss;

### 6.文件加载 ###

### 7.近邻查找算法 ###

这里有两个近邻查找，一个是查找最近的K个点（我们叫他K最近邻），一个是查找距离在某个范围（某个半径R）之内的点（我们叫他R最近邻）。

**（1）K最近邻**

//K nearest neighbor search
    	int K = 10;
    	std::vector<int> pointIdxNKNSearch(K);
    	std::vector<float> pointNKNSquaredDistance(K);
    	std::cout << "K nearest neighbor search at (" << searchPoint.x
    		<< " " << searchPoint.y	<< " " << searchPoint.z	<< ") with K=" << K << std::endl;
    	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    	{
    		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
    		{
    			ss << i;
    			lineId += ss.str();
    			viewer_plane2D->addLine<pcl::PointXYZRGBA>(cloud->points[pointIdxNKNSearch[i]], searchPoint, 1, 58, 82, lineId);
    			std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y
    				<< " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    		}
    	}

执行结果如下：

> PointCloud before filtering has: 3465 data points.
> 
> K nearest neighbor search at (0 0 0) with K=10
> 
>     0.0388017 -0.000405371 0 (squared distance: 0.00150574)
> 
>     0.0387321 -0.0049463 0 (squared distance: 0.00152464)
> 
>     0.0388713 0.00415187 0 (squared distance: 0.00152821)
> 
>     0.0386857 -0.00947657 0 (squared distance: 0.00158639)
> 
>     0.0389872 0.00873581 0 (squared distance: 0.00159631)
> 
>     0.0386623 -0.0140043 0 (squared distance: 0.0016909)
> 
>     0.0390335 0.0133232 0 (squared distance: 0.00170112)
> 
>     0.038639 -0.0185266 0 (squared distance: 0.00183621)
> 
>     0.0391029 0.017932 0 (squared distance: 0.00185059)
> 
>     0.043377 -0.000405608 0 (squared distance: 0.00188173)

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F76FFF_t_70][]

可以看到，他是找到了最近的十个点，九个在第一列上，一个在第二列中间。

**（1）R最近邻**

// Neighbors within radius search
    	//创建2个向量来存储有关近邻的信息。
    	std::vector<int> pointIdxRadiusSearch;
    	std::vector<float> pointRadiusSquaredDistance;
    	float radius = 0.05;//近邻半径为0.05m
    	
    	std::cout << "Neighbors within radius search at (" << searchPoint.x	<< " " << searchPoint.y
    		<< " " << searchPoint.z	<< ") with radius=" << radius << std::endl;
    	if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    	{
    		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
    			ss << i;
    			lineId += ss.str();
    			viewer_plane2D->addLine<pcl::PointXYZRGBA>(cloud->points[pointIdxRadiusSearch[i]], searchPoint, 1, 58, 82, lineId);
    			std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y
    				<< " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    		}
    	}

执行结果如下：

> PointCloud before filtering has: 3465 data points.
> 
> Neighbors within radius search at (0 0 0) with radius=0.05
> 
>     0.0388017 -0.000405371 0 (squared distance: 0.00150574)
> 
>     0.0387321 -0.0049463 0 (squared distance: 0.00152464)
> 
>     0.0388713 0.00415187 0 (squared distance: 0.00152821)
> 
>     0.0386857 -0.00947657 0 (squared distance: 0.00158639)
> 
>     0.0389872 0.00873581 0 (squared distance: 0.00159631)
> 
>     0.0386623 -0.0140043 0 (squared distance: 0.0016909)
> 
>     0.0390335 0.0133232 0 (squared distance: 0.00170112)
> 
>     0.038639 -0.0185266 0 (squared distance: 0.00183621)
> 
>     0.0391029 0.017932 0 (squared distance: 0.00185059)
> 
>     0.043377 -0.000405608 0 (squared distance: 0.00188173)
> 
>     0.0432992 -0.0049492 0 (squared distance: 0.00189932)
> 
>     0.0434806 0.00415678 0 (squared distance: 0.00190784)
> 
>     0.0432214 -0.00947646 0 (squared distance: 0.00195789)
> 
>     0.0435842 0.00874091 0 (squared distance: 0.00197598)
> 
>     0.038546 -0.0230018 0 (squared distance: 0.00201488)
> 
>     0.039149 0.0225437 0 (squared distance: 0.00204087)
> 
>     0.0431435 -0.0139874 0 (squared distance: 0.00205701)
> 
>     0.0436618 0.0133389 0 (squared distance: 0.00208428)
> 
>     0.0431174 -0.0185042 0 (squared distance: 0.00220151)
> 
>     0.0384298 -0.0274387 0 (squared distance: 0.00222973)
> 
>     0.0437393 0.0179531 0 (squared distance: 0.00223544)
> 
>     0.039172 0.0271501 0 (squared distance: 0.00227157)
> 
>     0.0480148 -0.00040633 0 (squared distance: 0.00230558)
> 
>     0.0479002 -0.00495507 0 (squared distance: 0.00231898)
> 
>     0.0481006 0.00416169 0 (squared distance: 0.00233099)
> 
>     0.0478142 -0.00948771 0 (squared distance: 0.00237622)
> 
>     0.0430913 -0.0230154 0 (squared distance: 0.00238657)
> 
>     0.0481864 0.008746 0 (squared distance: 0.00239843)
> 
>     0.043765 0.0225568 0 (squared distance: 0.00242419)
> 
>     0.0476995 -0.0139956 0 (squared distance: 0.00247112)
> 
>     0.0383367 -0.0318675 0 (squared distance: 0.00248524)

**注：这两个代码，执行其中一个即可，另一个可以先注释掉。**

### 8.可视化 ###

上一个可视化是创建一个界面，并没有点云文件，这个可视化是将点云文件显示在界面上。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_FFFFFF_t_50][]

这个代码就是以0.05为半径的全部点找出来了，他不会考虑找到多少个点，只考虑距离是否在我们要求的范围之内。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_2FFF3F_t_70]: /images/20210516/c41ab68411ed4d3584c5e80abe5d60ac.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_FF86FF_t_70]: /images/20210516/55de8b38ebfe46538573d03fb3b4f63b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F7548F_t_30]: /images/20210516/e1719b1095d94d278c0067410ed1c623.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F7548F_t_30 1]: /images/20210516/61174ae951e7409282aa78d9925bcfb4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_F76FFF_t_70]: /images/20210516/ebaabbf6e7ba4670847769d0f9fc3fe9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NodWl5aXhpbg_size_26_color_FFFFFF_t_50]: /images/20210516/a4ccab15538d4bc7988e994fc5a36cd8.png