PCLでKdTreeを使用して点群でK近隣および半径クエリーを行う

KdTreeの背景知識
KdTree(k-dツリーとも呼ばれる)は、k次元データ空間を分割するための高次元空間インデックス構造であり、本質は制約付きの二叉探索ツリーであり、KdTreeの近似クエリに基づくアルゴリズムは、クエリ点の近隣を迅速かつ正確に見つけることができ、特徴点マッチングにおける類似性アルゴリズムによく適用される.
インデックス構造における類似性クエリーアルゴリズムには、範囲クエリー(radius searches)とK近隣クエリー(K-neighbor searches)の2つの基本的な方法がある.範囲クエリーは、指定されたクエリーポイントとクエリー距離のしきい値(クエリーポイントを中心とし、クエリー距離を半径とする)であり、データセットからクエリーポイントとの距離がしきい値より小さいすべてのデータ(半径内のデータ)を探し出す.K近隣クエリーは、所与のクエリーポイントおよび正の整数Kであり、データセットからクエリーポイントに最も近いK個のデータが見つかり、K=1の場合、近隣クエリー(nearest neighbor searches)である
3 Dの点群では、すべてのK-Dツリーが3 Dであり、k-dツリーを構築することは、各レベルの展開時に対応する軸に垂直なスーパープレーンを使用して、残りのすべてのデータセットを特定の次元に沿って分割する逐次展開の再帰的なプロセスです.KdTreeのルートノードでは、すべてのデータが最初の次元に基づいて分割されます(最初の次元座標がルートノードデータより小さい場合、サブデータは左サブツリーに、サブデータがルートノードデータより大きい場合、サブ要素は右サブツリーに表示されます).KdTreeの次の階層は次の次元で分割され、他のすべての次元が使い果たされると最初の次元に戻ります.K-Dツリーを構築する最も効果的な方法は、高速ソートのようなパーティション化方法を使用して、中間点をルートノードに配置し、中間点より小さい数値を左サブツリーに配置し、中間点より大きい数値を右サブツリーに配置し、最後に最後の要素に分割するまで左右のサブツリーでこのプロセスを繰り返します.
PCLにおけるKdTreeの使用
PCLでKdTreeを使用するには、ヘッダファイルを追加する必要があります.

#include 

KdTreeオブジェクトを作成し、入力点群を設定します.

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

K近隣検索:
K値を初期化し、2つの配列を新規作成して検索後のK近傍結果を格納し、kdtreeがnearestKSearchを呼び出すと、K近傍のインデックスと平方距離が得られます.

  // K nearest neighbor search
  int K = 10;
  std::vector pointIdxNKNSearch(K);
  std::vector 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):
新しい2つの配列は、半径Rの範囲内の検索結果を格納し、radiusSearchを呼び出して半径R内のすべての点のインデックスと平方距離を取得します.

  // Neighbors within radius search
  std::vector pointIdxRadiusSearch;
  std::vector 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;
  }

プログラムのコンパイルと実行
開発環境:VS 2015&PCL 1.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 2 DKdTreeの構造と検索アルゴリズムの紹介:https://baike.baidu.com/item/kd-tree/2302515
2 PCLでKdTree検索を使用する:http://www.pointclouds.org/documentation/tutorials/kdtree_search.php#kdtree-search