kd-tree理論以及在PCL 中的代碼的實現

(小技巧記錄：博客園編輯的網頁界面變小了使用Ctrl  ++來變大網頁字體)

通過雷達，激光掃描，立體攝像機等三維測量設備獲取的點雲數據，具有數據量大，分布不均勻等特點，作為三維領域中一個重要的數據來源，點雲主要是表征目標表面的海量點的集合，並不具備傳統網格數據的幾何拓撲信息，所以點雲數據處理中最為核心的問題就是建立離散點間的拓撲關系，實現基於鄰域關系的快速查找。

k-d樹 （k-dimensional樹的簡稱），是一種分割k維數據空間的數據結構。主要應用於多維空間關鍵數據的搜索（如：范圍搜索和最近鄰搜索）。K-D樹是二進制空間分割樹的特殊的情況。用來組織表示K維空間中點的幾何，是一種帶有其他約束的二分查找樹，為了達到目的，通常只在三個維度中進行處理因此所有的kd_tree都將是三維的kd_tree,kd_tree的每一維在指定維度上分開所有的字節點，在樹 的根部所有子節點是以第一個指定的維度上被分開。

k-d樹算法可以分為兩大部分，一部分是有關k-d樹本身這種數據結構建立的算法，另一部分是在建立的k-d樹上如何進行最鄰近查找的算法。

構建算法

k-d樹是一個二叉樹，每個節點表示一個空間范圍。表1給出的是k-d樹每個節點中主要包含的數據結構。

域名	數據類型	描述
Node-data	數據矢量	數據集中某個數據點，是n維矢量（這里也就是k維）
Range	空間矢量	該節點所代表的空間范圍
split	整數	垂直於分割超平面的方向軸序號
Left	k-d樹	由位於該節點分割超平面左子空間內所有數據點所構成的k-d樹
Right	k-d樹	由位於該節點分割超平面右子空間內所有數據點所構成的k-d樹
parent	k-d樹	父節點

 

先以一個簡單直觀的實例來介紹k-d樹算法。假設有6個二維數據點{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}，數據點 位於二維空間內（如圖1中黑點所示）。k-d樹算法就是要確定圖1中這些分割空間的分割線（多維空間即為分割平面，一般為超平面）。下面就要通過一步步展 示k-d樹是如何確定這些分割線的。

                  

 

由於此例簡單，數據維度只有2維，所以可以簡單地給x，y兩個方向軸編號為0,1，也即split={0,1}。

（1）確定split域的首先該取的值。分別計算x，y方向上數據的方差得知x方向上的方差最大，所以split域值首先取0，也就是x軸方向；

（2）確定Node-data的域值。根據x軸方向的值2,5,9,4,8,7排序選出中值為7，所以Node-data = （7,2）。這樣，該節點的分割超平面就是通過（7,2）並垂直於split = 0（x軸）的直線x = 7；

（3）確定左子空間和右子空間。分割超平面x = 7將整個空間分為兩部分，如圖2所示。x < = 7的部分為左子空間，包含3個節點{（2,3），（5,4），（4,7）}；另一部分為右子空間，包含2個節點{（9,6），（8,1）}。

 （4）k-d樹的構建是一個遞歸的過程。然后對左子空間和右子空間內的數據重復根節點的過程就可以得到下一級子節點（5,4）和（9,6）（也就是 左右子空間的'根'節點），同時將空間和數據集進一步細分。如此反復直到空間中只包含一個數據點，如圖1所示。最后生成的k-d樹如圖3所示。

 關於Kdtree算法的相關內容網上有很多比如blog.csdn.net/silangquan/article/details/41483689

 

查找算法

在k-d樹中進行數據的查找也是特征匹配的重要環節，其目的是檢索在k-d樹中與查詢點距離最近的數據點。這里先以一個簡單的實例來描述最鄰近查找的基本思路。

星號表示要查詢的點（2.1,3.1）。通過二叉搜索，順着搜索路徑很快 就能找到最鄰近的近似點，也就是葉子節點（2,3）。而找到的葉子節點並不一定就是最鄰近的，最鄰近肯定距離查詢點更近，應該位於以查詢點為圓心且通過葉 子節點的圓域內。為了找到真正的最近鄰，還需要進行'回溯'操作：算法沿搜索路徑反向查找是否有距離查詢點更近的數據點。此例中先從（7,2）點開始進行 二叉查找，然后到達（5,4），最后到達（2,3），此時搜索路徑中的節點為<（7,2），（5,4），（2,3）>，首先以（2,3）作為 當前最近鄰點，計算其到查詢點（2.1,3.1）的距離為0.1414，然后回溯到其父節點（5,4），並判斷在該父節點的其他子節點空間中是否有距離查 詢點更近的數據點。以（2.1,3.1）為圓心，以0.1414為半徑畫圓，如圖4所示。發現該圓並不和超平面y = 4交割，因此不用進入（5,4）節點右子空間中去搜索。

                                               

PCL中kd_tree模塊及類的介紹

 

對此類的函數有更加詳細的介紹

 

類KdTree關鍵成員函數

virtual void pcl::KdTree< PointT >::setInputCloud	(	const PointCloudConstPtr &	cloud,
		const IndicesConstPtr &	indices = IndicesConstPtr ()
	)

 

 設置輸入點雲，參數cloud 作為輸入點雲的共享指針引用，indices為在kd_tree中使用的點對應的索引，如果不設置，則默認使用整個點雲填充kd_tree

 

 

virtual int pcl::KdTree< PointT >::nearestKSearch	(	int	index,
		int	k,
		std::vector< int > &	k_indices,
		std::vector< float > &	k_sqr_distances
	)		const

純虛函數，具體實現在其子類KdTreeFLANN中，其用來進行K 領域搜索，k_sqr_distances 為搜索完成后每個鄰域點與查詢點的歐式距離，

還更多的成員的介紹查看 docs.pointclouds.org/trunk/classpcl_1_1_kd_tree.html#ac81c442ff9c9b1e03c10cb55128e726d

 應用實例

 新建文件

 

#include <pcl/point_cloud.h>        //點類型定義頭文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree類定義頭文件

#include <iostream>
#include <vector>
#include <ctime>

int
main (int argc, char** argv)
{
  srand (time (NULL));   //用系統時間初始化隨機種子
  //創建一個PointCloud<pcl::PointXYZ>
  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);
  }
 //創建KdTreeFLANN對象，並把創建的點雲設置為輸入,創建一個searchPoint變量作為查詢點
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
 //設置搜索空間
  kdtree.setInputCloud (cloud);
  //設置查詢點並賦隨機值
  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);

  // K 臨近搜索
  //創建一個整數（設置為10）和兩個向量來存儲搜索到的K近鄰，兩個向量中，一個存儲搜索到查詢點近鄰的索引，另一個存儲對應近鄰的距離平方
  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 )  //執行K近鄰搜索
  {
     //打印所有近鄰坐標
    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;
  }
  /**********************************************************************************
   下面的代碼展示查找到給定的searchPoint的某一半徑（隨機產生）內所有近鄰，重新定義兩個向量
   pointIdxRadiusSearch  pointRadiusSquaredDistance來存儲關於近鄰的信息
   ********************************************************************************/
  // 半徑 R內近鄰搜索方法

  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 )  //執行半徑R內近鄰搜索方法
  {
    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;
}

  編譯執行的結果如圖：

 

微信公眾號號可掃描二維碼一起共同學習交流

未完待續************************************8