ORB-SLAM2中Optimizer::BundleAdjustment()函数代码阅读总结 #

该函数主要是在Local Mapping线程中对当前关键帧及其共视关键帧进行局部BA优化。优化的目标是最小化重投影误差和IMU测量误差(如果有的话)。

ORBslam中有个文件定义了VertexSE3Expmap，EdgeSE3ProjectXYZ，EdgeSE3ProjectXYZOnlyPose，乍一看还需要自行扩展定义点与边动手翻译这些需要大量数学知识的公式，其实g2o都翻译好了，作者只是把这几个库复制粘贴到同一个文件中。

为什么g2o需要用到指数空间优化 #

假设我们有两个旋转矩阵$\mathbf{R}_1$和$\mathbf{R}_2$: $$ \mathbf{R}_1 = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 \\ \sin\theta_1 & \cos\theta_1 & 0 \\ 0 & 0 & 1 \end{bmatrix}， \quad \mathbf{R}_2 = \begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 \\ \sin\theta_2 & \cos\theta_2 & 0 \\ 0 & 0 & 1 \end{bmatrix} $$ 它们都是绕$z$轴旋转的旋转矩阵，分别旋转了$\theta_1$和$\theta_2$角度。单独看$\mathbf{R}_1$和$\mathbf{R}_2$，它们都满足正交性和行列式为1的约束。单位正交为什么行列式为1？（从几何意义来看，因为在这些单位向量张成的空间中，由这些单位正交向量包围成的"体积"是1，所以行列式为1。从行列式代表这个矩阵的伸缩因子角度解释，旋转矩阵不改变原图形的体积，所以行列式为1。

现在我们将它们相加得到一个新矩阵$\mathbf{R}_s$: $$ \mathbf{R}_s = \mathbf{R}_1 + \mathbf{R}_2 = \begin{bmatrix} \cos\theta_1+\cos\theta_2 & -\sin\theta_1-\sin\theta_2 & 0 \\ \sin\theta_1+\sin\theta_2 & \cos\theta_1+\cos\theta_2 & 0 \\ 0 & 0 & 2 \end{bmatrix} $$ 检查$\mathbf{R}_s$的性质:

行向量的模长: $$ |\mathbf{r}_1| = \sqrt{(\cos\theta_1+\cos\theta_2)^2 + (\sin\theta_1+\sin\theta_2)^2} \ne 1 $$ 列向量的模长: $$ |\mathbf{c}_1| = \sqrt{(\cos\theta_1+\cos\theta_2)^2 + (\sin\theta_1+\sin\theta_2)^2} \ne 1 $$ 行列式的值: $$ \det(\mathbf{R}_s) = 2(\cos\theta_1+\cos\theta_2)^2 \ne 1 $$ 总之，$\mathbf{R}_s$不满足旋转矩阵的约束，不能看做是一个新的旋转矩阵。

这就是为什么在优化过程中，不能直接将旋转矩阵$\mathbf{R}$作为优化变量，因为优化过程中矩阵元素的更新无法保证更新后的矩阵仍然是一个合法的旋转矩阵。

在图优化中，相机位姿通常使用李群$SE3$表示，即旋转+平移。但由于旋转矩阵有正交约束，无法直接进行优化。解决方法是$将SE3$映射到其对应的李代数$se3$，在$se3$上进行优化更新，再将更新量乘到原来的$SE3$上。 g2o的做法则是将$SE3$通过旋转的四元数表示，即将旋转矩阵换成四元数，平移照旧。这样旋转就可以直接优化了，只是优化完之后要重新归一化。

主要步骤 #

将当前关键帧及其共视关键帧的pose作为顶点(vertex)加入优化器。 #

使用g2o::VertexSE3Expmap表示位姿顶点，内部使用李代数表示SE3。
调用Converter::toSE3Quat将马氏距矩阵转换为四元数+平移向量的格式。
设置顶点的id，以及是否固定(fix)该顶点。

添加MapPoint作为顶点 #

除了关键帧，LocalBA还会把MapPoint也作为顶点加入到优化中。步骤如下:

对于每个MapPoint，构造一个g2o::VertexSBAPointXYZ表示3D点的顶点
使用pMP->GetWorldPos()获取3D点坐标，并转换成g2o::Vector3d类型
顶点的id为对应的MapPoint id + maxKFid + 1
将该顶点设置为边缘化的，因为MapPoint不是主要优化对象，优化中会被边缘化掉以保持稀疏性
将顶点加入优化器

这一步相当于告诉优化器除了关键帧位姿，3D点的坐标也是需要优化的。

添加重投影误差边 #

LocalBA最主要的误差项是MapPoint在关键帧中的重投影误差。对于每一个MapPoint:

遍历所有能观测到该点的关键帧(由pMP->GetObservations()得到)
对于每个关键帧，构造一个重投影误差边(单目为g2o::EdgeSE3ProjectXYZ，双目为g2o::EdgeStereoSE3ProjectXYZ)
边连接MapPoint顶点和关键帧顶点
边的观测值为MapPoint在该关键帧中的特征点坐标(单目为x，y;双目为x，y，disparity)
设置边的信息矩阵，即各维度上的误差权重
如果使用Huber核函数，则设置对应的鲁棒核
设置相机内参
将边加入优化器

这些重投影误差边的作用就是，优化后的3D点和关键帧位姿，尽可能的使重投影误差最小。

优化与更新 #

调用optimizer.initializeOptimization()对Problem进行初始化，再调用optimizer.optimize(nIterations)启动迭代优化过程，优化完成后再调用optimizer.vertex(id)提取出优化后的结果，并赋值给对应的关键帧位姿和3D点坐标。

至此，LocalBA的核心流程就是:

添加关键帧位姿顶点
添加3D点顶点
添加重投影误差边
调用优化器迭代优化
从优化结果中提取并更新位姿和3D点

g2o相关概念 #

g2o::VertexSE3Expmap
- 表示SE3位姿的顶点，内部使用李代数(即se3)表示。
- 优化过程中，顶点的更新量是在se3上进行的，然后左乘到当前估计值上得到新的位姿。
g2o::EdgeSE3ProjectXYZ
- 表示单目相机重投影误差的边，即将MapPoint通过相机投影后与实际观测点的像素坐标误差。
- 误差定义为:$e = obs - K*exp(xi^{wedge})*Xc$，其中obs为观测像素坐标，K为相机内参，xi为相机位姿的se3更新量，Xc为MapPoint在相机坐标系下的坐标。
g2o::EdgeStereoSE3ProjectXYZ
- 表示双目相机重投影误差的边，与单目类似，只是投影函数略有不同。
- 误差定义为:$e = obs - pi(K*exp(xi^{wedge})*Xc)$，其中pi为双目相机投影函数。