Scan Context / Scan Context ++ 论文和源码阅读

Title: Scan Context / Scan Context ++ 论文和源码阅读

---

前言

Scan Context/Scan Context ++ 论文^{[1], [2]}和源码^[3]理解起来并不难, 但还是记录一下以备忘.

---

I. Scan Context 的构造

A. 论文内容

Scan Context 用于激光点云的回环检测或者重访识别. 该方法从特殊的空间分割与点云处理开始:

以机器人/激光雷达为中心, 将周围区域以极坐标形式划分为径向 $N_r=20$ 等分和周向 $N_s=60$ 等分, 并限制传感器的在径向的最大有效扫描范围为 $L_{\max }$.

这样划分形成了一个一个区域称为 bin (水桶), 属于径向第 $i\in \{1,2,\dots ,N_r\}$ 圈和周向第 $j\in \{1,2,\dots ,N_s\}$ 扇区的 Bin 的指标为 $\{ij\}$. 每个有效扫描点属于一个 bin. 落到 bin $\{ij\}$ 中扫描点集合记为 ${\mathcal{P}}_{ij}$. 每个水桶都映射为一个值, 记为 $\varphi ({\mathcal{P}}_{ij})$, 为所有落入该水桶中点的最高值 (pt.z 最大值).

根据这种空间划分和映射关系, 由每帧扫描点云就产生了 $N_r\times N_s$ 矩阵, 这个矩阵就是这一帧扫描对应的 Scan Context (下面简记为 SC).

本质上, 这个 Bin (水桶) 是对稠密点云的粗颗粒化, 为一个降采样过程.

---

B. 源码内容

“cpp/module/Scancontext/Scancontext.cpp” 中 C++ 源码如下^[3]. 注意和论文中理论描述不同, 程序中编号是从 0 开始的.

MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
    TicToc t_making_desc;

    int num_pts_scan_down = _scan_down.points.size();

    // main
    const int NO_POINT = -1000;
    MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);  // 初始化 水桶 Bin

    SCPointType pt;
    float azim_angle, azim_range; // wihtin 2d plane
    int ring_idx, sctor_idx;
    for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
    {
        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).
		// 保证 pt.z > 0, 也就是以地平面为 0
        
        // xyz to ring, sector
        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);   // 扫描点的径向坐标
        azim_angle = xy2theta(pt.x, pt.y);              // 扫描点的周向坐标

        // if range is out of roi, pass
        if( azim_range > PC_MAX_RADIUS )    // 判断是否有效扫描点
            continue;

        // 确定点所落水桶的极坐标指标 (径向指标和周向指标)
        ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );   // 点落到某水桶中, 对应水桶的径向指标
        sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );      // 点落到某水桶中, 对应水桶的周向指标

        // 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
    }

    // reset no points to zero (for cosine dist later)
    for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
        for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
            if( desc(row_idx, col_idx) == NO_POINT )   // 没有点落进来的水桶的值保持为初始化值, 需设为 0
                desc(row_idx, col_idx) = 0;

    t_making_desc.toc("PolarContext making");

    return desc;
} // SCManager::makeScancontext

---

2. Ring Key/Retrieval Key 的计算

根据上述 Scan Context 的定义, 就可以构建历史扫描点云的 SC 以及当前扫描点云的 SC. 要知道是否重新到访了历史到访过的地点 (loop closure), 就要将当前 SC 和历史上的各个 SC 进行比较. 但是这就成暴力搜索了, 随着历史扫描的累计, 就非常耗计算资源而且慢.

原论文中就采用了两阶段启发式搜索方法来减小计算量. 其中第一段就是构建更浓缩的 Ring Key/Retrieval Key 键值来进行粗略的搜索与定位.

---

A. Ring Key

在论文 “Scan Context”^[1] 中称为 Ring Key. 已知 Scan Context 构建的是划分了径向 $N_r$ 圈, 每圈又可以细分为周向分布的 $N_s$ 个水桶 (Bins), 每个水桶对应一个编码值. Ring Key 试图把每一圈压缩为一个元素. 极坐标每圈上的 $N_s$ 个编码值组成了 SC 矩阵的行向量, 用该行向量 $L0$ 范数作为 Ring Key 的一个元素 ($L0$ 范数就是向量中非零元素的数量). 一帧扫描数据划分为 $N_r$ 圈, 这样每帧扫描对应一个 $N_r$ 维的向量 Ring Key. 由原来 SC 矩阵的搜索匹配降维到向量 Ring Key 的搜索匹配, 计算量就大大降低了.

如 “fast_evaluator/src/ringkey.m” 中 Matlab 源码:

function [ ring_key ] = ringkey(sc)

num_rings = size(sc, 1);

ring_key = zeros(1, num_rings);
for ith=1:num_rings   %%% 每一行即极坐标中的每一圈 ring
    ith_ring = sc(ith,:);
%     ring_key(ith) = mean(ith_ring);
    ring_key(ith) = nnz(ith_ring);
end

end

---

B. Retrieval Key

对应于 Ring Key, 在论文 “Scan Context ++”^[2] 中称为 Retrieval Key. 其实是类似东西, 只是改成了 $L1$ 范数除以 $N_s$. 因为每个 bin 的编码值都大于 0 的缘故, 源码中直接算了每一圈各个编码值的平均值作为这个 Retrieval Key 元素.

“cpp/module/Scancontext/Scancontext.cpp” 中 C++ 源码:

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);  // 每一行就对应极坐标中的每一圈 ring
        invariant_key(row_idx, 0) = curr_row.mean();
    }

    return invariant_key;
} // SCManager::makeRingkeyFromScancontext

---

3. KD 树搜索 —— 第一阶段搜索

A. KD 树构建

以 Ring Key/Retrieval Key 为元素构建 KD 树. 这里最主要的考虑是排除时间上最新的历史帧, 因为当前位置的重新到访是需要隔一段时间的. 源码中对应为 keyframe gap 的设定 (NUM_EXCLUDE_RECENT = 50).

“cpp/module/Scancontext/Scancontext.cpp” 中 C++ 源码:

    /* 
     * step 1: candidates from ringkey tree_
     */
    if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)  
    // 事实上 polarcontext_invkeys_mat_ 是按照一帧一帧扫描的先后顺序 push_back 的
    // 为避免把刚刚扫描的地方当作回环, 需要忽略紧挨着当前帧的历史扫描帧, 通过 keyframe gap (NUM_EXCLUDE_RECENT 帧)实现
    // 如果历史扫描帧数小于这个 keyframe gap, 就意味刚启动呢, 不用考虑回环，直接返回结果
    {
        std::pair<int, float> result {loop_id, 0.0};
        return result; // Early return 
    }

    // tree_ reconstruction (not mandatory to make everytime)  // KD 树的构造
    if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost  // KD 树构造频率, 每隔 TREE_MAKING_PERIOD_ 扫描周期才重构一次 KD 树
    {
        TicToc t_tree_construction;

        polarcontext_invkeys_to_search_.clear();
        polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
        // 将所有历史帧的 Ring Key 导入用于构建 KD 树
        // 但是紧挨着当前帧之前的 NUM_EXCLUDE_RECENT 帧需要排除在外, 因为时间上太近了, 不用考虑如此短时间内的回环, 所以不用放到 KD 树中去

        polarcontext_tree_.reset(); 
        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)
        // std::make_unique 模板函数, 用于动态分配指定类型的内存, 并返回一个指向分配内存的唯一指针 (std::unique_ptr )
        // 构造了 InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >, 并返回指向其的指针
        // 调用完毕接口函数后, KD 树的具体构造执行就交给外部库 nanoflann 处理了 
        t_tree_construction.toc("Tree construction");
    }
    tree_making_period_conter = tree_making_period_conter + 1;

---

B. KD 树搜索

接着, 可以基于构造的 KD 树利用外部库 nanoflann 进行最近领搜索了.

利用 ring key 找到和当前扫描帧最接近的历史扫描帧若干, 作为候选帧.

程序中设定为共 NUM_CANDIDATES_FROM_TREE = 10 个最近邻候选帧.

    double min_dist = 10000000; // init with somthing large
    int nn_align = 0;
    int nn_idx = 0;

    // knn search
    std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );

    TicToc t_tree_search;
    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) ); 
	// nanoflann 进行 KD 树搜索
    t_tree_search.toc("Tree search");

---

4. Scan Context 距离搜索 —— 第二阶段搜索

A. 帧间距离搜索

第二阶段搜索为精准搜索匹配, 从已由 Ring key 粗搜索得到的 NUM_CANDIDATES_FROM_TREE 个候选历史扫描中, 通过 SC 精确距离计算找到和当前帧最近邻的历史扫描帧, 从而判断回环或重访问.

而这个 SC 精确距离搜索又细分为两小步, 具体思路如下.

考虑到同一个地点的重访, 可能扫描时机器人的航向角度改变了 (甚至逆向 180° 重新到访历史位置). 航向角度的改变在 SC 矩阵中的体现为列向量的偏移 (以 360° 为模). 所以 Scan Context 方法为了实现角度不变, 在计算精确计算 SC 距离之前,

- 先通过对候选扫描帧进行角度偏移 (或列偏移), 找出每一个候选扫描帧的粗偏移匹配

- 再以粗偏移匹配为中心向两边扩展角度偏移作为精确搜索的偏移范围, 进行计算获得查询帧 (当前帧) 与候选历史帧之间的精确距离

角度偏移搜索用数学公式描述比语言描述更直达本意
$$\begin{array}{rl}D(I^q,I^c)& =\underset{n\in [N_s]}{\min }d(I^q,I_n^c)\\ n^{\ast }& =\underset{n\in [N_s]}{\mathrm{argmin}}d(I^q,I_n^c)\end{array}\text{\hspace{1em}(1)}$$
其中

$I^q$ 是查询帧 (当前帧) 的 SC 或者 Sector key (下面描述);

$I^c$ 是历史帧 (候选帧) 的 SC 或者 Sector key;

$I_n^c$ 是历史帧的 SC 或者 Sector key 以列向量切换 $n$ 列后的值;

$[N_s]$ 可以取值 $\{1,2,\dots ,N_s\}$.

无论候选帧进行角度切换或者查询帧进行角度切换都是一样的, 因为角度的相对关系.

角度偏移/列偏移像钟表一样进行, 模为 360°.

重访时除了有角度的偏移, 也有横向位置的偏移. 论文对将横向偏移的处理命名为 root shifting, 将每一帧的扫描数据向两边偏移并拷贝, 再以当前位置及偏移拷贝的点云来构造 SC, 以此来模拟合成机器人以不同车道线到访扫描同一位置. 虽然作者假设了横向偏移不大幅改变扫描点云, 给我的感觉是有点牵强, 当然如果实践中好用的话就自有其道理 (关于横向偏移没找到具体源码).

---

B. 角度偏移粗搜索

对照 Ring key, 我们对 SC 按列压缩得到 Sector Key, 以 SC 中每列的均值代替列向量. 源码如下

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(); //以 SC 中每列的均值代替列向量
    }

    return variant_key;
} // SCManager::makeSectorkeyFromScancontext

角度偏移/列偏移的实现源码:

MatrixXd circshift( MatrixXd &_mat, int _num_shift )
{
    // shift columns to right direction 
    assert(_num_shift >= 0);

    if( _num_shift == 0 )
    {
        MatrixXd shifted_mat( _mat );  // 列偏移 0, 就是不变
        return shifted_mat; // Early return 
    }

    MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() );
    for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
    {
        int new_location = (col_idx + _num_shift) % _mat.cols();  // 列指标的偏移
        shifted_mat.col(new_location) = _mat.col(col_idx);
    }

    return shifted_mat;

} // circshift

压缩 SC 的列向信息得到 Sector Key 是为了角度偏移所搜的快速粗略计算. 利用 Sector key 的角度偏移粗搜索匹配的源码:

int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
{
    int argmin_vkey_shift = 0;
    double min_veky_diff_norm = 10000000;
    for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
    {
        MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);  // 角度偏移/列偏移

        MatrixXd vkey_diff = _vkey1 - vkey2_shifted;  // 比较 sector key 的距离

        double cur_diff_norm = vkey_diff.norm();
        if( cur_diff_norm < min_veky_diff_norm )  // 找出同一帧不同角度偏移后的 sector key 与查询帧之间的最小距离
        {
            argmin_vkey_shift = shift_idx;
            min_veky_diff_norm = cur_diff_norm;
        }
    }

    return argmin_vkey_shift;  // 最优的角度偏移

} // fastAlignUsingVkey

---

C. 位置重访精搜索

以粗偏移匹配为中心向两边扩展角度偏移量, 作为即将进行的位置重访精准搜索的角度偏移/列偏移的范围, 源码如下:

    // 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());  // 偏移指标范围排序

在做距离搜索前, 先看一下两 SC 之间距离的精准计算公式
$$\begin{array}{rl}d(I^q,I^c)& =\frac{1}{N_s}\sum _{j=1}^{N_s}\left(1-\frac{c_j^q\cdot c_j^c}{\Vert c_j^q\Vert \Vert c_j^c\Vert }\right)\\ & =1-\frac{1}{N_s}\sum _{j=1}^{N_s}\frac{c_j^q\cdot c_j^c}{\Vert c_j^q\Vert \Vert c_j^c\Vert }\end{array}\text{\hspace{1em}(2)}$$
以及实现代码:

double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 )
{
    int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
    double sum_sector_similarity = 0;
    for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
    {
        VectorXd col_sc1 = _sc1.col(col_idx);
        VectorXd col_sc2 = _sc2.col(col_idx);
        
        if( col_sc1.norm() == 0 | col_sc2.norm() == 0 )
            continue; // don't count this sector pair. 

        double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());

        sum_sector_similarity = sum_sector_similarity + sector_similarity;
        num_eff_cols = num_eff_cols + 1;
    }
    
    double sc_sim = sum_sector_similarity / num_eff_cols;
    return 1.0 - sc_sim;

} // distDirectSC

针对一个候选帧计算与查询帧之间的距离计算, 需对候选帧进行角度偏移后进行精准计算匹配, 其中角度偏移的范围以粗偏移匹配为中心向两边扩展得到.

查询帧与候选帧之间的距离为查询帧与该候选帧的角度切换 (偏移) 后的各个数据帧之间的距离值的最小值, 如式 (1) 数学描述.

    // 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);

完成所有候选帧与查询帧之间距离计算后, 再根据查询帧和不同候选帧之间的距离, 从中选出最短的距离.

该最短距离对应的候选帧就与当前帧形成了潜在的回环或位置重访.

最后再做最近邻搜索的门限判定, 如果符合要求就得到了回访地点或者回环.

数学上面表达为
$$\begin{gathered} c^{\ast }=\underset{c_k\in \mathcal{C}}{\mathrm{argmin}}D(I^q,I^{c_k})=\underset{c_k\in \mathcal{C}}{\mathrm{argmin}}\left(\underset{n\in [N_s]}{\min }d(I^q,I_n^c)\right) \\ \text{s.t.}D<\tau \text{\hspace{1em}(3)} \end{gathered}$$
其中

$\mathcal{C}$ 是候选帧的指标;

$c^{\ast }$ 是与查询帧最近邻的候选帧的指标;

$\tau$ 最近邻搜索距离的门限 (源码中设为 SC_DIST_THRES = 0.13).

以上计算的源代码:

    /* 
     *  step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
     */
    TicToc t_calc_dist;   
    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];  // 候选帧的指标
        }
    }
    t_calc_dist.toc("Distance calc");

    /* 
     * 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;
    }

    // To do: return also nn_align (i.e., yaw diff)
    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);   // 计算偏移的角度
    std::pair<int, float> result {loop_id, yaw_diff_rad};

    return result;

---

总结和讨论

以上为对 Scan Context 内容的整理笔记.

论文提到了部分点云特征描述子需要依赖法线, 而自动驾驶环境比较杂乱获得稳定地法线比较困难;

论文也提到了点云稀疏程度和扫描距离有关, 而极坐标形式能够很好的兼容这种问题.

认可和赞赏论文的这些描述并得到了较好的结果.

但是框架上或者思路上 Scan Context 和普通的点云特征描述子并无区别, 都是在人为构造适合的特征描述形式.

所以总体上感觉还是将传统点云特征描述子应用于大场景.

当然要有真正的改变应该要用深度学习了.

(不知道是否正确, 如有问题请指出)

---

参考文献

[1] G. Kim and A. 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), Madrid, Spain, 2018, pp. 4802-4809

[2] G. Kim, S. Choi and A. Kim, “Scan Context++: Structural Place Recognition Robust to Rotation and Lateral Variations in Urban Environments,” in IEEE Transactions on Robotics, vol. 38, no. 3, pp. 1856-1874, June 2022

[3] Scan Context, https://github.com/irapkaist/scancontext