KDtree最近点查找C++用例

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

在ICP求解运动估计问题中，首先要解决的就是特征关联问题，一般都是利用KD树进行最近邻（NN）搜索，即在点云集A中找出其中每个点在点云集B中的距离最近的点，以实现数据关联。
与PCL中的KD树最近邻搜索相比，更推荐nanoflann方法，后者运行速度更快，且只需要包含少量头文件即可（nanoflann.hpp和nanoflann_utils.h）。

nanoflann实例应用

在source点云中，找出每一个特征点$p_i^{src}$在target点云中的最近点$p_j^{tgt}$。

1. 构建target点云的KDtree。
2. 遍历确定source中每一个点云在target中的最近点。
   额外注意，下面demo代码是没有将src点云坐标转换到tgt时刻的坐标系下再进行最近邻搜索的。实际中在解决运动估计问题时，是需要将src时刻坐标系下的点云利用位姿变化初值（通常是DR值）坐标转换到tgt坐标系后，在同一坐标系下再进行KNN搜索以提高准确性。

以下代码为在解决地面语义slam中，解决的二维问题最近邻搜索数据关联问题。

#include "kd-tree/nanoflann.hpp"
#include "kd-tree/nanoflann_utils.h"

typedef nanoflann::KDTreeSingleIndexAdaptor<
nanoflann::L2_Simple_Adaptor<double, nanoflann::PointCloud<double> > ,
nanoflann::PointCloud<double>,
2 /* dim */
> kd_tree_t;

CloudPoints m_stCloudPoints_src;
CloudPoints m_stCloudPoints_target;
std::unique_ptr<kd_tree_t> m_pKDtree_target;
nanoflann::PointCloud<double> m_KDtree_pts_target;
std::vector<std::pair<int/*target index*/, double/*dist sqr*/>> m_vAssociation;

void BuildTargtKDtree();
void Association();

void BuildTargtKDtree()
{
    // build target cloudpoints kdtree
    size_t N = m_stCloudPoints_target.size();
    m_KDtree_pts_target.pts.clear();
    m_KDtree_pts_target.pts.resize(N);
    for(size_t i = 0; i < N; i++){
        CloudPoint pt = m_stCloudPoints_target.at(i);
        m_KDtree_pts_target.pts.at(i).x = pt.x;
        m_KDtree_pts_target.pts.at(i).y = pt.y;
    }

    m_pKDtree_target.reset(new kd_tree_t(2 /*dim*/, m_KDtree_pts_target, nanoflann::KDTreeSingleIndexAdaptorParams(1 /* max leaf */)));
    m_pKDtree_target->buildIndex();
}

void Association()
{
    m_vAssociation.clear();
    for(size_t i = 0; i < m_stCloudPoints_src.size(); i++)
    {
        CloudPoint pt_src_obs = m_stCloudPoints_src.at(i);
        // do a knn search
        // 通过初始里程计变换将src点云投影到target点云坐标系上
        Eigen::Vector3d pt_src_obs_vector = {pt_src_obs.x, pt_src_obs.y, 0};
        Eigen::Vector3d query_pt = pt_src_obs_vector;  // 运动估计时，要利用初始值进行坐标系统一，此处省略
        size_t num_results = 1;
        std::vector<size_t>   ret_index(num_results); // 搜索到按距离排序的点序号
        std::vector<double>   out_dist_sqr(num_results);  // 搜索到的点平方距离
        num_results = m_pKDtree_target->knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);

        //如果最近点距离大于一定阈值，则不计入关联表，算没有找到，设置为-1
        size_t target_index = ret_index[0];
        double dist_sqr     = out_dist_sqr[0];
        if(dist_sqr > 5+1E-6)
        {
            m_vAssociation.emplace_back(std::make_pair(-1,-1));
            continue;
        }
        m_vAssociation.emplace_back(std::make_pair(target_index,dist_sqr));
    }
}

int main(int argc, char **argv)
{
    BuildTargtKDtree();  // build target cloudpoints kdtree.
    Association();   // do knn search to achieve association between source and target cloudpoints.
}