一家之言,仅作分享,如有不合理或需要改进的地方,欢迎各位讨论。
在ICP求解运动估计问题中,首先要解决的就是特征关联问题,一般都是利用KD树进行最近邻(NN)搜索,即在点云集A中找出其中每个点在点云集B中的距离最近的点,以实现数据关联。
与PCL中的KD树最近邻搜索相比,更推荐nanoflann方法,后者运行速度更快,且只需要包含少量头文件即可(nanoflann.hpp和nanoflann_utils.h)。
nanoflann实例应用
在source点云中,找出每一个特征点 p i s r c p^{src}_i pisrc在target点云中的最近点 p j t g t p^{tgt}_j pjtgt。
- 构建target点云的KDtree。
- 遍历确定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.
}