点云聚类及匹配（KD-Tree & OCTree）

系统管理员 2022-09-15 12:22 177阅读 0赞

# 0. 简介 #

最近在看点云匹配相关的知识点，而KD树和八叉树作为点云匹配中最为重要的方法，当然需要好好看看。这里写一篇博客记录一下，便于后面回顾。（最近发现SLAM、ROS方面已经基本粗略的写了一遍，后面会针对一些重要的点去零碎的填坑）。  
![在这里插入图片描述][f6b7bb13e8324f77b0f341e4562b7106.png]

# 1. KD-Tree #

KD-Tree, 或称 k 维树,是计算机科学中使用的一种数据结构,用来组织表示 k 维空间中的点集合。一般在会基于 FLANN 进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、邻域特征提取中是非常基础的核心操作。 KD-Tree 模块利用 两个类与两个函数实现了利用 KD-Tree数据结构对点云的高效管理和检索。  
![在这里插入图片描述][8f0b21c89ccd4ddbbe2f7ee15d06545e.png]  
Kd树有几种常用的搜素方法：

*  体素内近邻搜索 （Neighbors with Voxel Search） 它把查询点所在的体素中的其他点的索引作为查询结果返回；
 *  K近邻搜索（K Nearest Neighbor Serach）；
 *  半径内近邻搜索（Neighbor with Radius Search）

下面展示了在OPENCV中使用FLANN来生成KD-Tree的方法。

int main() {
        
    	int queryNum = 3;//用于设置返回邻近点的个数
    	vector<float> vecQuery(2);//存放查询点的容器
    	vector<int> vecIndex(queryNum);//存放返回的点索引
    	vector<float> vecDist(queryNum);//存放距离
    	cv::flann::SearchParams params(32);//设置knnSearch搜索参数
    	cv::flann::KDTreeIndexParams indexParams(2);
    									   
    	vecQuery[0] = 3, vecQuery[1] = 4;
    
    	cv::Mat source;
    	vector<cv::flann::Index*> kdtrees;
    	{
        
    		vector<cv::Vec2d> features = {
         {
         1,1 },{
         2, 2 },{
         3, 3 },{
         4, 4 },{
         2, 4 } };
    		source = cv::Mat(features).reshape(1);
    		source.convertTo(source, CV_32F);
    		cv::flann::Index* kdtree = new cv::flann::Index(source, indexParams);
    		kdtrees.push_back(kdtree);
    	}
    
    	kdtrees[0]->knnSearch(vecQuery, vecIndex, vecDist, queryNum, params);
    	for (int i = 0; i < vecIndex.size(); i++)
    		cout << "nearest id: " << vecIndex[i] << "\tdist:" << vecDist[i] << endl;
    
    	return 0;
    }

而PCL也可以使用KD-Tree，但是这个部分KD-Tree由于是PCL库的，所以一般适用于三维特征点。

#include <pcl/point_cloud.h>
    #include <pcl/kdtree/kdtree_flann.h>
    #include <iostream>
    #include <vector>
    #include <ctime>
    
    int main (int argc, char**argv)
    {
        
      srand (time (NULL));
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // 创建一个PointCloud<PointXYZ> boost共享指针，并进行实例化为cloud
      
      // 随机生成一个1000个点的无序点云
      cloud->width =1000; // 注意因为cloud是指针，所以这里用->
      cloud->height =1;
      cloud->points.resize (cloud->width * cloud->height);
      for (size_t i=0; i< cloud->points.size (); ++i)
      {
        
        cloud->points[i].x =1024.0f * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].y =1024.0f * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].z =1024.0f * rand () / (RAND_MAX + 1.0f);
      }
      
      pcl::KdTreeFLANN<pcl::PointXYZ>kdtree; // 创建k-d tree对象
      kdtree.setInputCloud (cloud); // 将cloud设为k-d tree是搜索空间
      
      // 随机生成查询点
      pcl::PointXYZ searchPoint;
      searchPoint.x=1024.0f * rand () / (RAND_MAX + 1.0f);
      searchPoint.y=1024.0f * rand () / (RAND_MAX + 1.0f);
      searchPoint.z=1024.0f * rand () / (RAND_MAX + 1.0f);
    
      int K =10;
      std::vector<int>pointIdxNKNSearch(K); // 设置一个整型的<vector>，用于存放第几近邻的索引
      std::vector<float>pointNKNSquaredDistance(K); // 设置一个浮点型的<vector>, 用于存放第几近邻与查询点的平方距离
    
      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)
        {
        
          std::cout<<"    "<<   cloud->points[ pointIdxNKNSearch[i] ].x  <<" "<< cloud->points[pointIdxNKNSearch[i] ].y  <<" "<< cloud->points[pointIdxNKNSearch[i] ].z <<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
        }
      }
    
      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;
      float radius =256.0f * rand () / (RAND_MAX + 1.0f); // 设置半径阈值
      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)
          std::cout<<"    "<<   cloud->points[ pointIdxRadiusSearch[i] ].x <<" "<< cloud->points[pointIdxRadiusSearch[i] ].y <<" "<< cloud->points[pointIdxRadiusSearch[i] ].z <<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
      }
      return 0;
    }

# 2.八叉树 #

![在这里插入图片描述][cb667c60bf124033974c7b3c479fbade.png]  
八叉树是一种用于管理稀疏3D数据的树状数据结构，每个内部节点都正好有8个子节点。  
八叉树也是一种索引方式，可以基于八叉树（Octrees）对点云进行空间划分和搜索操作。  
八叉树结构通过循环递归的划分方法对大小为2n \* 2n \* 2n的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。

## …详情请参照[古月居][Link 1] ##

[f6b7bb13e8324f77b0f341e4562b7106.png]: /images/20220828/3f29b5434f654a88a0da8260e3156e05.png
[8f0b21c89ccd4ddbbe2f7ee15d06545e.png]: /images/20220828/4cdbc55420944cfda8c7be78cd7ff6be.png
[cb667c60bf124033974c7b3c479fbade.png]: /images/20220828/812b9ecdd6bf4260b79367e086cea03f.png
[Link 1]: https://www.guyuehome.com/35583