2021SC@SDUSC
LoopClosing分析（5）
这一篇博客接着分析LoopCLosing，依旧将分析以注释的形式直接写在代码中。
- RequestReset函数是由外部线程调用，用来请求复位当前线程。
void LoopClosing::RequestReset()
{
// 标志置位
{
unique_lock<mutex> lock(mMutexReset);
mbResetRequested = true;
}
// 堵塞,直到回环检测线程复位完成
while(1)
{
{
unique_lock<mutex> lock2(mMutexReset);
if(!mbResetRequested)
break;
}
//usleep(5000);
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
}
- ResetIfRequested函数则是由当前线程调用，用来检查是否有外部线程请求复位当前线程：如果有的话就复位回环检测线程
void LoopClosing::ResetIfRequested()
{
unique_lock<mutex> lock(mMutexReset);
// 如果有来自于外部的线程的复位请求,那么就复位当前线程
if(mbResetRequested)
{
mlpLoopKeyFrameQueue.clear(); // 清空参与和进行回环检测的关键帧队列
mLastLoopKFid=0; // 上一次没有和任何关键帧形成闭环关系
mbResetRequested=false; // 复位请求标志复位
}else if(mbResetActiveMapRequested)
{
for (list<KeyFrame*>::const_iterator it=mlpLoopKeyFrameQueue.begin(); it != mlpLoopKeyFrameQueue.end();)
{
KeyFrame* pKFi = *it;
if(pKFi->GetMap() == mpMapToReset)
{
it = mlpLoopKeyFrameQueue.erase(it);
}
else
++it;
}
mLastLoopKFid=mpAtlas->GetLastInitKFid(); //TODO old variable, it is not use in the new algorithm
mbResetActiveMapRequested=false;
}
}
- RunGlobalBundleAdjustment是一个较为重要的函数，它是全局BA线程的主函数。其参数nLoopKF 在调用的时候是当前关键帧的id。
ORB-SLAM3 采用g2o 图优化来完成BA：
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
Verbose::PrintMess("Starting Global Bundle Adjustment", Verbose::VERBOSITY_NORMAL);
const bool bImuInit = pActiveMap->isImuInitialized();
if(!bImuInit)
Optimizer::GlobalBundleAdjustemnt(pActiveMap,10,&mbStopGBA,nLoopKF,false);
else
Optimizer::FullInertialBA(pActiveMap,7,false,nLoopKF,&mbStopGBA);
// 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的
int idx = mnFullBAIdx;
// mbStopGBA直接传引用过去了,这样当有外部请求的时候这个优化函数能够及时响应并且结束掉
// Step 1 执行全局BA，优化所有的关键帧位姿和地图中地图点
// Update all MapPoints and KeyFrames
// Local Mapping was active during BA, that means that there might be new keyframes
// not included in the Global BA and they are not consistent with the updated map.
// We need to propagate the correction through the spanning tree
// 更新所有的地图点和关键帧
// 在global BA过程中local mapping线程仍然在工作，这意味着在global BA时可能有新的关键帧产生，但是并未包括在GBA里，
// 所以和更新后的地图并不连续。需要通过spanning tree来传播
{
unique_lock<mutex> lock(mMutexGBA);
// 如果全局BA过程是因为意外结束的,那么直接退出GBA
if(idx!=mnFullBAIdx)
return;
if(!bImuInit && pActiveMap->isImuInitialized())
return;
// 如果当前GBA没有中断请求，更新位姿和地图点
// 这里和上面那句话的功能还有些不同,因为如果一次全局优化被中断,往往意味又要重新开启一个新的全局BA;为了中断当前正在执行的优化过程mbStopGBA将会被置位,同时会有一定的时间
// 使得该线程进行响应;而在开启一个新的全局优化进程之前 mbStopGBA 将会被置为False
// 因此,如果被强行中断的线程退出时已经有新的线程启动了,mbStopGBA=false,为了避免进行后面的程序,所以有了上面的程序;
// 而如果被强行中断的线程退出时新的线程还没有启动,那么上面的条件就不起作用了(虽然概率很小,前面的程序中mbStopGBA置位后很快mnFullBAIdx就++了,保险起见),所以这里要再判断一次
if(!mbStopGBA)
{
cout << "Global Bundle Adjustment finished" << endl;
cout << "Updating map ..." << endl;
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
// 等待直到local mapping结束才会继续后续操作
while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
{
usleep(1000);
}
// Get Map Mutex
// 后续要更新地图所以要上锁
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Correct keyframes starting at map first keyframe
// 从第一个关键帧开始矫正关键帧。刚开始只保存了初始化第一个关键帧
list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());
// 问：GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA是不变的吧？那后面调整位姿的意义何在？
// 回答：注意在前面essential graph BA里只锁住了回环帧，没有锁定第1个初始化关键帧位姿。所以第1个初始化关键帧位姿已经更新了
// 在GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA应该是essential BA结果，在这里统一更新了
// Step 2 遍历并更新全局地图中的所有spanning tree中的关键帧
while(!lpKFtoCheck.empty())
{
KeyFrame* pKF = lpKFtoCheck.front();
const set<KeyFrame*> sChilds = pKF->GetChilds();
cv::Mat Twc = pKF->GetPoseInverse();
// 遍历当前关键帧的子关键帧
for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
{
KeyFrame* pChild = *sit;
// 记录避免重复
if(!pChild || pChild->isBad())
continue;if(pChild->mnBAGlobalForKF!=nLoopKF)
{
// 从父关键帧到当前子关键帧的位姿变换 T_child_farther
cv::Mat Tchildc = pChild->GetPose()*Twc;
// 再利用优化后的父关键帧的位姿，转换到世界坐标系下，相当于更新了子关键帧的位姿
// 这种最小生成树中除了根节点，其他的节点都会作为其他关键帧的子节点，这样做可以使得最终所有的关键帧都得到了优化
pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;
cv::Mat Rcor = pChild->mTcwGBA.rowRange(0,3).colRange(0,3).t()*pChild->GetRotation();
if(!pChild->GetVelocity().empty()){
//cout << "Child velocity: " << pChild->GetVelocity() << endl;
pChild->mVwbGBA = Rcor*pChild->GetVelocity();
}
else
Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL);
//cout << "Child bias: " << pChild->GetImuBias() << endl;
pChild->mBiasGBA = pChild->GetImuBias();
// 做个标记，避免重复
pChild->mnBAGlobalForKF=nLoopKF;
}
lpKFtoCheck.push_back(pChild);
}
// 记录未矫正的关键帧的位姿
pKF->mTcwBefGBA = pKF->GetPose();
// 记录已经矫正的关键帧的位姿
pKF->SetPose(pKF->mTcwGBA);
// 从列表中移除
lpKFtoCheck.pop_front();
}
// Correct MapPoints
const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();
// Step 3 遍历每一个地图点并用更新的关键帧位姿来更新地图点位置
for(size_t i=0; i<vpMPs.size(); i++)
{
MapPoint* pMP = vpMPs[i];
if(pMP->isBad())
continue;
// 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可
if(pMP->mnBAGlobalForKF==nLoopKF)
{
// If optimized by Global BA, just update
pMP->SetWorldPos(pMP->mPosGBA);
}
else
{
// 如这个地图点并没有直接参与到全局BA优化的过程中,那么就使用其参考关键帧的新位姿来优化自己的坐标
// Update according to the correction of its reference keyframe
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
// 如果参考关键帧并没有经过此次全局BA优化，就跳过
if(pRefKF->mnBAGlobalForKF!=nLoopKF)
continue;
if(pRefKF->mTcwBefGBA.empty())
continue;
// Map to non-corrected camera
cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
// 转换到其参考关键帧相机坐标系下的坐标
cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;
// Backproject using corrected camera
// 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下
// Map to non-corrected camera
cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;
// Backproject using corrected camera
cv::Mat Twc = pRefKF->GetPoseInverse();
cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
cv::Mat twc = Twc.rowRange(0,3).col(3);
pMP->SetWorldPos(Rwc*Xc+twc);
}
}
pActiveMap->InformNewBigChange();
pActiveMap->IncreaseChangeIndex();
// 释放
mpLocalMapper->Release();
Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL);
}
mbFinishedGBA = true;
mbRunningGBA = false;
}
}
RunGlobalBundleAdjustment函数用于闭环优化，在闭环结束前新开一个线程，全局优化，在此之前会OptimizeEssentialGraph。它会和用于单目初始化的CreateInitialMapMonocular函数组成GlobalBundleAdjustment函数，用于全局BA。
610
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
