回环检测是一个相对独立的模块，这里再开一篇专门说明。
前面两篇已经说过，先对点云做了预处理，然后进行连续帧之间的匹配即激光odom，然后是scan-to-map匹配，并保存关键帧的位姿，点云和ScanContext信息，这个过程中是存在这持续的误差积累的，因为没有进行回环检测，而进行回环检测的一个重要信息是ScanContext，这里来说明如何生成ScanContext，怎么检测回环以及如何进行优化。
生成ScanContext的函数其实在上一篇已经提及了，即void mapOptimization::saveKeyFramesAndFactor() 函数中有一个scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
下面主要分析这个函数是如何生成ScanContext，以及ScanContext到底包含了啥？
void SCManager::makeAndSaveScancontextAndKeys(pcl::PointCloud<SCPointType> &_scan_down) {
Eigen::MatrixXd sc = makeScancontext(_scan_down);
Eigen::MatrixXd ringkey = makeRingkeyFromScancontext(sc);
Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext(sc);
std::vector<float> polarcontext_invkey_vec = eig2stdvec(ringkey);
polarcontexts_.push_back(sc);
polarcontext_invkeys_.push_back(ringkey);
polarcontext_vkeys_.push_back(sectorkey);
polarcontext_invkeys_mat_.push_back(polarcontext_invkey_vec);
}
3D点云在xy两个维度上可以以极坐标表示，我们在径向上可以分割可以划分出很多同心圆环ring，在360圆周上进行分割可以划分出很多同心扇形sector，两者交叉形成的就是很多个同心半圆环，每个区域用该区域所有点云高度的最大值来表示，这就是ScanContext，它将3D点云压缩成了二维的矩阵;
makeRingkeyFromScancontext是将每一个完整的ring用一个数字来代替，进而用一个向量来描述整个点云，方法就是，对于一个完整ring上不同sector区域内的值取均值即可;
与此类似makeSectorkeyFromScancontext是想用一个数字来表述整个sector，方法就是将同一个sector内不同ring内的值取均值。
到这里就将一帧完整的关键帧点云，压缩成了ScanContext这种二维矩阵，以及Ringkey和Sectorkey这两种向量。
回环的检测和全局位姿的优化是在mapOptmization的main函数中单独开了一个线程进行的：
std::thread loopthread(&lego_loam::mapOptimization::loopClosureThread, &MO);
看一看具体的代码：
void mapOptimization::loopClosureThread() {
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(1);
while (ros::ok()) {
rate.sleep();
performLoopClosure();
}
}
以一定频率循环调用performLoopClosure，再来看看performLoopClosure：
void mapOptimization::performLoopClosure(void) {
if (cloudKeyPoses3D->points.empty() == true)
return;
if (potentialLoopFlag == false) {
if (detectLoopClosure() == true) {
std::cout << std::endl;
potentialLoopFlag = true;
timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
}
if (potentialLoopFlag == false) {
return;
}
}
potentialLoopFlag = false;
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame;
float noiseScore = 0.5;
gtsam::Vector Vector6(6);
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);
robustNoiseModel = gtsam::noiseModel::Robust::Create(
gtsam::noiseModel::mEstimator::Cauchy::Create(1),
gtsam::noiseModel::Diagonal::Variances(Vector6)
);
bool isValidRSloopFactor = false;
bool isValidSCloopFactor = false;
if (RSclosestHistoryFrameID != -1) {
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
icp.setInputSource(RSlatestSurfKeyFrameCloud);
icp.setInputTarget(RSnearHistorySurfKeyFrameCloudDS);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
std::cout << "[RS] ICP fit score: " << icp.getFitnessScore() << std::endl;
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) {
std::cout << "[RS] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")"
<< std::endl;
isValidRSloopFactor = false;
} else {
std::cout << "[RS] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure
<< " ] and RS nearest [ " << RSclosestHistoryFrameID << " ]" << std::endl;
isValidRSloopFactor = true;
}
if (isValidRSloopFactor == true) {
correctionCameraFrame = icp.getFinalTransformation();
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(
cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
Eigen::Affine3f tCorrect =
correctionLidarFrame * tWrong;
pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[RSclosestHistoryFrameID]);
gtsam::Vector Vector6(6);
std::lock_guard<std::mutex> lock(mtx);
gtSAMgraph.add(
BetweenFactor<Pose3>(latestFrameIDLoopCloure, RSclosestHistoryFrameID, poseFrom.between(poseTo),
robustNoiseModel));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);
}
}
aLoopIsClosed = true;
}
这个函数比较长，先说一下总体思路再详细看：
**首先detectLoopClosure() 找到回环信息，并保存在成员变量中，然后根据形成回环的帧的点云进行匹配建立当前最新关键帧与历史关键帧之间的约束，添加到gtsam进行图优化。**有个小细节是gtsam并没有立即根据优化后的位姿修改关键帧的位姿cloudKeyPoses3D，优化的最后只是对标志位aLoopIsClosed进行了修改，这个标志位的作用体现在主线程调用correctPoses()时将gtsam对位姿的优化同步出来。
下面重点说明的是detectLoopClosure（）这个函数，其作用是根据距离和ScanContext信息确定与当前帧形成回环的历史关键帧，这里才真正体现了ScanContext的作用。
// jin: 检测与最新帧可以形成回环的关键帧,两种方法:
// 1/ 根据几何距离,在一定半径范围内,30s以上,最早的那个帧
// 2/ ScanContext,确定相似度最高的关键帧
bool mapOptimization::detectLoopClosure() {
std::lock_guard<std::mutex> lock(mtx);// 和void mapOptimization::run()不同时进行
/*
* 邻域搜索闭环
* 1. xyz distance-based radius search (contained in the original LeGO LOAM code)
* - for fine-stichting trajectories (for not-recognized nodes within scan context search)
*/
// jin: 基于目前位姿,在一定范围内(20m)内搜索最近邻,若最早的那个超过了30s,则选中为回环目标
// 选取前后25帧组成点云,并保存当前最近一帧点云
RSlatestSurfKeyFrameCloud->clear();// 当前关键帧
RSnearHistorySurfKeyFrameCloud->clear();// 尝试进行回环的关键帧前后一定范围帧组成的点云
RSnearHistorySurfKeyFrameCloudDS->clear();// 上面的降采样
// find the closest history key frame
std::vector<int> pointSearchIndLoop; // 搜索完的邻域点对应的索引
std::vector<float> pointSearchSqDisLoop; // 搜索完的邻域点与当前点的欧氏距离
// 用当前的所有关键帧生成树
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
// currentRobotPosPoint为最新一帧关键帧的位姿
// 0：返回的邻域个数，为0表示返回全部的邻域点
kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop,
pointSearchSqDisLoop, 0);
// jin: 选取最近邻中,时间距离30s以上,最早的那帧
RSclosestHistoryFrameID = -1;
int curMinID = 1000000;
// policy: take Oldest one (to fix error of the whole trajectory)
for (int i = 0; i < pointSearchIndLoop.size(); ++i) {
int id = pointSearchIndLoop[i];
// 时间差值大于30s, 认为是闭环
if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0) {
// RSclosestHistoryFrameID = id;
// break;
if (id < curMinID) {
curMinID = id;
RSclosestHistoryFrameID = curMinID;
}
}
}
if (RSclosestHistoryFrameID == -1) {
// Do nothing here
// then, do the next check: Scan context-based search
// not return false here;
} else {
// jin: 回环检测的进程是单独进行的,因此这里需要确定最新帧
// 检测到回环了会保存四种点云
// save latest key frames
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
// jin: 根据当前的位姿,对点云进行旋转和平移
// 点云的xyz坐标进行坐标系变换(分别绕xyz轴旋转)
*RSlatestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure],
&cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
*RSlatestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],
&cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
// latestSurfKeyFrameCloud中存储的是下面公式计算后的index(intensity):
// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
// 滤掉latestSurfKeyFrameCloud中index<0的点??? index值会小于0?
pcl::PointCloud<PointType>::Ptr RShahaCloud(new pcl::PointCloud<PointType>());
int cloudSize = RSlatestSurfKeyFrameCloud->points.size();
for (int i = 0; i < cloudSize; ++i) {
if ((int) RSlatestSurfKeyFrameCloud->points[i].intensity >= 0) {
RShahaCloud->push_back(RSlatestSurfKeyFrameCloud->points[i]);
}
}
RSlatestSurfKeyFrameCloud->clear();
*RSlatestSurfKeyFrameCloud = *RShahaCloud;
// jin: 保存一定范围内最早的那帧前后25帧的点,并在对应位姿处投影后进行合并
// save history near key frames
// historyKeyframeSearchNum在utility.h中定义为25，前后25个点进行变换
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j) {
if (RSclosestHistoryFrameID + j < 0 || RSclosestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
// 要求closestHistoryFrameID + j在0到cloudKeyPoses3D->points.size()-1之间,不能超过索引
*RSnearHistorySurfKeyFrameCloud += *transformPointCloud(
cornerCloudKeyFrames[RSclosestHistoryFrameID + j],
&cloudKeyPoses6D->points[RSclosestHistoryFrameID + j]);
*RSnearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[RSclosestHistoryFrameID + j],
&cloudKeyPoses6D->points[
RSclosestHistoryFrameID + j]);
}
// 下采样
downSizeFilterHistoryKeyFrames.setInputCloud(RSnearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*RSnearHistorySurfKeyFrameCloudDS);
}
/*
* 2. Scan context-based global localization
*/
SClatestSurfKeyFrameCloud->clear();
SCnearHistorySurfKeyFrameCloud->clear();
SCnearHistorySurfKeyFrameCloudDS->clear();
// std::lock_guard<std::mutex> lock(mtx);
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
SCclosestHistoryFrameID = -1; // init with -1
// jin: 这是最重要的部分，根据ScanContext确定回环的关键帧,返回的是关键帧的ID,和yaw角的偏移量
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;
}
// 以下，如果SC检测到了回环，保存回环上的帧前后25帧和当前帧，过程与上面完全一样
// 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
// jin: 将最新一帧激光点在回环位姿处进行投影???
*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;
// jin: ScanContext确定的回环关键帧,前后一段范围内的点组成点云地图
// 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);
// // optional: publish history near key frames
// if (pubHistoryKeyFrames.getNumSubscribers() != 0){
// sensor_msgs::PointCloud2 cloudMsgTemp;
// pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);
// cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
// cloudMsgTemp.header.frame_id = "/camera_init";
// pubHistoryKeyFrames.publish(cloudMsgTemp);
// }
return true;
} // detectLoopClosure
检测回环的方法有两个：
1: //根据距离（RangeSearch）
2: //根据ScanContext
对于前者，寻找回环的条件可以概括为在物理空间上在一定范围内（这里是20m），时间上相差一定间隔（这里是30S）的所有帧中最早的那个，然后将前后25帧加起来形成一个局部map，两者之间就形成了回环，也是需要添加约束的两个对象，这里不再赘述，下面重点说一下根据ScanContext检测回环的方法。
从上面的代码中可以看出ScanContext的总体流程也差不多，都是寻找一个回环帧，然后把前后25个帧加起来作为回环的对象。不同之处在于，前者是根据关键帧的位姿在kd-tree中搜索出来的最近邻，后者是根据ScanContext信息，这是在函数scManager.detectLoopClosureID()中实现的：
// jin: 根据ScanContext确定回环的关键帧
// 先根据ring key向量,在kd-tree中搜索出多个位置相似的关键帧
// 然后对每个ScanContext列偏移多次，计算最好的列偏移及对应的距离,计算匹配度最好的关键帧的ID及其列偏移量
// 其中,为了加快对列偏移计算,用到了SectorKey先确定一个粗略的初始值
std::pair<int, float> SCManager::detectLoopClosureID(void) {
int loop_id{-1}; // init with -1, -1 means no loop (== LeGO-LOAM’s variable “closestHistoryFrameID”)
// jin: 最新一帧的ring key
auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) 容器末尾元素
// jin: 最新一帧的Scan Context
auto curr_desc = polarcontexts_.back(); // current observation (query)
/*
* step 1: candidates from ringkey tree_
*/
// jin: 之所以使用ring key是因为,ring代表了一周,即便角度相差很大对应ring的key(均值)也相差不大
// 因而,可以起到筛选处距离不会太远的关键帧的作用(圆环之间的相似度很高)
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);// 最近一定范围内的不参加搜索吧
// jin: 搜索树
polarcontext_tree_.reset();
/// 使用std::vector中的元素创建的TREE，方便搜索向量的最近邻
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;
// jin: 结果保存在这里
// 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));
t_tree_search.toc("Tree search");
/*
* step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
*/
// jin: 对上一步筛选出来的所有位姿,计算不同列偏移下的最好匹配度
TicToc t_calc_dist;
for (int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++) {
// jin: 候选帧的ScanContext
MatrixXd polarcontext_candidate = polarcontexts_[candidate_indexes[candidate_iter_idx]];
// jin: 这里也是一个重点
// 根据不同列偏移,计算两帧之间匹配程度,返回的是最小差距和最好的偏移，这表征着两个ScanContext最终的差异
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};// jin: 返回匹配最优的KeyFrame的ID和计算出来的角度差
return result;
} // SCManager::detectLoopClosureID
这里先将所有关键帧的ring key信息生成一个tree，然后在树中搜索一定数量的最近邻。可以想想一下为什么是ring key以及ring key为什么可以。对于位置和姿态都未知的历史关键帧，如果其角度与当前帧角度存在较大差异，那么点云差别会非常大，这是由于雷达扫描机理造成的，而径向即便存在比较大的差距点云的相似度也会非常大。所以，如果某个历史关键帧位置比较接近，但是角度相差很大，那么ring的相似度依然会很高，这样就在不知道角度偏差的情况下，鲁棒的检测到相似帧。
但是仅仅如此还是不够的，很有可能错误检测到回环，而且不知道初始的角度差别， ICP也没法计算约束，因此还需要进一步估算方向的偏差，并从所有的候选相似帧中选出一个最优的。
distanceBtnScanContext函数即是计算当前ScanContext与所有候选历史关键帧ScanContext之间匹配度的函数，也即计算两个矩阵之间的相似度。该函数中为了快速确定最优的列偏移量和减小计算量，还调用了makeSectorkeyFromScancontext来将ScanContext压缩成向量先进行粗略的比较，再将整个ScanContext进行对比和计算相似度。
到这里通过两种不同的方式找出了当前最新关键帧与历史关键帧之间存在的回环，performLoopClosure中接下来的工作就是对回环的对象通过ICP计算约束，添加到gtsam并进行全局位姿优化。
到这里，SC-Lego-LOAM的分析基本就结束了。实际上有很多的细节可以分析，不再具体展开了，有些内容笔者也不太明白，比如说多次进行的坐标的交换怎么对应，gtsam的使用等（倒是常用Ceres…），以后慢慢学习，大家有合适的资源也欢迎留言推荐。
参考：
SC-Lego-LOAM解析（上）
SC-Lego-LOAM解析（中）
SC-Lego-LOAM解析（下）
本文经允许后转自知乎：https://zhuanlan.zhihu.com/p/348281520
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
