nanoflannでKdTreeを使う

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

}