スポンサーリンク
nanoflannというヘッダオンリーのライブラリがあり、KdTreeを実装している。
flannとついているがFLANN（Fast Library for Approximate Nearest Neighbors）とは別物らしい。
https://github.com/jlblancoc/nanoflann
ヘッダオンリーで、nanoflann.hppをプロジェクトにインクルードするだけで使える。
さらに、点群型をテンプレートで指定するのでOpen3D等の他のライブラリと併用するときも便利。
#include <iostream> #include <vector> #include <array> #include "nanoflann.hpp"
// 点群データを表現するクラス struct MyPointCloud { // 点群データ std::vector< std::array<float,3> > points; // nanoflannで必要となるインタフェース関数 inline size_t kdtree_get_point_count() const { return points.size(); } // nanoflannが呼び出す、点群の座標を返す関数 // dim [0] [1] [2] は、それぞれ x, y, z inline float kdtree_get_pt(const size_t idx, const size_t dim) const { return points[idx][dim]; } // BBOXは使わなくても定義は必要 template <class BBOX> bool kdtree_get_bbox(BBOX&) const { return false; } };
/////////////////////////////////////////////////////////// // KdTreeの定義（L2ノルム L2_Simple_Adaptor を使う3次元の点群） // L2ノルムは、ユークリッド距離の2乗 // L1ノルムは、マンハッタン距離 using KdTree = nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor<float, MyPointCloud>, MyPointCloud, 3 /* 次元数 */>; ///////////////////////////////////////////////////////////
void createCloud(MyPointCloud& cloud) { // サンプル点群を作成 for (size_t i = 0; i < 30000; i++) { cloud.points.push_back(std::array<float, 3>{ static_cast<float>(rand()) / RAND_MAX, static_cast<float>(rand()) / RAND_MAX, static_cast<float>(rand()) / RAND_MAX } ); } }
int main() { MyPointCloud cloud; createCloud(cloud); //////////////////////////////////////////////////////////////////////// // leaf max size は1にしているので、最後まで分割される // leaf max size を2以上にすると、ツリーの末端では配列になり、総当たりでの探索になる KdTree tree( 3 /* 次元数 */, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(1 /* leaf max size */) ); tree.buildIndex(); //////////////////////////////////////////////////////////////////////// float P[3] = { 0.5, 0.5, 0.5 }; float radius = 0.2; float search_radius = radius * radius; // 半径の二乗 // 結果の格納先 std::vector<nanoflann::ResultItem<unsigned int, float>> ret_matches; nanoflann::SearchParameters params; // 探索 const size_t nMatches = tree.radiusSearch( P, search_radius, ret_matches, params ); //////////////////////////////////////////////////////////////////////// std::cout << "found " << nMatches << " points\n"; for (size_t i = 0; i < nMatches; i++) { std::cout << i << " point index: " << ret_matches[i].first << " distance: " << sqrt(ret_matches[i].second) << std::endl; } }