利用KDTreel计算模型点云和场景点云的匹配点对

利用 KdTree 进行模型点云和场景点云的匹配可以分为以下几步：

1. 对模型点云中的每个点，利用 KdTree 搜索场景点云中距离其最近的 k 个点（一般取 k=1）。

2. 对于每个模型点云中的点，找到距离其最近的场景点云中的点，计算它们之间的距离，如果距离小于一个设定的阈值，则将它们视为匹配点对。

3. 将所有匹配点对存储起来，可以用于后续的位姿估计和姿态估计。

下面是一段 C++ 代码示例，用于计算模型点云和场景点云之间的匹配点对：

pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud(model_descriptors);

std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);

for (int i = 0; i < scene_descriptors->size(); ++i) {
    if (match_search.nearestKSearch(*scene_descriptors, i, 1, neigh_indices, neigh_sqr_dists) > 0 && neigh_sqr_dists[0] < threshold) {
        pcl::Correspondence corr(neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
        model_scene_corrs->push_back(corr);
    }
}

其中，model_descriptors 和 scene_descriptors 分别表示模型点云和场景点云的描述子。threshold 表示设定的距离阈值，neigh_indices 和 neigh_sqr_dists 分别表示最近邻点的索引和距离平方。利用 nearestKSearch 函数可以搜索到场景点云中距离该点最近的点，并返回最近邻点的索引和距离平方。如果距离平方小于阈值，则将它们视为匹配点对，并将其存储到 model_scene_corrs 中。