0. 简介

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

1. KD-Tree

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

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

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>

int main (int argc, char**argv)
{
  srand ((unsigned int) time (NULL));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  cloud->width =1000;
  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);
  }

  float resolution =128.0f;
  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //设置最深层八叉树的最小体素尺寸,初始化八叉数
  octree.setInputCloud (cloud);
  octree.addPointsFromInputCloud ();

  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);

  std::vector<int>pointIdxVec;
  if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
    std::cout<<"Neighbors within voxel search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<")"<<std::endl;
    for (size_t i=0; i<pointIdxVec.size (); ++i)
    {
      std::cout<<"    "<< cloud->points[pointIdxVec[i]].x <<" "<< cloud->points[pointIdxVec[i]].y <<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
    }
  }

  int K =10;
  std::vector<int>pointIdxNKNSearch;
  std::vector<float>pointNKNSquaredDistance;
  std::cout<<"K nearest neighbor search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<") with K="<< K <<std::endl;
  if (octree.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 (octree.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;
    }
  }
}

3. 算法比对

最近邻搜索nn问题有很多中方法,nns问题最差的时间复杂是O(nm);平均复杂度是O(nlogm);在点云领域常用的是kd-tree octree;其他的方法有R-tree、range-tree、vp-tree

八叉树中的NNS必须对3个分割平面进行邻近度计算,对距离进行排序,并为每个经过的节点选择合适的遍历顺序。 与此相比,k-d树中的遍历顺序可通过一次邻近度检查立即确定,从而避免了不必要访问节点时的不必要计算。所以从下图来看八叉树的速率会低于KD-Tree。

在这里插入图片描述

4. 参考链接

https://blog.csdn.net/u011426016/article/details/100145791

https://blog.csdn.net/qq_40985985/article/details/108381582

https://blog.csdn.net/qq_36814762/article/details/110946583

https://blog.csdn.net/qq_33287871/article/details/107332728