LIO-SAM后端中的回环检测及位姿计算
文章目录
- performLoopClosure()
-
- 寻找回环关键帧detectLoopClosureDistance
- 检出回环之后开始计算两帧位姿变换
回环检测函数调用在mapOptmization.cpp 的main中单独开了一个线程运行，1秒运行一遍回环检测
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
回环检测思路描述：主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧，开始用icp匹配计算位姿，算完后把两帧索引，两帧相对位姿，噪声（icp得分）放入回环约束队列中，对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解
流程综述：
- 如果不需要进行回环检测就退出这个线程，有标志位设置
// 回环检测线程
void loopClosureThread()
{
// 如果不需要进行回环检测，那么就退出这个线程
if (loopClosureEnableFlag == false)
return;
// 设置回环检测的频率
ros::Rate rate(loopClosureFrequency);
while (ros::ok())
{
// 执行完一次就必须sleep一段时间，否则该线程的cpu占用会非常高
rate.sleep();
// 执行回环检测
performLoopClosure();
visualizeLoopClosure();
}
}
- 执行回环检测
performLoopClosure()
- 如果没有关键帧就无法退出回环检测
if (cloudKeyPoses3D->points.empty() == true)
return;
- 把存储关键帧的位姿的点云copy出来，避免线程冲突，这里开了把锁防止数据调用冲突
mtx.lock();
// 把存储关键帧的位姿的点云copy出来，避免线程冲突
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock()
寻找回环关键帧detectLoopClosureDistance
主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧
bool detectLoopClosureDistance(int *latestID, int *closestID)
{
// 检测最新帧是否和其他帧形成回环，所以后面一帧的id就是最后一个关键帧
int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
int loopKeyPre = -1;
// check loop constraint added before
// 检查一下较晚帧是否和别的形成了回环，如果有就算了
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
// find the closest history key frame
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
// 把只包含关键帧位移信息的点云填充kdtree
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
// 根据最后一个关键帧的平移信息，寻找离他一定距离内的其他关键帧
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
// 遍历找到的候选关键帧
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{
int id = pointSearchIndLoop[i];
// 必须满足时间上超过一定阈值，才认为是一个有效的回环
if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
{
// 此时就退出了
loopKeyPre = id;
break;
}
}
// 如果没有找到回环或者回环找到自己身上去了，就认为是此次回环寻找失败
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}
检出回环之后开始计算两帧位姿变换
- 把回环帧的前后各25帧都取出来，共51帧，构建一个局部点云地图，然后用当前帧与这个局部点云地图进行匹配
- 构建回环帧的局部地图用loopFindNearKeyframes()
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
// extract near keyframes
nearKeyframes->clear();
int cloudSize = copy_cloudKeyPoses6D->size();
// searchNum是搜索范围
for (int i = -searchNum; i <= searchNum; ++i)
{
// 找到这个idx
int keyNear = key + i;
// 如果超出范围就算了
if (keyNear < 0 || keyNear >= cloudSize )
continue;
// 否则把对应角点和面点的点云转到世界坐标系下去
*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
}
// 如果没有有效的点云就算了
if (nearKeyframes->empty())
return;
// downsample near keyframes
// 把点云下采样
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterICP.setInputCloud(nearKeyframes);
downSizeFilterICP.filter(*cloud_temp);
*nearKeyframes = *cloud_temp;
}
- 将当前帧的点云cureKeyframeCloud和回环帧的局部地图点云prevKeyframeCloud加入icp的类中，直接用库进行点云配准，然后检查icp的收敛得分是否满足要求和把修正后的当前点云发布供可视化使用
- icp匹配如下图，一般情况下的正确匹配是红色圈圈，绿色圈圈是显示匹配到最近距离的其它点的情况，但是通过内部得分优化，会去掉绿色这个错误的匹配，这些优化都在pcl的icp库里面自己进行
// 设置两个点云
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
// 执行点云配准
icp.align(*unused_result);
// 检查icp是否收敛且得分是否满足要求
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// publish corrected cloud
// 把修正后的当前点云发布供可视化使用
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
- 获取当前帧到局部地图的位姿变换矩阵结果 T c o r r e c t − c u r T_{correct-cur} Tcorrect−cur，再获取当前帧的世界坐标的位姿 T c u r − w T_{cur-w} Tcur−w，然后 T c o r r e c t − c u r ∗ T c u r − w T_{correct-cur}*T_{cur-w} Tcorrect−cur∗Tcur−w补偿后获取当前帧正确的位姿结果
// Get pose transformation
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
// 获得两个点云的变换矩阵结果
correctionLidarFrame = icp.getFinalTransformation();
// transform from world origin to wrong pose
// 找到稍早点云的位姿结果
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// transform from world origin to corrected pose
// 将icp的结果补偿过去，就是稍晚帧的更为准确的位姿结果
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
// 将稍晚帧的位姿转成平移+欧拉角
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
- 然后将两帧索引（当前帧cur和回环帧pre），校正后的两帧相对位姿，噪声（icp的得分作为噪声）加入回环约束队列中，使得整个图根据最优值进行全局误差优化
其中校正后的相对位姿计算，回环帧为 T p r e − w T_{pre-w} Tpre−w，校正后的当前帧为 T c o r r e c t − w T_{correct-w} Tcorrect−w， T c o r r e c t − p r e = T c o r r e c t − w ∗ T p r e − w − 1 T_{correct-pre}=T_{correct-w}*T_{pre-w}^{-1} Tcorrect−pre=Tcorrect−w∗Tpre−w−1
// from是修正后的稍晚帧的点云位姿
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
// to是修正前的稍早帧的点云位姿
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
// 使用icp的得分作为他们的约束的噪声项
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
// Add pose constraint
mtx.lock();
// 将两帧索引，两帧相对位姿和噪声作为回环约束送入队列
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();
// add loop constriant
// 保存已存在的约束对
loopIndexContainer[loopKeyCur] = loopKeyPre;
对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解
整体代码如下：
void performLoopClosure()
{
// 如果没有关键帧，就没法进行回环检测了
if (cloudKeyPoses3D->points.empty() == true)
return;
mtx.lock();
// 把存储关键帧的位姿的点云copy出来，避免线程冲突
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
// find keys
int loopKeyCur;
int loopKeyPre;
// 首先看一下外部通知的回环信息
if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
// 然后根据里程记的距离来检测回环
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
return;
// 检出回环之后开始计算两帧位姿变换
// extract cloud
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
{
// 稍晚的帧就把自己取了出来
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
// 稍早一点的就把自己和周围一些点云取出来，也就是构成一个帧到局部地图的一个匹配问题
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
// 如果点云数目太少就算了
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
// 把局部地图发布出去供rviz可视化使用
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
// ICP Settings
// 使用简单的icp来进行帧到局部地图的配准
static pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// Align clouds
// 设置两个点云
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
// 执行点云配准
icp.align(*unused_result);
// 检查icp是否收敛且得分是否满足要求
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// publish corrected cloud
// 把修正后的当前点云发布供可视化使用
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// Get pose transformation
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
// 获得两个点云的变换矩阵结果
correctionLidarFrame = icp.getFinalTransformation();
// transform from world origin to wrong pose
// 找到稍早点云的位姿结果
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// transform from world origin to corrected pose
// 将icp的结果补偿过去，就是稍晚帧的更为准确的位姿结果
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
// 将稍晚帧的位姿转成平移+欧拉角
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
// from是修正后的稍晚帧的点云位姿
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
// to是修正前的稍早帧的点云位姿
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
// 使用icp的得分作为他们的约束的噪声项
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
// Add pose constraint
mtx.lock();
// 将两帧索引，两帧相对位姿和噪声作为回环约束送入队列
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();
// add loop constriant
// 保存已存在的约束对
loopIndexContainer[loopKeyCur] = loopKeyPre;
}