前言
从顶点和边的类型来看，顶点和边的定义与局部建图线程中的局部地图优化函数LocalBundleAdj ustment 是一样的。边的误差类型也是一样的，只不过这里使用的是全局地图，而不是局部地图。
相关链接：
ORB-SLAM2源码学习：Optimizer.cc：Optimizer::LocalBundleAdjustment局部建图线程：局部BA-CSDN博客
1.函数声明
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
可设置迭代的次数nIterations，在局部BA时默认为5。
2.函数定义
具体流程如下：
1.初始化g2o 优化器。
2.向优化器中添加顶点： 所有的关键帧位姿和所有的地图点。
3.向优化器中添加边，这里的边就是地图点和观测到它的关键帧的投影关系。在单目相机模式下和双目相机模式下有所不同。
4.开始优化，迭代nIterations次。
5.将优化结果保存起来。注意，这里没有直接原位替换更新，而是将更新的关键帧位姿和地图点分别保存在变量mTcwGBA 和mPosGBA 中。
/*bundle adjustment 优化过程vpKFs 参与BA的所有关键帧vpMP 参与BA的所有地图点nIterations 优化迭代次数pbStopFlag 外部控制BA结束标志nLoopKF 形成了闭环的当前关键帧的id
bRobust 是否使用核函数*/
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{// 不参与优化的地图点vector<bool> vbNotIncludedMP;vbNotIncludedMP.resize(vpMP.size());// Step 1 初始化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);// 如果这个时候外部请求终止，那就结束// 注意这句执行之后，外部再请求结束BA，就结束不了了if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);// 记录添加到优化器中的顶点的最大关键帧idlong unsigned int maxKFid = 0;// Step 2 向优化器添加顶点// Set KeyFrame vertices// Step 2.1 ：向优化器添加关键帧位姿顶点// 遍历当前地图中的所有关键帧for(size_t i=0; i<vpKFs.size(); i++){KeyFrame* pKF = vpKFs[i];// 跳过无效关键帧if(pKF->isBad())continue;// 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));// 顶点的id就是关键帧在所有关键帧中的idvSE3->setId(pKF->mnId); // 只有第0帧关键帧不优化（参考基准）vSE3->setFixed(pKF->mnId==0);// 向优化器中添加顶点，并且更新maxKFidoptimizer.addVertex(vSE3);if(pKF->mnId>maxKFid)maxKFid=pKF->mnId;}// 卡方分布 95% 以上可信度的时候的阈值const float thHuber2D = sqrt(5.99); // 自由度为2const float thHuber3D = sqrt(7.815); // 自由度为3// Set MapPoint vertices// Step 2.2：向优化器添加地图点作为顶点// 遍历地图中的所有地图点for(size_t i=0; i<vpMP.size(); i++){MapPoint* pMP = vpMP[i];// 跳过无效地图点if(pMP->isBad())continue;// 创建顶点g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();// 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));// 前面记录maxKFid 是在这里使用的const int id = pMP->mnId+maxKFid+1;vPoint->setId(id);// 注意g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。// 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,// 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。// Ceres库则没有这样的限制vPoint->setMarginalized(true);optimizer.addVertex(vPoint);// 取出地图点和关键帧之间观测的关系const map<KeyFrame*,size_t> observations = pMP->GetObservations();// 边计数int nEdges = 0;//SET EDGES// Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）// 遍历观察到当前地图点的所有关键帧for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++){KeyFrame* pKF = mit->first;// 跳过不合法的关键帧if(pKF->isBad() || pKF->mnId>maxKFid)continue;nEdges++;// 取出该地图点对应该关键帧的2D特征点const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];if(pKF->mvuRight[mit->second]<0){// 以下是单目相机模式：// 构造观测Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;// 创建边g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();// 边连接的第0号顶点对应的是第id个地图点e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));// 边连接的第1号顶点对应的是第id个关键帧e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));e->setMeasurement(obs);// 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，// 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差// 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);// 使用鲁棒核函数if(bRobust){g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);// 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，// 核函数是为了避免其误差的平方项出现数值上过大的增长rk->setDelta(thHuber2D);}// 设置相机内参e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;// 添加边optimizer.addEdge(e);}else{// 双目或RGBD相机按照下面操作// 双目相机的观测数据则是由三个部分组成：投影点的x坐标，投影点的y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）Eigen::Matrix<double,3,1> obs;const float kp_ur = pKF->mvuRight[mit->second];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;// 对于双目输入，g2o也有专门的误差边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(pKF->mnId)));e->setMeasurement(obs);// 信息矩阵这里是相同的，考虑的是左目特征点的所在图层const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);// 如果要使用鲁棒核函数if(bRobust){g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);// 由于现在的观测有三个值，重投影误差会有三个平方项的和组成，因此对应的卡方分布的自由度为3，所以这里设置的也是自由度为3的时候的阈值rk->setDelta(thHuber3D);}// 填充相机的基本参数e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;e->bf = pKF->mbf;optimizer.addEdge(e);} } // 向优化器添加投影边,也就是遍历所有观测到当前地图点的关键帧// 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化if(nEdges==0){optimizer.removeVertex(vPoint);vbNotIncludedMP[i]=true;}else{vbNotIncludedMP[i]=false;}}// Optimize!// Step 4：开始优化optimizer.initializeOptimization();optimizer.optimize(nIterations);// Recover optimized data// Step 5：得到优化的结果// Step 5.1 遍历所有的关键帧for(size_t i=0; i<vpKFs.size(); i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;// 获取到优化后的位姿g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();if(nLoopKF==0){// 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现,只能够说明是在创建初始地图点的时候调用的这个全局BA函数.// 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中pKF->SetPose(Converter::toCvMat(SE3quat));}else{// 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用pKF->mTcwGBA.create(4,4,CV_32F);Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);pKF->mnBAGlobalForKF = nLoopKF;}}// Step 5.2 Points// 遍历所有地图点,去除其中没有参与优化过程的地图点for(size_t i=0; i<vpMP.size(); i++){if(vbNotIncludedMP[i])continue;MapPoint* pMP = vpMP[i];if(pMP->isBad())continue;// 获取优化之后的地图点的位置g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));// 和上面对关键帧的操作一样if(nLoopKF==0) {// 如果这个GBA是在创建初始地图的时候调用的话,那么地图点的位姿也可以直接写入pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}else{// 反之,如果是正常的闭环过程调用,就先临时保存一下pMP->mPosGBA.create(3,1,CV_32F);Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);pMP->mnBAGlobalForKF = nLoopKF;}// 判断是因为什么原因调用的GBA} // 遍历所有地图点,保存优化之后地图点的位姿
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。