Scan Context 介绍及理解

Scan Context 由韩国KAIST大学的 Giseop Kim, Ayoung Kim 于2018年发表在IROS(International Conference on Intelligent Robots and Systems)

论文：Kim, Giseop, and Ayoung Kim. "Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map."2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

开源工程：https://github.com/irapkaist/scancontext 目前已经集成在了一些开源工程中

• SC-A-LOAM
• SC-LeGO-LOAM
• SC-LIO-SAM
• FAST_LIO_SLAM
• PyICP-SLAM
• navtech-radar-slam

Scan Context 应用于基于3D点云的重定位和场景识别，主要思想是将场景3维信息压缩，将笛卡尔坐标系的信息转换到极坐标系下计算。

优势是高效利用场景点云分布特征，引入"旋转不变性"描述子，快速搜索。

1.主要流程：

读取单帧3D点云数据，建立Scan-Context，在Mapping过程中生成KeyFrame中查找，使用Ring-key在KD-tree下查找最近邻结果，结果计算统计得分，得到最佳匹配，完成回环检测。

2.Scan-Context描述子

Scan-Context描述子如图所示，

• Ring为黄色圆环，
• Sector为青色部分，

SectorRing类似于极坐标中的角度，Sector类似于极坐标中的长度，3D物理空间被Ring和Sector划分成2D的“区块”空间，每个区块由Ring和Sector唯一确定。同时，将所需相关信息存放在每个区块中，比如：区块中的点云数量、区块中的点云max height、区块中的点云mean height等；

Scan-Context的表示为Ring-Sector矩阵，如图所示：Sector的数量为60，表示0~360°，每个Sector的分辨率为6°；Ring的数量为20，表示距离$0 - L_{max}$；

图中(a)和(b)为同一场景下的两帧数据所形成的Scan-Context，可以看出，两个矩阵图像除去水平方向的位移偏差外，是高度相似的。这就是刚刚提到的“旋转不变性”；

也就是说，即便是每次采集数据的角度偏差很多，采集的点云数据也可以通过本描述子找到对应的场景。直观能想到的，只要每次将其中的一个矩阵图像水平移动一个单位，然后计算两个矩阵的相似度，最终一定能找到对应的结果；

抛开计算效率，先来看一下计算相似度的计算公式：

$d(I^{q},I^{c}) = \frac{1}{N_s}\sum_{j=1}^{N_s}{(1-\frac{c^{q}_j \cdot c^{c}_j}{\parallel c^{q}_j \parallel \parallel c^{c}_j\parallel})} (1)$

$I^{q},I^{c}$ 分别是两个Scan-Context，相似度是每一列的对比之和，余弦距离(cosine distance)作为计算其中列向量$c^{q}_j , c^{c}_j$的方法 (注意：相似度越高与余弦距离越小)，$N_s $是列数，也就是一个Scan-Conetext中Sector的数量；将$I^{c}$ 沿列方向平移n个单位为 $I^{c}_n$ , 那么可以遍历移动这个列向量$N_s$ 次，得到每次移动后相似度的结果，取最小值为$I^{q},I^{c}$ 的相似度:

$D(I^{q},I^{c})= \min_{n\in[N_s]}d(I^{q}, I^c_{n}) (2)$

$n^*=\mathop{\arg\min}_{n\in [N_s]}{d(I^q,I^c_n) } (3)$

从理论上，使用当前帧Scan-Context遍历历史所有Scan-Context一定能找到对应的结果，即相似度的全局极值。但从效率上，这并不可取，我们还需要更快的方式。于是引入了Ring-Key。

3.Ring-Key描述子

Ring-Key是一种旋转不变描述子，具体表示为一位数组kkk ， 数组中每个元素$\psi(r_i)$ 为第$i$个Ring的编码值，$r_i$ 的排布为距离原点从近到远；

$k=(\psi(r_1),...,\psi(r_{N_r})), where \ \psi : r_i\rightarrow \mathbb{R } (4)$

$\psi(r_i)=\frac{|| r_i ||_0}{N_s} (5)$

为$\psi $的表达式，其中$|| r_i ||_0$表示$r_i $中的非零数量；

Ring-Key相当于对数据进行了压缩和降维，而由于Ring-Key是旋转不变描述子，所以两个Scan-Context做匹配的时候可以直接对比Ring-Key的相似度，而不用直接使用(2)的方式，至此有了高效的匹配方式。

4.加速匹配

论文中使用Ring-Key构建kd-tree来查找相似候选帧，再进行匹配计算；

在开源代码中，其实还使用到了与Ring-Key类似的Sector-Key的概念，同样起到了数据降维和搜索加速的作用。

5.代码

不同工程中的Scan-Context的具体细节不同，我们以SC-LeGO-LOAM嵌入的Scan-Context为例；

5.1.Scan-Context的生成函数：

MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down ) 

5.1.1.其中Scan-Context的数据结构：

 // main 

 const int NO_POINT = -1000;
 MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); 

5.1.2.使用雷达安装高度外参，将点云移动至z方向大于零的区域，同时将直角坐标系转换为极坐标系：

 pt.x = _scan_down.points[pt_idx].x; 
  pt.y = _scan_down.points[pt_idx].y;
  pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). 

  // xyz to ring, sector 

  azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
  azim_angle = xy2theta(pt.x, pt.y); 

5.1.3.矩阵中的每个元素为z的最大值：

// taking maximum z 

if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 

     desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin 

5.2.Ring-Key的生成函数，提取的是行向量的均值：

MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    /* 
 * summary: rowwise mean vector */
    Eigen::MatrixXd invariant_key(_desc.rows(), 1);
    for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
    {
        Eigen::MatrixXd curr_row = _desc.row(row_idx);
        invariant_key(row_idx, 0) = curr_row.mean();
    }

    return invariant_key;
} 

5.3.Sector-Key的生成函数，提取的是列向量的均值：

MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    /* 
 * summary: columnwise mean vector */
    Eigen::MatrixXd variant_key(1, _desc.cols());
    for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
    {
        Eigen::MatrixXd curr_col = _desc.col(col_idx);
        variant_key(0, col_idx) = curr_col.mean();
    }

    return variant_key;
} 

5.4.计算两个Scan-Context的距离(相似度)的函数：

使用Sector-Key进行加速计算，此处与论文中的不同;

std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
{
    // 1. fast align using variant key (not in original IROS18) 

    MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
    MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
    int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );

    const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 

    std::vector<int> shift_idx_search_space { argmin_vkey_shift };
    for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
    {
        shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
        shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
    }
    std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());

    // 2. fast columnwise diff 

    int argmin_shift = 0;
    double min_sc_dist = 10000000;
    for ( int num_shift: shift_idx_search_space )
    {
        MatrixXd sc2_shifted = circshift(_sc2, num_shift);
        double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
        if( cur_sc_dist < min_sc_dist )
        {
            argmin_shift = num_shift;
            min_sc_dist = cur_sc_dist;
        }
    }

    return make_pair(min_sc_dist, argmin_shift);

} 

5.5.回环检测主函数：

std::pair<int, float> SCManager::detectLoopClosureID ( void )

5.5.1.使用kd-tree找候选Scan-Context：

nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 

5.5.2.计算候选Scan-Context的距离(相似度)，从中找到距离最近(相似度最高)的那个：

for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
    {
        MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
        std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 
        
        double candidate_dist = sc_dist_result.first;
        int candidate_align = sc_dist_result.second;

        if( candidate_dist < min_dist )
        {
            min_dist = candidate_dist;
            nn_align = candidate_align;

            nn_idx = candidate_indexes[candidate_iter_idx];
        }
    } 

5.5.3.回环判断，根据阈值判断是否检测到回环：

/* 
 * loop threshold check */
if( min_dist < SC_DIST_THRES )
{
     loop_id = nn_idx; 
    
     // std::cout.precision(3); 

     cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
     cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
}
else
{
     std::cout.precision(3); 
     cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
     cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
} 

6.总结

Scan-Context是一种基于极坐标系的3D点云描述子和匹配方法，可快速实现场景重识别，应用于回环检测和重定位。算法层面的实现样式较多，针对不同场合和不同数据可以进行改进。本人结合SLAM算法自测过KITTI数据集、4D毫米波雷达点云数据、固态激光点云数据，都获得了良好的效果。当然，没有完美的算法，Scan-Context也存在其使用局限性，相信不少使用过的同学都有所体会，也能想到改进的方案。

其它相关扩展论文：

[1] Wang H , Wang C , Xie L . Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection[J]. 2020. 来自：Scan Context 介绍及理解