概要
sc描述子是一种经典的全局描述子方法, 对于较稀疏的3D激光点云, 没有足够的数据去提取局部描述子, 因此, 提取全局描述子较为合适, 下面主要学习SC-legoloam的回环检测方法.
SC-legoloam回环解析
sc描述子
在sc-legoloam中, 关于sc描述子的代码文件是:
src/Scancontext.cpp: sc描述子闭环检测的核心代码
include/Scancontext.h: 头文件
kdtree相关
include/KDTreeVectorOfVectorsAdaptor.h
include/nanoflann.hpp
核心代码就在 src/Scancontext.cpp 中, Scancontext.cpp 主要实现两部分功能, 如下图
其中代码中比较关键的部分总结如下:
- 描述子生成
对每一个关键帧都进行描述子的提取
如下:
/**
* @brief 每一个帧都用这个函数生成全局描述子
* @param[in] _scan_down 输入点云 SCPointType = pcl::PointXYZI
* @details 提取sc描述子的全部流程
**/
void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 提取sc全局特征描述符
Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); // 求 ring key 特征
Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); // 提取sector key 与论文不同 粗略确定平移范围
std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
polarcontexts_.push_back( sc ); // 当前帧点云检测完毕后 SC描述子 存放于 polarcontexts_
polarcontext_invkeys_.push_back( ringkey ); // 保存ringkey
polarcontext_vkeys_.push_back( sectorkey ); // 保存sectorkey
polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); // 保存 vector类型的ringkey
} // SCManager::makeAndSaveScancontextAndKeys
该函数计算了3种描述子:
sc描述子: 一个二维的矩阵, 初始化为:
MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
即 PC_NUM_RING 行, PC_NUM_SECTOR 列.
如下图
矩阵每个元素的取值为对应(ring, sector) 区域点云的特征, 论文中直接用该区域的点云高度的最大值作为特征, 也可以用平均高度或者其他计算方式来作为特征.
ringkey描述子
一个列向量:
Eigen::MatrixXd invariant_key(_desc.rows(), 1); // 一个列向量 列数为ring 的个数
直接对sc描述子的每一行求均值, 作用是 提供旋转不变性.
sectorkey描述子
一个行向量, 通过对sc描述子的每一列求均值得到.
主要在求解旋转方向时用到.
- 相似性检测
闭环检测是在 std::pair<int, float> SCManager::detectLoopClosureID ( void ) 函数中实现的:
整体的流程如下:
1 首先将最新的关键帧的sc描述子与ringkey取出来, 进行检测.
// 首先将最新的帧的sc描述子提取出来 进行回环检测
auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) 提取最新一帧ring-key
auto curr_desc = polarcontexts_.back(); // current observation (query) 提取最新的sc描述子
2 将ringkey 构建kdtree
// tree_ reconstruction (not mandatory to make everytime) 使用kdtree对ring key进行查找
// 把历史关键帧的ringkey构造kdtree
if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost 频率控制
{
TicToc t_tree_construction;
// std::vector<std::vector<float> > 类型
polarcontext_invkeys_to_search_.clear();
// 构造用于搜索的ringkey 集合 assign() 将区间[first,last)的元素赋值到当前的vector容器中 这里减去 NUM_EXCLUDE_RECENT 也就是 不考虑最近的若干帧
polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
// KDTreeVectorOfVectorsAdaptor<>的 unique_ptr
polarcontext_tree_.reset();
// TODO: 构建kdtree的细节 ?????????????????????????????????????????
polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
// tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
t_tree_construction.toc("Tree construction");
}
3 在kdtree中搜索与当前闭环检测帧的ringkey距离最近的若干帧
// knn search NUM_CANDIDATES_FROM_TREE = 10 , 10个候选关键帧
std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE );
std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); // 保存候选关键帧的ringkey的距离
TicToc t_tree_search;
// 找 NUM_CANDIDATES_FROM_TREE 个最近关键帧
nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
// 初始化 用 candidate_indexes 和 out_dists_sqr 数组的数组名地址初始化 设置搜索结果存放的数组
knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
// 查找与当前待搜索的ringkey 距离最近的10帧
polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) );
t_tree_search.toc("Tree search");
搜索结果保存在 knnsearch_result 中.
4
2696
