LocalBundleAdjustment():用于LocalMapping线程中剔除关键帧之前的局部地图优化。局部BA的共视图(其中帧都是关键帧)。用数据结构的术语这个图表示的数据结构称为无向加权图,又叫网,权重就是共视地图点的个数。
/*
pKF:当前关键帧
pbStopFlag:优化停止标志位
vpNonEnoughOptKFs:没有足够观测的那些关键帧(这里显然是入参)
*/
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vector<KeyFrame*> &vpNonEnoughOptKFs)
{
// 局部关键帧,在后面可以看出是当前关键帧的二级相邻关键帧范围的关键帧
list<KeyFrame*> lLocalKeyFrames;
// 第一步: 将当前关键帧及其共视关键帧加入lLocalKeyFrames这个容器中
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;//记录下当前关键帧
Map* pCurrentMap = pKF->GetMap();
// 找到关键帧连接的一级相邻共视关键帧,加入lLocalKeyFrames中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
KeyFrame* pKFi = vNeighKFs[i];
pKFi->mnBALocalForKF = pKF->mnId;// 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId,防止重复添加
// 此关键帧包含在当前地图中,且是好帧,就放入要优化的容器内存储
if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)
lLocalKeyFrames.push_back(pKFi);
}
//试图将那些认为观测数目不够多的关键帧也参与优化。
for(KeyFrame* pKFi : vpNonEnoughOptKFs)
{
if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap && pKFi->mnBALocalForKF != pKF->mnId)
{
pKFi->mnBALocalForKF = pKF->mnId;
lLocalKeyFrames.push_back(pKFi);
}
}
// 第二步: 遍历 lLocalKeyFrames 中一级相邻关键帧,将它们观测的地图点加入到lLocalMapPoints
list<MapPoint*> lLocalMapPoints;
set<MapPoint*> sNumObsMP;
int num_fixedKF;
// 遍历 lLocalKeyFrames 中的每一个关键帧
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)//迭代器遍历
{
KeyFrame* pKFi = *lit;
//此关键帧如果是当前地图的第一帧,那就固定不优化。
if(pKFi->mnId==pCurrentMap->GetInitKFid())
{
num_fixedKF = 1;
}
// 得到此关键帧的所有地图点
vector<MapPoint*> vpMPs = pKFi->GetMapPointMatches();
// 遍历这个关键帧观测到的每一个地图点,加入到lLocalMapPoints
for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
if(!pMP->isBad() && pMP->GetMap() == pCurrentMap)
{
// 为了防止重复添加,把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
}
}
}
// 第三步:得到能被局部MapPoints观测到,但不属于局部关键帧的(二级相邻)关键帧,这些关键帧在局部BA优化时不优化
list<KeyFrame*> lFixedCameras;
// 遍历局部地图中的每个地图点
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 观测到该MapPoint的KF和该MapPoint在KF中的索引
map<KeyFrame*,tuple<int,int>> observations = (*lit)->GetObservations();
// 遍历所有观测到该地图点的关键帧
for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)//迭代器遍历
{
KeyFrame* pKFi = mit->first;
// pKFi->mnBALocalForKF!=pKF->mnId 表示不在局部关键帧之列,
// pKFi->mnBAFixedForKF!=pKF->mnId 表示还未固定的关键帧
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId )
{
// 也就是将局部地图点能观测到的、但是不在局部BA范围的关键帧固定住。
pKFi->mnBAFixedForKF=pKF->mnId;
if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)
lFixedCameras.push_back(pKFi);
}
}
}
num_fixedKF = lFixedCameras.size() + num_fixedKF;
// 如果固定的关键帧太少,想办法在局部关键帧里找
if(num_fixedKF < 2)
{
//Verbose::PrintMess("LM-LBA: New Fixed KFs had been set", Verbose::VERBOSITY_NORMAL);
//TODO We set 2 KFs to fixed to avoid a degree of freedom in scale
list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin();
int lowerId = pKF->mnId;
KeyFrame* pLowerKf;
int secondLowerId = pKF->mnId;
KeyFrame* pSecondLowerKF;
for(; lit != lLocalKeyFrames.end(); lit++)
{
KeyFrame* pKFi = *lit;
// 如果正好是当前帧或者已经固定过就跳过
if(pKFi == pKF || pKFi->mnId == pCurrentMap->GetInitKFid())
{
continue;
}
// 进行两次比较,记录与当前帧距离大小排序(两个)
if(pKFi->mnId < lowerId)
{
lowerId = pKFi->mnId;
pLowerKf = pKFi;
}
else if(pKFi->mnId < secondLowerId)
{
secondLowerId = pKFi->mnId;
pSecondLowerKF = pKFi;
}
}
lFixedCameras.push_back(pLowerKf);//将离当前帧有点远的帧固定
lLocalKeyFrames.remove(pLowerKf);//然后在局部关键帧容器中剔除
num_fixedKF++;
// 如果还是不够,把再远点的固定住(按理说不至于到这步境地吧)
if(num_fixedKF < 2)
{
lFixedCameras.push_back(pSecondLowerKF);
lLocalKeyFrames.remove(pSecondLowerKF);
num_fixedKF++;
}
}
//我感觉这不可能啊,除非当前帧没有共视关键帧???
if(num_fixedKF == 0)
{
Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_NORMAL);
//return;
}
// 第四步 构造g2o优化器,按照前面我们梳理的步骤来就行了
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
if (pCurrentMap->IsInertial())
solver->setUserLambdaInit(100.0);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);//调试器关闭
// 外界设置的停止优化标志
// 可能在 Tracking::NeedNewKeyFrame() 里置位
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
// 记录参与局部BA的最大关键帧mnId
unsigned long maxKFid = 0;
// 第五步: 添加待优化的位姿顶点
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
// 设置初始优化位姿
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
// 如果是初始关键帧,要锁住位姿不优化
vSE3->setFixed(pKFi->mnId==pCurrentMap->GetInitKFid());
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// 第六步: 添加不优化的位姿顶点,目的是增加约束
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
// 所有的这些顶点的位姿都不优化,只是为了增加约束项
vSE3->setFixed(true);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
//第七步: 添加待优化的3D地图点顶点
// 边的数目 = pose数目 * 地图点数目
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
// 定义一些容器存储各种类型的边
vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
vpEdgesBody.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFBody;
vpEdgeKFBody.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeBody;
vpMapPointEdgeBody.reserve(nExpectedSize);
vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
// 卡方检验
// 自由度为2的卡方分布,显著性水平为0.05,对应的临界阈值5.991
const float thHuberMono = sqrt(5.991);
// 自由度为3的卡方分布,显著性水平为0.05,对应的临界阈值7.815
const float thHuberStereo = sqrt(7.815);
int nPoints = 0;
int nKFs = lLocalKeyFrames.size()+lFixedCameras.size(), nEdges = 0;
// 遍历所有的局部地图中的地图点
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 添加顶点:MapPoint
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
// 地图点的id在记录的最大关键帧id基础上增加
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
// 因为使用了LinearSolverType,所以需要将所有的三维点边缘化掉
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
nPoints++;
// 观测到该地图点的KF和该地图点在KF中的索引
const map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations();
// 第八步: 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
// 遍历所有观测到当前地图点的关键帧
for(map<KeyFrame*,tuple<int,int>>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)
{
const int cam0Index = get<0>(mit->second);
// 这里只注释单目模式下的边构造
if(cam0Index != -1 && pKFi->mvuRight[cam0Index]<0)
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[cam0Index];//去完畸变的特征点像素
Eigen::Matrix<double,2,1> obs;//记录观测信息(上面像素)
obs << kpUn.pt.x, kpUn.pt.y;
ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
// 权重为特征点所在图像金字塔的层数的倒数
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
// 使用鲁棒核函数抑制外点
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
e->pCamera = pKFi->mpCamera;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
nEdges++;
}
//……
}
}
}
}
// 开始BA前再次确认是否有外部请求停止优化,因为这个变量是引用传递,会随外部变化
// 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位
if(pbStopFlag)
if(*pbStopFlag)
{
return;
}
// 第九步: 开始优化。分成两个阶段
// 第一阶段优化
optimizer.initializeOptimization();
// 迭代5次
int numPerform_it = optimizer.optimize(5);
bool bDoMore= true;
// 检查是否外部请求停止
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
// 如果有外部请求停止,那么就不在进行第二阶段的优化
if(bDoMore)
{
int nMonoBadObs = 0;
// 第十步: 检测outlier,并设置下次不优化
// 遍历所有的单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
if(e->chi2()>5.991 || !e->isDepthPositive())
{
//e->setLevel(1);
nMonoBadObs++;
}
//e->setRobustKernel(0);
}
// 第十一步::排除误差较大的outlier后再次优化
//optimizer.initializeOptimization(0);
//numPerform_it = optimizer.optimize(10);
numPerform_it += optimizer.optimize(5);
}
vector<pair<KeyFrame*,MapPoint*> > vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesBody.size()+vpEdgesStereo.size());
// 第十二步::在优化后重新计算误差,剔除连接误差比较大的关键帧和MapPoint
// 对于单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
// 基于卡方检验计算出的阈值(假设测量有一个像素的偏差)
// 自由度为2的卡方分布,显著性水平为0.05,对应的临界阈值5.991
// 如果 当前边误差超出阈值,或者边链接的地图点深度值为负,说明这个边有问题,要删掉了
if(e->chi2()>5.991 || !e->isDepthPositive())
{
// outlier
KeyFrame* pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
//这应该是将地图点转换到IMU系进行检测,前边误删了……
for(size_t i=0, iend=vpEdgesBody.size(); i<iend;i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i];
MapPoint* pMP = vpMapPointEdgeBody[i];
if(pMP->isBad())
continue;
if(e->chi2()>5.991 || !e->isDepthPositive())
{
KeyFrame* pKFi = vpEdgeKFBody[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
bool bRedrawError = false;
bool bWriteStats = false;
unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);//拿到锁进行地图更新
// 删除点
// 连接偏差比较大,在关键帧中剔除对该地图点的观测
// 连接偏差比较大,在地图点中剔除对该关键帧的观测
if(!vToErase.empty())
{
for(size_t i=0;i<vToErase.size();i++)
{
KeyFrame* pKFi = vToErase[i].first;
MapPoint* pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
}
// 第十三步:优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性
vpNonEnoughOptKFs.clear();
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKFi->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
cv::Mat Tiw = Converter::toCvMat(SE3quat);
cv::Mat Tco_cn = pKFi->GetPose() * Tiw.inv();
cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3);
double dist = cv::norm(trasl);
pKFi->SetPose(Converter::toCvMat(SE3quat));
pKFi->mnNumberOfOpt += numPerform_it;
//cout << "LM-LBA: KF " << pKFi->mnId << " had performed " << pKFi->mnNumberOfOpt << " iterations" << endl;
if(pKFi->mnNumberOfOpt < 10)
{
vpNonEnoughOptKFs.push_back(pKFi);
}
}
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
pCurrentMap->IncreaseChangeIndex();
}