在ORB-SLAM系统中，优化并非只存在于所谓的“后端”，只有全局优化。事实上，正如在这篇博客中提到的：优化无处不在，在Tracking、LocalMapping、LoopClosing都有涉及。
本篇博客我们以ORB-SLAM2为例(因为ORB-SLAM3中函数内容也是一样的，但ORB-SLAM2系统结构更简单一些、方便理解)，重点分析前端Tracking和LocalMapping中两个重要的位姿优化函数：PoseOptimization()和LocalBundleAdjustment()。这两个函数都是Optimizer类的成员函数，在Optimizer.h中声明、Optimizer.cc中定义。如果用一句话来概括这两个函数就是：PoseOptimization()主要用于单帧的位姿优化，通过当前帧关联的地图点优化当前帧位姿；LocalBundleAdjustment()则是构造一个局部观测图，同时优化这个局部图中的地图点和关键帧位姿。
在之前的这篇、这篇、这篇和这篇博客中，我们已经稍微介绍了ORB-SLAM系统中的单帧位姿优化函数PoseOptimization()。它主要在前端Tracking中的TrackWithMotionModel()、TrackReferenceKeyFrame()和TrackLocalMap()函数被调用，所以对于Tracking前端而言，这是一个很重要的函数。它只优化当前帧位姿，不优化当前帧关联的地图点。
而在之前的这篇、这篇和这篇博客中，提到了LocalBundleAdjustment()。它主要在前端的LocalMapping中的Run()函数中被调用。它会尝试同时优化地图点与关键帧位姿。
本篇博客我们就进入这两个函数内部，进一步分析一下它到底做了什么。
1.PoseOptimization()
1.1 输入是什么
如下可以看到，函数的输入其实非常简单，就是一个Frame类对象pFrame而已。
但事实上没有那么简单，函数中调用了很多Frame的成员变量或者函数。我们可以在函数里找pFrame在哪里被调用了，有哪些成员变量被访问了。经过查找，访问了如下Frmae类的成员变量或者函数。
- (1)
pFrame->mTcw: 获取当前帧的位姿，作为每次优化的初值 - (2)
pFrame->N: 获取当前帧提取的ORB特征点的个数 - (3)
pFrame->mvpMapPoints[i]: 根据索引，获取当前帧关联的某个MapPoint类型的地图点 - (4)
pFrame->mvuRight[i]: 根据索引，获取某个地图点对应的右影像上的横坐标(针对双目情况，该值默认为-1) - (5)
pFrame->mvbOutlier[i]: 根据索引，获取某个地图点是否是离群点的flag(该值默认为false) - (6)
pFrame->mvKeysUn[i]: 根据索引，获取某个cv::KeyPoint类型的特征点对象 - (7)
pFrame->mvInvLevelSigma2[kpUn.octave]: 根据特征点所在金字塔层数获取对应的sigma值 - (8)
pFrame->fx,fy,cx,cy,mbf: 获取储存在Frame类中的相机内参(双目的话多一个mbf)
以上便是函数中从输入的Frame对象中进一步获取的信息，之后函数便会拿着这些信息构建优化图、执行优化。
1.2 执行了什么
有了上面的数据输入，下面就需要看看函数中拿着这些“信息”做了什么。整个ORB-SLAM系统的优化都是基于图优化的思想，采用G2O实现。所以它也遵循使用G2O优化的一般流程。在之前这篇博客中，我们介绍了G2O的基本使用。主要包含四个步骤：
- (1) 定义顶点和边的类型
- (2) 构建优化图
- (3) 选择优化算法
- (4) 调用g2o优化，返回结果
在整个ORB-SLAM2系统中，没有自定义的顶点或边类型，所以第一步就可以忽略了。在ORB-SLAM3里由于引入了IMU，所以有一些自定义边和节点，都放在了G2OTypes中，之后有空再介绍，这里就不涉及了。下面的2、3、4步骤，则是我们下面要重点介绍的。
1.2.1 优化算法选择
如下图所示，函数中新建了一个稀疏优化器，并且将优化算法设置为Levenberg。
这样对于优化器的基本配置就完成了。对这一块感兴趣的话可以参考这篇博客。
1.2.2 优化图构建
(a) 基本概念
一个优化图自然是由基本的顶点(Vertex)和边(Edge)构成。对于G2O而言，一般情况下顶点为可调整的待优化变量(手动设定为固定的除外)，边为约束或者说误差项，表达顶点之间的关系。利用优化图进行优化的核心思想就是调整一切可调的参数，以尽可能满足约束。
在本函数中，我们的目标是根据地图优化当前帧的位姿。因此我们把当前帧位姿变成一个节点、地图点的观测作为边(约束)。这里优化图中只有一个节点，因此，边就是一个一元边(Unary Edge)，也就是只连接到一个节点(或者理解为从自己指向自己)。
(b) 节点构建
在这里，我们只需要构建一个节点，用于表示当前帧位姿，如下图所示。
可以看到，这里我们获取到了当前帧的位姿mTcw，将其赋给vSE3。我们设置当前节点的ID为0，可以看到，这里我们不固定节点(这样才能调整优化)。节点新建好以后，就将该节点添加到图中。
(c) 边构建
前面说了，我们将对地图点的观测作为边(约束)，由于有多个地图点，所以我们构建vector来存放这些节点，如下所示。
这里可以看到，我们主要有两种类型的边，一种是单目边，另一种是双目边。由于PoseOptimization()函数会在单目、双目中调用，所以代码会根据不同的状态进行判断。什么算单目边或者双目边？如果是单目模式，自然只会有一种单目边。如果是双目模式，如果某个特征点双目匹配成功、有对应的地图点，那么这个地图点就会被构建双目边；如果左目影像中的特征点在右目影像中没有对应，那么就建立单目边。
所以我们依次遍历地图点，构建边。
单目边构建
如下图所示，是构建单目边的代码。
可以看到，在代码中我们通过setVertex()函数将边与刚刚我们建立的节点关联起来。通过setMeasurement()函数设置地图点观测(误差计算其实就是和观测有关的)。然后，我们就可以将该地图边添加到优化图中。并且将这个边对象添加到vpEdgesMono中，并且将当前边对应的索引添加到vnIndexEdgeMono中。对于单目而言，我们构建的是g2o::EdgeSE3ProjectXYZOnlyPose类型的边，表达将地图点投影到相机坐标系下的相机平面。
双目边构建
双目边其实是类似的，如下所示。
相比于单目的观测，双目的观测是一个三维向量。其它都是类似的。最后将这个边对象添加到优化图，并且将对象添加到vpEdgesStereo中、索引添加到vnIndexEdgeStereo中。构建的是g2o::EdgeStereoSE3ProjectXYZOnlyPose类型的边。
至此，我们就完成了对于优化图的构建。一个构建好的优化图示意如下图所示。
图中橙色的为待优化节点，蓝色线为单目边，绿色线为双目边。
1.2.3 迭代优化
构造好优化图以后，就开始迭代优化，如下图所示。
在ORB-SLAM系统中，可以看到，从大的层面来说，会执行4次优化，每次优化包含10次迭代。在每次优化后，都会分别针对单目边和双目边的结果对地图点的外点flag设为true或false。这个过程其实是很重要的，因为这直接决定了经过优化以后会保留多少内点。而内点的个数又会作为评判优化是否成功的唯一指标，返回给Tracking线程。所以如果没有异常的情况下，优化失败了，那么基本就是因为优化以后保留的内点个数太少。
最后，返回优化以后的位姿结果给输入帧，结束优化，如下图所示。
1.3 输出是什么
执行完了优化以后，这个函数会输出什么呢？如果从“字面”上来看，会返回过滤后内点的个数，如下。
但显然，以C++的习惯和这个函数的目的，并不会只有这一个返回值，一些返回值会直接赋给Frame对象。我们可以在函数中寻找，如下。
- (1)
pFrame->mvbOutlier[i] = false/true: 根据地图点边的误差大小判断该点是否属于外点，如果是的话就设为true，否则设为false - (2)
pFrame->SetPose(pose): 将优化更新后的当前帧位姿重新赋给当前帧
所以可以看到，PoseOptimization()函数会范围三个东西：第一个是筛选以后地图内点的个数；第二个是哪些地图点是内点哪些是外点；第三个是更新以后的位姿。
2.LocalBundleAdjustment()
2.1 输入是什么
从字面上来看，LocalBundleAdjustment()函数的输入有三个：当前关键帧KeyFrame，是否停止的flagpbStopFlag以及当前地图pMap，如下图所示。
当然了，事实上显然不会这么简单。和PoseOptimization()函数一样，通过对成员变量或者函数的调用，进一步访问了其它变量。这里我们也把它们找出来。
2.1.1 关键帧相关
- (1)
pKF->mnId: 获取当前关键帧的ID -
(2)
pKF->GetVectorCovisibleKeyFrames(): 获取和当前关键帧共视的其它关键帧，保存在vector类型的vNeighKFs中。最终vNeighKFs中的相邻共视关键帧连同输入函数的当前关键帧pKF一起，被添加到list类型的lLocalKeyFrames中，如下图所示。
之后，函数就会遍历lLocalKeyFrames中的关键帧进行后续操作。为了详细分析，我们也找到了相关调用，列举如下。 - (3)
(lit)->GetMapPointMatches(): 迭代lLocalKeyFrames，依次获取每个关键帧关联的地图点，并将获取到的地图点全部放到list类型的lLocalMapPoints中 - (4)
pKFi->GetPose(): 迭代lLocalKeyFrames，依次获取每个关键帧的位姿，作为优化的初值 - (5)
pKFi->mvKeysUn[mit->second]: 对于每个地图点，遍历它的观测，然后获取某个观测到它的关键帧中，该地图点对应的特征点(可能有点绕，需要理解一下，也可以参考这篇博客，了解一下地图点、观测、关键帧之间的基本关系，可能就更好理解了) - (6)
pKFi->mvuRight[mit->second]: 获取某个地图点对应的特征点的右目影像中的横坐标(默认为-1)，计算公式为kpU.pt.x-mbf/d - (7)
pKFi->mvInvLevelSigma2[kpUn.octave]: 获取某个地图点对应的特征点所在金字塔那一层的尺度信息 - (8)
pFrame->fx,fy,cx,cy,mbf: 获取储存在Frame类中的相机内参(双目的话多一个mbf) - (9)
pKFi->mnBALocalForKF: 获取执行当前局部BA的关键帧ID(也就是输入函数的那个关键帧ID) - (10)
pKFi->mnBAFixedForKF: 获取执行局部BA的固定关键帧ID
2.1.2 地图与地图点相关
- (1)
pMap->mMutexMapUpdate： 由于是BA，也就是同时优化关键帧位姿和地图点，所以很自然的，当优化结束以后，除了需要更新关键帧位姿，还需要更新地图点。由于是多线程系统，为了避免线程冲突(多个线程同时对一个变量进行修改)，需要在更新地图点之前来个线程锁
直接传入的地图指针pMap只在函数中这一个地方被调用了。那你可能会有疑问，不是局部BA吗？那些地图点是在哪里被传进来的呢？答案是通过关键帧。还记得上面关键帧相关部分的第三点，我们就是获取了某个关键帧相关联的地图点，如下图所示。
为了细致地分析，这里也列出对于地图点的相关调用，如下。
- (2)
pMP->isBad(): 返回某个地图点的状态，是否是坏的 - (3)
pMP->mnBALocalForKF: 获取执行当前局部BA的关键帧ID(也就是输入函数的那个关键帧ID) - (4)
(lit)->GetObservations(): 获取某个地图点所对应的观测，地图点观测的格式在之前的这篇博客中已经介绍过了。简单来说就是关键帧+该地图点对应的特征点在该关键帧中的索引。
至此，我们就基本把输入分析完了。可以看到，所有的输入，基本都进了两个list类型变量: lLocalKeyFrames、lLocalMapPoints。分别用于存放关键帧和地图点。在之后的优化图构建中，也主要是遍历这两个变量。
2.2 执行了什么
在有了关键帧与地图点之后，下面要做的就是构造优化图。由于还是使用G2O，所以整体流程还是和上面一样的。我们还是核心关注优化算法选择、优化图构建、迭代优化和输出步骤。
2.2.1 优化算法选择
如下图所示，构造了一个优化器。
我们使用Levenberg算法。
2.2.2 优化图构建
(a) 数据准备
对于局部BA，我们首先介绍涉及的数据，可以分为两类：关键帧与地图点。关键帧又可以进一步分为：直接相邻关键帧和间接相邻关键帧(这个名字是自己起的)。所谓直接相邻关键帧可以理解为两个关键帧之间有较多的共视关系，可以看作是一种“直接关联”；而间接相邻关键帧可以理解为两个关键帧之间没有明显的共视关系，但是它们确实共同观测到了某些地图点(或者换句话说这个地图点同时被这两个关键帧观测到了)，它们之间有关系，但又不够强烈，可以看做是一种“间接关联”。回到正题，数据准备整体的运行逻辑可以描述如下：一切的一切从输入的关键帧说起，首先，我们获取到输入关键帧的所有直接相邻关键帧。然后把输入关键帧和直接相邻关键帧放到一个vector中。然后我们遍历这个vector中所有的关键帧所对应的地图点，就可以得到一个局部地图点vector。最后，我们遍历这个局部地图点vector，每一个地图点都会有多个观测，遍历所有观测，如果有没添加到直接相邻关键帧vector的关键帧，我们再将这些关键帧添加到间接相邻关键帧vector中。这便是数据准备过程的大体流程，如下图所示。
对于这部分的关系，我们也画了个图，如下所示。
图中橙色的为输入关键帧(KF4)，它有两个共视关键帧(KF3、KF5, 黄色)，它们共同观测到了7个地图点(MP1-7, 绿色点)。而对于这每一个地图点，它又有可能被其它非共视关键帧观测到。比如MP1、MP2、MP4又同时被KF2观测到；MP7又同时被KF6观测到。因此，我们把KF2和KF6也加入到优化图中。因此优化图中共有KF2-6四个关键帧节点(KF2、KF6位姿固定，KF3-5可优化)，MP1-7七个地图点节点(全部可优化)。实际构建的优化图和上面的示意图是基本一致的，只是有两处不同。一是没有图中虚线的部分，二是没有关键帧之间的连接箭头(因为实际优化图中，关键帧之间是没有约束的(这点很重要，也是和ORB-SLAM3中视觉IMU联合优化的显著区别之一)，图中只是为了展示运动的效果才画了蓝色的箭头)。
下面再稍微细节地描述一下这个过程。首先我们将输入的关键帧pKF添加到lLocalKeyFrames中。然后把输入关键帧的mnBALocalForKF属性(默认为0)设为它自己的关键帧ID(mnId)。然后，下面就开始针对关键帧和地图点分别处理了。
如上图所示，对于直接相邻关键帧，我们首先利用KeyFrame的成员函数GetVectorCovisibleKeyFrames()获得与输入关键帧共视的其它多个关键帧，将它们暂时存到相邻关键帧列表vNeighKFs中。然后，我们遍历这个vector，将所有相邻关键帧的mnBALocalForKF属性设为输入函数的关键帧ID。然后再做个判断，当前遍历的关键帧状态好不好，如果好的话，就添加到lLocalKeyFrames中。这样，构造优化图所要用到的直接相邻关键帧就准备好了。
如上图所示，对于地图点，我们依次遍历lLocalKeyFrames中的关键帧，利用KeyFrame的成员函数GetMapPointMatches()获取当前帧关联的地图点，暂时放到vpMPs中。然后遍历这个vector，进行一系列的判断。如果地图点不为空、而且状态是好的，而且该地图点的mnBALocalForKF属性不等于输入关键帧的ID，那就把这个地图点添加到lLocalKeyFrames中，并将该地图点的mnBALocalForKF属性设为输入关键帧的ID。可以看到整体逻辑就是：根据输入关键帧找到它的所有直接相邻关键帧、然后逐个遍历关键帧；对于每一个关键帧再逐个遍历它关联的地图点，如果某个地图点满足一定条件，就把它加到局部地图点vector中。
如上图所示，对于间接相邻关键帧，我们遍历添加好的所有局部地图点。每个地图点一般都会有多个观测(也就是被多个关键帧观测到)。我们利用MapPoint类的成员函数GetObservations()获得这些关键帧。然后我们逐个遍历这些关键帧：如果这些关键帧的mnBALocalForKF属性(默认为0)不等于输入帧ID，并且mnBAFixedForKF也不等于输入帧ID，就现将这个关键帧的mnBAFixedForKF设为输入帧ID。最后判断关键帧是不是好的，如果是好的，就把它添加到list类型的局部相邻关键帧lFixedCameras中。
至此，数据准备阶段就做完了。我们主要做了三件事：(1) 将输入关键帧以及直接相邻关键帧存到了lLocalKeyFrames中；(2) 根据直接相邻关键帧获取局部地图点并存到lLocalMapPoints中；(3) 根据局部地图点寻找间接相邻关键帧并存到lFixedCameras中。之后我们就可以基于这些数据构造优化图了，如下图所示。
下面分别介绍。
(b) 节点构建
根据上面的分析，主要有直接相邻关键帧、间接相邻关键帧、地图点三种类型。不同类型需要分别构建节点。
直接相邻关键帧节点
如下图所示，为构建直接相邻关键帧节点的过程。
可以看到，我们遍历lLocalKeyFrames，然后利用GetPose()函数获取关键帧的位姿，作为节点的初始值。然后获取关键帧ID作为节点ID。同时判断关键帧ID是否等于0，如果为0，那么这个节点就被设为Fixed，也就是在优化时不会调整这个节点。它的实际意义就是我们不动第一个关键帧的位姿。设置完成以后，就将节点添加到优化器中。
间接相邻关键帧节点
如下图所示，为构建间接相邻关键帧节点的过程。
可以看到，类似的，我们遍历lFixedCameras，然后获取其位姿、ID。这里不同的是，所有间接相邻关键帧节点都被Fix住了。也就是说在优化时不会修改它们的值，而是把它们作为优化直接相邻关键帧位姿的一种参考。所以从这里也可以看出，虽然LocalBundleAdjustment()会优化局部关键帧位姿，但事实上只优化了直接相邻关键帧的位姿，没有改变间接相邻关键帧，这点需要注意一下。
地图点节点
在构建完关键帧节点以后，下面就要构建地图点节点了，如下图所示。
我们遍历局部地图点列表lLocalMapPoints，先依次获得该地图点在世界坐标系下的坐标(通过GetWorldPos()成员函数)，再将其设为待优化的初值。然后设置节点ID、以及是否边缘化。最终就把所有的地图点都以节点的形式添加到了优化图中。
(c) 边构建
在构建完节点之后，下面自然就要构建边，也就是节点之间的约束了。这个过程其实是通过遍历局部地图点列表lLocalMapPoints完成的，如下图所示。
对于遍历的每个局部地图点，我们首先获取到它的所有观测。然后遍历每个观测，获得每个观测所对应的关键帧。如果这个关键帧状态好的话，就尝试添加边，否则就什么都不做。根据传感器类型不同，分别构建单目和双目边。
单目边构建
如下图所示，是单目边构建过程。
它对应的是单目模式下的特征点或者双目模式下左目影像中双目匹配不成功的点。我们新建一个g2o::EdgeSE3ProjectXYZ类型的边。然后分别设置边的两个顶点，一边连接地图节点，一边连接观测它的那个关键帧节点。再设置边的观测为一个2×1的矩阵。以及根据地图点在关键帧中对应的特征点所在的金字塔层级获得尺度信息，根据尺度信息计算并设置信息矩阵。所谓信息矩阵简单来说是协方差矩阵的逆。然后，为了更加鲁棒，给边添加了Huber核。最后，是一些比较容易理解的操作，将相机参数传给边。最终，将这个边添加到图中，并且在相应的vector中另存一份。
双目边构建
如下图所示，是双目边构建过程。
构建过程和单目边是类似的。不同之处在于双目边的观测是一个3×1的矩阵。多出来的一个数是从KeyFrame的mvuRight中获得的。最终将构造好的边放到优化图中。
至此，边的构建也完成了。我们也完成了整个优化图的构建，下面就是实质上的优化步骤了。
2.2.3 迭代优化
迭代优化的过程其实非常简单，如下所示。
首先会根据传入的pbStopFlag进行判断，如果要求停止，就直接返回，不然就继续下去。可以看到，代码中优化迭代次数被设为5，也就是优化5次。然后新建了一个bDoMore变量，默认为true。如果这个时候接收到了pbStopFlag停止信号，那么就不再继续优化。否则，会再做10次优化。但在优化前，我们会先根据前5次优化的结果，把一些离群地图点去掉，对于单目、双目都会进行，如下图所示。
简单来说，会获取每个边的误差，如果误差大于一定阈值，就把这个边的level设为1(调用边的setLevel()成员函数)、鲁棒核为0(调用边的setRobustKernel()成员函数)。
然后，我们会重新开启优化器。这个地方需要注意，在initializeOptimization()函数中，我们传入了0。这个参数的意义是我们要优化什么级别的节点(默认不写就是优化所有节点)。经过上面的过程，离群点节点的level就为1，合群点的level则为0。也就是说，在后面的10次优化中，我们就不会再考虑被过滤掉的离群点了。
最终，我们将更新以后的直接相邻关键帧位姿和地图点坐标重新赋给关键帧和地图点，大功告成，如下。
我们根据索引，调用Optimizer的成员函数vertex()依次取回优化后节点，再调用节点的estimate()函数获得优化后的数值。最终，调用KeyFrame的SetPose()以及MapPoint的SetWorldPos()设置关键帧位姿和地图点位置。
2.3 输出是什么
看完了上面的流程，LocalBundleAdjustment()函数到底输出了什么就比较清晰了。如果只从字面来说，这个函数没有返回值。但事实上，它更新了直接相邻关键帧位姿和地图点坐标，分别是通过KeyFrame的SetPose()以及MapPoint的SetWorldPos()函数完成。我们可以简单看一下这两个函数如下。
对于SetPose()，函数会将输入的变换矩阵进行分解，得到旋转和平移部分，进而更新一些其它位姿相关变量。
对于SetWorldPos()，函数会直接将传入的矩阵以深拷贝的形式赋给MapPoint的mWorldPos成员变量。
3.参考资料
- [1] https://blog.csdn.net/weixin_43205582/article/details/98184719
- [2] https://blog.csdn.net/moyu123456789/article/details/105097980
- [3] https://blog.csdn.net/shyjhyp11/article/details/115437485
本文作者原创,未经许可不得转载,谢谢配合
