|
本文分享自华为云社区《lio-sam框架：回环检测及位姿计算-云社区-华为云》，作者：月照银海似蛟龙 。 媒介图优化本身有成形的 开源的库 例如 lio-sam 中就是 通过 gtsam 库 举行 图优化的，其中束缚因子就包括回环检测因子 本篇主要剖析lio-sam框架下，是怎样举行回环检测及位姿计算的。 Pose Graph的概念用一个图(Graph 图论)来表现SLAM题目 图中的节点来表现呆板人的位姿 二维的话即为 (x,y,yaw) 两个节点之间的边表现两个位姿的空间束缚(相对位姿关系以及对应方差或线性矩阵) 边分为了两种边 - 帧间边:毗连的前后,时间上是连续的
- 回环边:毗连的前后,时间上是不连续的,但是直接也是两个位姿的空间束缚
构建了回环边才会有误差出现,没有回环边是没有误差的 图优化的基本头脑:
出现回环边,有了误差之后.构建图,而且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小 一旦形成回环即可举行优化消除误差 里程积分的相对位姿视为预测值 图上的各个节点就是通过里程(激光里程计\轮速里程计)积分得到的
回环计算的相对位姿视为观测值 图上就是说通过 X2和X8的帧间匹配作为观测值 图优化要干的事:
构建图并调整各节点的位姿,让预测与观测的误差最小 回环检测及位姿计算在点云匹配之后，可以来看回环检测部分的代码了 这部分的代码入口在 main函数中 std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
单独开了一个回环检测的线程 下面来看loopClosureThread这个函数 void loopClosureThread() {
if (loopClosureEnableFlag == false) return;
假如不需要举行回环检测，那么就退出这个线程 ros::Rate rate(loopClosureFrequency);
设置回环检测的频率 loopClosureFrequency默以为 1hz
没有必要太频仍 while (ros::ok()) { rate.sleep(); performLoopClosure(); visualizeLoopClosure(); }
设置完频率后，举行一个while的死循环。
执行完一次就必须sleep一段时间，否则该线程的cpu占用会非常高
通过performLoopClosure visualizeLoopClosure 执行回环检测 下面来看performLoopClosure 函数的具体内容 void performLoopClosure() {
if (cloudKeyPoses3D->points.empty() == true) return;
假如没有关键帧，就没法举行回环检测了
就直接退出 mtx.lock(); *copy_cloudKeyPoses3D = *cloudKeyPoses3D; *copy_cloudKeyPoses6D = *cloudKeyPoses6D; mtx.unlock();
把存储关键帧额位姿的点云copy出来，制止线程冲突 cloudKeyPoses3D就是关键帧的位置 cloudKeyPoses6D就是关键帧的位姿 if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
起首看一下外部通知的回环信息 if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false) return;
然后根据里程计的距离来检测回环
假如还没有则直接返回 来看detectLoopClosureDistance 函数的具体内容 int loopKeyCur = copy_cloudKeyPoses3D->size() - 1; int loopKeyPre = -1;
检测最新帧是否和别的帧形成回环
取出最新帧的索引 auto it = loopIndexContainer.find(loopKeyCur); if (it != loopIndexContainer.end()) return false;
检查一下较晚帧是否和别的形成了回环，假如有就算了
因为当前帧刚刚出现，不会和别的帧形成回环，以是基本不会触发 kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
把只包罗关键帧位移信息的点云填充kdtree kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
根据最后一个关键帧的平移信息，探求离他肯定距离内的别的关键帧 - historyKeyframeSearchRadius 搜索范围 15m
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; }
汗青帧，必须比当前帧隔断30s以上
必须满足时间上超过肯定阈值，才以为是一个有效的回环 - historyKeyframeSearchTimeDiff 时间阈值 30s
假如时间上满足要做就找到了汗青回环帧
那么赋值id 而且 break
一次找一个回环帧就行了 if (loopKeyPre == -1 || loopKeyCur == loopKeyPre) return false;
假如没有找到回环或者回环找到自己身上去了，就以为是本次回环探求失败 *latestID = loopKeyCur; *closestID = loopKeyPre; return true; }
至此则找到了认真关键帧和汗青回环帧
赋值当前帧和汗青回环帧的id 假如在一个地方静止不动的时候，那么按照这个逻辑也会形成关键帧
可以通过以关键帧序列号的方式加以改进 假如检测回环存在了，那么则可以举行下面内容，就是计算检测出这两帧的位姿变换 pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
声明当前关键帧的点云
声明汗青回环帧四周的点云（局部舆图） loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
当前关键帧把自己取了出来 来看 loopFindNearKeyframes 这个函数 void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum) {
for (int i = -searchNum; i <= searchNum; ++i) {
searchNum 是搜索范围 ，遍历帧的范围 int keyNear = key + i;
找到这个 idx if (keyNear < 0 || keyNear >= cloudSize ) continue;
假如超出范围了就算了 *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], _cloudKeyPoses6D->points[keyNear]); *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], _cloudKeyPoses6D->points[keyNear]);
否则吧对应角点和面点的点云转到世界坐标系下去 if (nearKeyframes->empty()) return;
假如没有有效的点云就算了 pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>()); downSizeFilterICP.setInputCloud(nearKeyframes); downSizeFilterICP.filter(*cloud_temp); *nearKeyframes = *cloud_temp;
吧点云下采样 然后会到之前的地方： loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
回环帧把自己四周一些点云取出来，也就是构成一个帧局部舆图的一个匹配题目 - historyKeyframeSearchNum 25帧
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000) return;
假如点云数目太少就算了 if (pubHistoryKeyFrames.getNumSubscribers() != 0) publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
把局部舆图发布出来供rviz可视化使用 现在有了当前关键帧投到舆图坐标系下的点云和汗青回环帧投到舆图坐标系下的局部舆图，那么接下来就可以举行两者的icp位姿变换求解 static pcl::IterativeClosestPoint<PointType, PointType> icp;
使用简单的icp来举行帧到局部舆图的配准 icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
设置最大干系距离 - historyKeyframeSearchRadius 15m
icp.setMaximumIterations(100);
最大优化次数 icp.setTransformationEpsilon(1e-6);
单次变换范围 icp.setEuclideanFitnessEpsilon(1e-6);icp.setRANSACIterations(0);
残差设置 icp.setInputSource(cureKeyframeCloud); icp.setInputTarget(prevKeyframeCloud);
设置两个点云 pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>()); icp.align(*unused_result);
执行配准 if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) return;
检测icp是否收敛 且 得分是否满足要求 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); }
把修正后的当前点云发布供可视化使用 correctionLidarFrame = icp.getFinalTransformation();
获得两个点云的变换矩阵结果 Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
取出当前帧的位姿 Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
将icp结果补偿已往，就是当前帧的更为准确的位姿结果 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(copy_cloudKeyPoses6D->points[loopKeyPre]);
将当前帧补偿后的位姿 转换成 gtsam的形式
From 和 To相当于帧间束缚的因子
To是汗青回环帧的位姿 gtsam::Vector Vector6(6);float noiseScore = icp.getFitnessScore();noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
使用icp的得分作为他们的束缚噪声项 loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));//两帧索引 loopPoseQueue.push_back(poseFrom.between(poseTo));//当前帧与汗青回环帧相对位姿 loopNoiseQueue.push_back(constraintNoise);//噪声
将两帧索引，两帧相对位姿和噪声作为回环束缚 送入对列 loopIndexContainer[loopKeyCur] = loopKeyPre;
生存已经存在的束缚对 总结lio-sam回环检测的方式 构建关键帧，将关键帧的位姿存储。以固定频率举行回环检测。每次处理最新的关键帧，通过kdtree探求汗青关键帧中距离和时间满足条件的一个关键帧。然后就以为形成了回环。
形成回环后，汗青帧四周25帧，构建局部舆图，与当前关键帧举行icp匹配求解位姿变换。 lio-sam 以为里程计累计漂移比较小，以是通过距离与时间这两个概念举行的关键帧的回环检测 点击下方，第一时间了解华为云新鲜技术~ 华为云博客_大数据博客_AI博客_云计算博客_开辟者中心-华为云 |
评论