简介
Scan Context 的原始 Repo 为：Scan Context。作者以 LeGO-LOAM 作为一个例子集成使用。所以这里以 SC-LeGO-LOAM 为例整理以下 Scan Context 的实现以及使用流程。注释版代码见：SC-LeGO-LOAM-w-Comment。这里不会涉及太多关于 LeGO-LOAM 本身的说明，关于 LeGO-LOAM 的注释参考：LEGO-LOAM With Chinese Comments
使用 Scan Context 一般需要引入以下文件：
SLAM Project
└── include
├── KDTreeVectorOfVectorsAdaptor.h-------第三方文件，实现一个基于 vector<vector<double>> 的 KD 树接口，vector 元素比较的距离定义为 L2 距离
├── nanoflann.hpp------------------------第三方文件，实现 FLANN 算法，参考：[nanoflann](https://github.com/jlblancoc/nanoflann)
├── tictoc.h-----------------------------一个简单的计时类，非 SC 算法必须
└── Scancontext.h------------------------SCManager 类声明，外部使用 Scan Context 进行回环检测的主要接口
└── src
└── Scancontext.cpp----------------------SCManager 实现
SCManager 使用
首先看一下 SCManager 对外部开放的接口部分：
class SCManager
{
public:
...
// User-side API
///@brief 对传入的点云(Lidar 坐标系下)计算 Scan Context 并保存用于后续匹配
void makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down );
///@brief 使用最新一帧点云的 SC 在历史帧中需要匹配帧
///@return 返回找到的历史匹配帧和对应的水平旋转角估计
std::pair<int, float> detectLoopClosureID( void ); // int: nearest node index, float: relative yaw
public:
// hyper parameters ()
const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);
// tree
const int NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok.
const int NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper)
// loop thres
const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
// const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
// config
const int TREE_MAKING_PERIOD_ = 10; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N.
int tree_making_period_conter = 0;
// data
...
}; // SCManager
可以看出，SCManager 对开放的使用接口实际上只有两个：
makeAndSaveScancontextAndKeys：对点云计算对应的 Scan Context 并进行保存detectLoopClosureID：在已经保存的所有 SC 进行回环检测
下面来看看 LeGO-LOAM 具体怎么使用 SCManager：
对新的关键帧进行 SC 计算并保存
LeGO-LOAM 将关键帧点云传入 SCManager 中保存相关的代码如下。这里作者采用两个思路：
- 只取平面特征(点云中去除角点之后的大部分点) 构建 SC，这里将角点去除的用意主要是考虑角点对 SC 的贡献不大，从鸟瞰图来看的话，一根边缘线（Edge）上的所有点都会投影到一个 bin 上（即 SC 图像中的一个像素），因此实际上会存在很多重复计算
- 取原始点云并进行下采样然后构建 SC
这里注意由于 SCManager 内部会维护一个独立容器用于存储所有关键帧的 SC，但这个容器和外部关键帧存储容器（这里是 mapOptimization 的容器）没有机制保证对齐，因此在将关键帧加入 SCManager 之后，在这一个关键帧之前的所有关键关键顺序（和索引）都不能改变。
void mapOptimization::saveKeyFramesAndFactor(){
...
/*
Scan Context loop detector -- SC 回环检测两种策略，使用下采样后的原始点云构建 SC，或者只使用平面点
- ver 1: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.)
- ver 2: using downsampled original point cloud (/full_cloud_projected + downsampling)
*/
bool usingRawCloud = true;
if( usingRawCloud ) { // v2 uses downsampled raw point cloud, more fruitful height information than using feature points (v1)
pcl::PointCloud<PointType>::Ptr thisRawCloudKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudRawDS, *thisRawCloudKeyFrame);
scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
}
else { // v1 uses thisSurfKeyFrame, it also works. (empirically checked at Mulran dataset sequences)
scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame);
}
...
} // saveKeyFramesAndFactor
基于 SC 进行回环检测构建回环因子
这一部分主要分成两步：
- 使用
SCManager进行回环检测，获得和当前帧构成回环的匹配帧 - 在匹配帧附近构建局部点云，进行 ICP 匹配获得相对位姿变化
代码如下：
bool mapOptimization::detectLoopClosure(){
...
/*
* 2. Scan context-based global localization -- 基于 Scan Context 的全局定位
*/
SClatestSurfKeyFrameCloud->clear();
SCnearHistorySurfKeyFrameCloud->clear();
SCnearHistorySurfKeyFrameCloudDS->clear();
// std::lock_guard<std::mutex> lock(mtx);
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
SCclosestHistoryFrameID = -1; // init with -1
// 使用最新一帧数据在历史帧中找相似帧，返回该相似帧的索引和偏航角差
auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff
SCclosestHistoryFrameID = detectResult.first;
yawDiffRad = detectResult.second; // not use for v1 (because pcl icp withi initial somthing wrong...)
// if all close, reject
if (SCclosestHistoryFrameID == -1){
return false;
}
// save latest key frames: query ptcloud (corner points + surface points)
// NOTE: using "closestHistoryFrameID" to make same root of submap points to get a direct relative between the query point cloud (latestSurfKeyFrameCloud) and the map (nearHistorySurfKeyFrameCloud). by giseop
// i.e., set the query point cloud within mapside's local coordinate
// 以最近历史帧的位姿将当前帧点云（包括角点和平面点）投影至地图坐标系下，这样通过 ICP 得到的相对位姿变换不会将里程计的结果考虑在内，得到是当前帧点云和该地图的直接匹配关系
*SClatestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[SCclosestHistoryFrameID]);
*SClatestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[SCclosestHistoryFrameID]);
pcl::PointCloud<PointType>::Ptr SChahaCloud(new pcl::PointCloud<PointType>());
int cloudSize = SClatestSurfKeyFrameCloud->points.size();
for (int i = 0; i < cloudSize; ++i){
// 将索引不合理的点去掉（理论上应该不会有）
if ((int)SClatestSurfKeyFrameCloud->points[i].intensity >= 0){
SChahaCloud->push_back(SClatestSurfKeyFrameCloud->points[i]);
}
}
SClatestSurfKeyFrameCloud->clear();
*SClatestSurfKeyFrameCloud = *SChahaCloud;
// save history near key frames: map ptcloud (icp to query ptcloud) -- 构建找到的回环历史帧附近的局部地图并下采样
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
if (SCclosestHistoryFrameID + j < 0 || SCclosestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
*SCnearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[SCclosestHistoryFrameID+j], &cloudKeyPoses6D->points[SCclosestHistoryFrameID+j]);
*SCnearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[SCclosestHistoryFrameID+j], &cloudKeyPoses6D->points[SCclosestHistoryFrameID+j]);
}
downSizeFilterHistoryKeyFrames.setInputCloud(SCnearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*SCnearHistorySurfKeyFrameCloudDS);
...
return true;
} // detectLoopClosure
这里实际上使用到 SCManager 只有一条语句，即通过 auto detectResult = scManager.detectLoopClosureID(); 获得匹配帧索引和相对偏航角（yaw）估计。后续怎么构造相对位姿约束实际上是按不同情况由用户自行选择的，这里 SC-Lego-LOAM 是将当前帧的点云用匹配帧的位姿投影到世界坐标系下，然后和匹配帧附近的局部世界地图进行 ICP 匹配。这样获得的相对位姿实际上是估计当前帧相对于历史帧的位姿变化，而 LeGO-LOAM 原来的方式是估计两个帧世界位姿的相对变化，实质上应该是等效的。
SCManager 内部实现及注意事项
从第一部分不难看出，使用 SCManager 进行关键帧之间的回环检测很简单。下面来看一下 SCManager 的内部实现。
配置
SCManager 涉及到的配置主要有：
Scan Context 本身配置
LIDAR_HEIGHT = 2.0：Lidar 相对于地面的安装高度PC_MAX_RADIUS = 80：Scan Context 覆盖最远的距离，超过这个距离的点不考虑PC_NUM_RING = 20,PC_NUM_SECTOR = 60：Scan Context 环和扇面的数量，默认情况下，环和扇的分辨率分别为：80 / 20 = 4m,360 / 60 = 6°
回环检测相关配置
NUM_EXCLUDE_RECENT = 50：不考虑最近 N 个关键帧NUM_CANDIDATES_FROM_TREE = 10：KD 树中搜索的候选匹配帧的数量SEARCH_RATIO = 0.1：二次搜索范围，0.1 表示在粗筛之后进行二次搜索范围为360 x 0.1 = 36°SC_DIST_THRES = 0.5：SC 接受匹配阈值
SC 构建
对于每一个点云，构建以下三个变量作为其描述（作用不同）。
sc：完整 Scan Context，为一个 MxN 的二维矩阵，M 为环数，N 为扇数ringkey：对 SC 中每一个环（行）计算平均值，M 个环组成的 Mx1 的列向量。这个主要用于作为 KD 树的键，进行搜索，搜索时两个ringkey的距离定义为 L2 距离，即d(r1, r2) = ||r1 - r2||2，这里作者也把它们称为Invariant Key，因为 SC 不会对其进行垂直旋转（即不考虑载体相对于地面的高度发生变化）sectorkey：对 SC 中每一个扇面（列）计算平均值，N 个扇面组成的 1xN 的行向量。扇键主要用来进行 SC 之间的快速匹配，同时会对其进行不同角度的水平旋转，来获得一个水平旋转角度（yaw）使得两个扇键最小
代码如下：
void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
// 将环键（Eigen::VectorXd）转为 std::vector
std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
// 将当前帧的 SC，环键和扇键保存起来
polarcontexts_.push_back( sc );
polarcontext_invkeys_.push_back( ringkey );
polarcontext_vkeys_.push_back( sectorkey );
polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
// cout <<polarcontext_vkeys_.size() << endl;
} // SCManager::makeAndSaveScancontextAndKeys
回环检测
回环检测相关代码如下所示，大致流程为以最新一帧的 SC 作为目标，在历史帧中寻找匹配帧，整个过程包含 3 次筛选，分别为：
- 第一次筛选：构建以
ringkey为键的 KD 树，搜索出若干个匹配候选帧，匹配距离为ringkey之间的 L2 距离 - 第二次筛选：对所有候选帧，先进行基于
sectorkey的快速匹配：sectorkey为 1 x N 的行向量，将其中一个键进行 a 次旋转（默认情况下以 6° 为分辨率旋转一周 360°），每次旋转之后分别和另一个键进行比较，距离定义同样为 L2 距离。返回取得最小距离的旋转，作为旋转角估计
- 第三次筛选：对于每一对匹配帧，我们估计出一个比较可能的相对水平旋转角，在这个旋转附近再对完整 SC 进行一个小范围内的水平旋转（默认情况下以 6° 为分辨率左右各旋转 18°，总共 36°），对每一次旋转后的 SC 进行 SC 之间的匹配。SC 的距离定义参考论文，大致是每个列向量之间的平均夹角的余弦值作为相似度
- 经过三轮筛选之后，取 SC 距离最小的帧已经对应的水平旋转角进行返回。用户可以使用该水平旋转角作为 ICP 或者其他方法进行位姿估计的初始值。
std::pair<int, float> SCManager::detectLoopClosureID ( void )
{
int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
// 取最新一帧点云的 SC 和环键作为搜索请求
auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
auto curr_desc = polarcontexts_.back(); // current observation (query)
/*
* step 1: candidates from ringkey tree_，不对较近的关键帧搜索（默认设为 50帧）
*/
if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)
{
std::pair<int, float> result {loop_id, 0.0};
return result; // Early return
}
// tree_ reconstruction (not mandatory to make everytime)，按一定频率进行搜索树的重新构建
if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
{
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 ) ;
// 构建基于环键的搜索树（用于进行最近邻搜索）
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)
t_tree_construction.toc("Tree construction");
}
tree_making_period_conter = tree_making_period_conter + 1;
double min_dist = 10000000; // init with somthing large
int nn_align = 0;
int nn_idx = 0;
// knn search -- 进行 K 近邻搜索（默认搜索 10 个近邻作为匹配帧候选）
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) );
t_tree_search.toc("Tree search");
/*
* step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) -- 对所有候选帧，和当前帧的 SC 进行距离比较，选出最相似的帧
*/
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 -- 默认阈值为 0.5，即要求相似度在 60° 以内，不算很严苛
*/
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; // 返回匹配结果（匹配帧的索引，已经对应的偏航角 yaw）
} // SCManager::detectLoopClosureID
注意事项
目前发现的 Scan Context 的注意事项有：
- 传入的点云的坐标系选择：SC-LeGO-LOAM 中选择传原始点云坐标系，并固定 Lidar 相对于地面的高度（将每个点的高度 + Lidar 高度，作为该 bin 的值），这包含了两个假设：
- Lidar 相对于其载体是水平安装的，因此相对地面高度不会变。因此如果 Lidar 不是水平安装的话直接将 Lidar 高度相加可能会有问题，应该根据其安装角度(roll, pitch, yaw) 进行一定旋转，使得整体点云朝向和地面是对齐的
- Lidar 相对于地面的高度不变，这个基本上就是假设 Lidar 按照在移动机器人或者车上，不适用于手持设备和无人机。因此如果场景不一样建议还是根据其位姿转化到世界坐标系下
- Scan Context 只假设了两帧之间的 yaw 可变，roll 和 pitch 不变（或者变化不大），这个假设同样一般也使用于移动小车或者无人车，因此在其他场景下慎用