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
目前已经集成在了一些开源工程中
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是一种旋转不变描述子，具体表示为一位数组 k ， 数组中每个元素 \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)
(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.
