PCL中使用KdTree在點雲中進行K近鄰及半徑查詢

KdTree背景知識

       KdTree(也稱k-d樹)是一種用來分割k維數據空間的高維空間索引結構，其本質就是一個帶約束的二叉搜索樹，基於KdTree的近似查詢的算法可以快速而準確地找到查詢點的近鄰，經常應用於特徵點匹配中的相似性算法。

       而索引結構中相似性查詢算法有兩種基本的方式：一種是範圍查詢（radius searches），另一種是K近鄰查詢（K-neighbor searches）。範圍查詢就是給定查詢點和查詢距離的閾值（以查詢點爲圓心，查詢距離爲半徑），從數據集中找出所有與查詢點距離小於閾值的數據（半徑內的數據）；K近鄰查詢是給定查詢點及正整數K，從數據集中找到距離查詢點最近的K個數據，當K=1時，就是最近鄰查詢（nearest neighbor searches）

       對於三維的點雲來說，所有的K-D樹都是三維的，構建k-d樹是一個逐級展開的遞歸過程，在每一級展開時都使用垂直於相應軸的超平面沿特定維度分割所有剩下數據集。在KdTree的根節點上，所有數據都將根據第一個維度進行拆分（如果第一個維度座標小於根節點數據，則子數據將位於左子樹中，如果子數據大於根節點數據，則子元素顯然位於右子樹中）。KdTree中的下一層在下一個維度上進行劃分，當所有其他維度都用盡後，將返回到第一個維度。構建K-D樹最有效的方法是使用一種類似於快速排序的分區方法，將中間點放在根節點上，然後比中間點小的數值放在左子樹上，比中間點大的數值放在右子樹上，最後在左右子樹上重複此過程，直到分割至最後一個元素。

PCL中KdTree的使用

PCL 中使用KdTree需要加入頭文件：

#include <pcl/kdtree/kdtree_flann.h>

 創建KdTree對象並設置輸入點雲：

  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud (cloud);

 K近鄰搜索：

初始化K值並新建兩個數組存儲搜索後得到的K緊鄰結果，kdtree調用nearestKSearch即可得到K個最緊鄰點的索引和平方距離。

  // K nearest neighbor search
  int K = 10;
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

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

特定半徑範圍內查詢（Radius Search）：

 新建兩個數組存儲搜索得到半徑R範圍內結果，然後調用radiusSearch得到半徑R內所有點的索引和平方距離。

  // Neighbors within radius search
  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;
  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

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

 編譯並運行程序

開發環境： VS2015 & PCL1.8.1

工程配置PCL頭文件及庫

Include:
$(PCL_ROOT);
$(PCL_ROOT)\include\pcl-1.8;
$(PCL_ROOT)\3rdParty\Eigen\eigen3;
$(PCL_ROOT)\3rdParty\FLANN\include;
$(PCL_ROOT)\3rdParty\VTK\include\vtk-8.0;
$(PCL_ROOT)\3rdParty\Boost\include\boost-1_64;

lib:
$(PCL_ROOT)\lib\*.lib
$(PCL_ROOT)\3rdParty\VTK\lib\*.lib
$(PCL_ROOT)\3rdParty\FLANN\lib\*.lib
$(PCL_ROOT)\3rdParty\Boost\lib\*.lib

相關工程與程序可在我的Github: https://github.com/yazhouzheng/KdTreeInPCL 找到。

編譯運行程序可的如下結果：

D:\asher\project\KdTreeInPCL\bin>KdTreeInPCL.exe
K nearest neighbor search at (35.3125 465.594 237.813) with K=10
    45.0625 433.031 234.906 (squared distance: 1163.83)
    72.5313 437.719 211.656 (squared distance: 2846.4)
    48.5 514.344 210 (squared distance: 3324.01)
    20.25 501.813 281.719 (squared distance: 3466.44)
    4.90625 468.781 303.813 (squared distance: 5290.7)
    27 543.656 202.656 (squared distance: 7398.81)
    110.031 515.625 272.188 (squared distance: 9267.66)
    133.438 411.313 204.438 (squared distance: 13688.9)
    99.4063 375.813 189.219 (squared distance: 14530)
    87.9688 573.438 220.438 (squared distance: 14704.8)
Neighbors within radius search at (35.3125 465.594 237.813) with radius=128.359
    45.0625 433.031 234.906 (squared distance: 1163.83)
    72.5313 437.719 211.656 (squared distance: 2846.4)
    48.5 514.344 210 (squared distance: 3324.01)
    20.25 501.813 281.719 (squared distance: 3466.44)
    4.90625 468.781 303.813 (squared distance: 5290.7)
    27 543.656 202.656 (squared distance: 7398.81)
    110.031 515.625 272.188 (squared distance: 9267.66)
    133.438 411.313 204.438 (squared distance: 13688.9)
    99.4063 375.813 189.219 (squared distance: 14530)
    87.9688 573.438 220.438 (squared distance: 14704.8)
    150.969 479.719 200.313 (squared distance: 14982.1)

References

1 二維KdTree 的構造及查找算法介紹： https://baike.baidu.com/item/kd-tree/2302515

2 PCL中使用KdTree 搜索：http://www.pointclouds.org/documentation/tutorials/kdtree_search.php#kdtree-search