3D激光回环检测系列之SC描述子

概要

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 主要实现两部分功能, 如下图

在这里插入图片描述
其中代码中比较关键的部分总结如下:

  1. 描述子生成
    对每一个关键帧都进行描述子的提取
    如下:
/**
 * @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描述子的每一列求均值得到.
主要在求解旋转方向时用到.

  1. 相似性检测

闭环检测是在 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

已标记关键词 清除标记
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页