1. LocalBA步骤
- 1 将当前关键帧及其共视关键帧加入lLocalKeyFrames
- 2 遍历 lLocalKeyFrames 中(一级)关键帧,将它们观测的MapPoints加入到lLocalMapPoints
- 3 得到能被局部MapPoints(被)观测到,但不属于局部关键帧的(二级)关键帧,这些关键帧在局部BA优化时不优化
- 4 构造g2o优化器,按照步骤来就行了
- 5 添加待优化的位姿顶点:Pose of Local KeyFrame
- 6 添加不优化的位姿顶点:Pose of Fixed KeyFrame,注意这里调用了vSE3->setFixed(true) // 不优化为啥也要添加?回答:为了增加约束信息
- 7 添加待优化的3D地图点顶点
- 8 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
- 9 开始优化。分成两个阶段:第一阶段优化,迭代5次;检查是否外部请求停止, 如果有外部请求停止,那么就不在进行第二阶段的优化,即下面一步
- 10 检测outlier,并设置下次不优化
- 11 排除误差较大的outlier后再次优化 -- 第二阶段优化
- 12 在优化后重新计算误差,剔除连接误差比较大的关键帧和MapPoint
- 13 优化后,更新关键帧位姿以及地图点的位置、平均观测方向等属性
参考:https://github.com/electech6/ORB_SLAM2_detailed_comments
2. ORB_SLAM2中的示例
/*
* @brief Local Bundle Adjustment
*
* 1. Vertex:
* - g2o::VertexSE3Expmap(),LocalKeyFrames,即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
* - g2o::VertexSE3Expmap(),FixedCameras,即能观测到LocalMapPoints的关键帧(并且不属于LocalKeyFrames)的位姿,在优化中这些关键帧的位姿不变
* - g2o::VertexSBAPointXYZ(),LocalMapPoints,即LocalKeyFrames能观测到的所有MapPoints的位置
* 2. Edge:
* - g2o::EdgeSE3ProjectXYZ(),BaseBinaryEdge
* + Vertex:关键帧的Tcw,MapPoint的Pw
* + measurement:MapPoint在关键帧中的二维位置(u,v)
* + InfoMatrix: invSigma2(与特征点所在的尺度有关)
* - g2o::EdgeStereoSE3ProjectXYZ(),BaseBinaryEdge
* + Vertex:关键帧的Tcw,MapPoint的Pw
* + measurement:MapPoint在关键帧中的二维位置(ul,v,ur)
* + InfoMatrix: invSigma2(与特征点所在的尺度有关)
*
* @param pKF KeyFrame
* @param pbStopFlag 是否停止优化的标志
* @param pMap 在优化后,更新状态时需要用到Map的互斥量mMutexMapUpdate
* @note 由局部建图线程调用,对局部地图进行优化的函数
*/
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
// 该优化函数用于LocalMapping线程的局部BA优化
// Local KeyFrames: First Breadth Search from Current Keyframe
list<KeyFrame*> lLocalKeyFrames;
// Step 1 将当前关键帧及其共视关键帧加入lLocalKeyFrames
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
// 找到关键帧连接的共视关键帧(一级相连),加入lLocalKeyFrames中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
KeyFrame* pKFi = vNeighKFs[i];
// 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId,防止重复添加
pKFi->mnBALocalForKF = pKF->mnId;
// 保证该关键帧有效才能加入
if(!pKFi->isBad())
lLocalKeyFrames.push_back(pKFi);
}
// Local MapPoints seen in Local KeyFrames
// Step 2 遍历 lLocalKeyFrames 中(一级)关键帧,将它们观测的MapPoints加入到lLocalMapPoints
list<MapPoint*> lLocalMapPoints;
// 遍历 lLocalKeyFrames 中的每一个关键帧
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
// 取出该关键帧对应的地图点
vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
// 遍历这个关键帧观测到的每一个地图点,加入到lLocalMapPoints
for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(!pMP->isBad()) //保证地图点有效
// 把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId
// mnBALocalForKF 是为了防止重复添加
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
} // 判断这个地图点是否靠谱
} // 遍历这个关键帧观测到的每一个地图点
} // 遍历 lLocalKeyFrames 中的每一个关键帧
// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
// Step 3 得到能被局部MapPoints观测到,但不属于局部关键帧的(二级)关键帧,这些关键帧在局部BA优化时不优化
list<KeyFrame*> lFixedCameras;
// 遍历局部地图中的每个地图点
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 观测到该MapPoint的KF和该MapPoint在KF中的索引
map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
// 遍历所有观测到该地图点的关键帧
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
// pKFi->mnBALocalForKF!=pKF->mnId 表示不属于局部关键帧,
// pKFi->mnBAFixedForKF!=pKF->mnId 表示还未标记为fixed(固定的)关键帧
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
{
// 将局部地图点能观测到的、但是不属于局部BA范围的关键帧的mnBAFixedForKF标记为pKF(触发局部BA的当前关键帧)的mnId
pKFi->mnBAFixedForKF=pKF->mnId;
if(!pKFi->isBad())
lFixedCameras.push_back(pKFi);
}
}
}
// Setup optimizer
// Step 4 构造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);
// LM大法好
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// 外界设置的停止优化标志
// 可能在 Tracking::NeedNewKeyFrame() 里置位
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
// 记录参与局部BA的最大关键帧mnId
unsigned long maxKFid = 0;
// Set Local KeyFrame vertices
// Step 5 添加待优化的位姿顶点:Pose of Local KeyFrame
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==0);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// Set Fixed KeyFrame vertices
// Step 6 添加不优化的位姿顶点:Pose of Fixed KeyFrame,注意这里调用了vSE3->setFixed(true)
// 不优化为啥也要添加?回答:为了增加约束信息
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;
}
// Set MapPoint vertices
// Step 7 添加待优化的3D地图点顶点
// 边的数目 = pose数目 * 地图点数目
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeMono;
vpMapPointEdgeMono.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);
// 遍历所有的局部地图中的地图点
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()));
// 前面记录maxKFid的作用在这里体现
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
// 因为使用了LinearSolverType,所以需要将所有的三维点边缘化掉
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
// 观测到该地图点的KF和该地图点在KF中的索引
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
// Set edges
// Step 8 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
// 遍历所有观测到当前地图点的关键帧
for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad())
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
// 根据单目/双目两种不同的输入构造不同的误差边
// Monocular observation
// 单目模式下
if(pKFi->mvuRight[mit->second]<0)
{
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZ* e = new g2o::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->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
}
else // Stereo observation
{
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKFi->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
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];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
e->bf = pKFi->mbf;
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKFi);
vpMapPointEdgeStereo.push_back(pMP);
}
}
} // 遍历所有观测到当前地图点的关键帧
} // 遍历所有的局部地图中的地图点
// 开始BA前再次确认是否有外部请求停止优化,因为这个变量是引用传递,会随外部变化
// 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位
if(pbStopFlag)
if(*pbStopFlag)
return;
// Step 9 开始优化。分成两个阶段
// 第一阶段优化
optimizer.initializeOptimization();
// 迭代5次
optimizer.optimize(5);
bool bDoMore= true;
// 检查是否外部请求停止
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
// 如果有外部请求停止,那么就不在进行第二阶段的优化
if(bDoMore)
{
// Check inlier observations
// Step 10 检测outlier,并设置下次不优化
// 遍历所有的单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
// 基于卡方检验计算出的阈值(假设测量有一个像素的偏差)
// 自由度为2的卡方分布,显著性水平为0.05,对应的临界阈值5.991
// 如果 当前边误差超出阈值,或者边链接的地图点深度值为负,说明这个边有问题,不优化了。
if(e->chi2()>5.991 || !e->isDepthPositive())
{
// 不优化
e->setLevel(1);
}
// 第二阶段优化的时候就属于精求解了,所以就不使用核函数
e->setRobustKernel(0);
}
// 对于所有的双目的误差边也都进行类似的操作
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
// 自由度为3的卡方分布,显著性水平为0.05,对应的临界阈值7.815
if(e->chi2()>7.815 || !e->isDepthPositive())
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
// Optimize again without the outliers
// Step 11:排除误差较大的outlier后再次优化 -- 第二阶段优化
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
vector<pair<KeyFrame*,MapPoint*> > vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
// Check inlier observations
// Step 12:在优化后重新计算误差,剔除连接误差比较大的关键帧和MapPoint
// 对于单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::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));
}
}
// 双目误差边
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
if(e->chi2()>7.815 || !e->isDepthPositive())
{
KeyFrame* pKFi = vpEdgeKFStereo[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// Get Map Mutex
unique_lock<mutex> lock(pMap->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);
}
}
// Recover optimized data
// Step 13:优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性
//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKF = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
pKF->SetPose(Converter::toCvMat(SE3quat));
}
//Points
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();
}
}