从高斯牛顿法到ORB-SLAM2：从公式角度解析g2o中的自定义边和顶点

0 文章导读

在 SLAM 技术中，优化问题无处不在，g2o 作为一个流行的优化框架，在许多相关的算法中都有使用，当我们想基于这些工作来实现自己的 idea 的时候，往往离不开自定义 g2o 边和顶点。本文希望以高斯牛顿法在优化问题中的应用为出发点，逐步阐述分享一下我对于自定义 g2o 的边和顶点的理解（本文结合 ORB-SlAM2 的顶点 VertexSE3Expmap 与二元边 EdgeSE3ProjectXYZ 进行讲解）。

本文假设读者已有一定的 g2o 基础，目的是通过具体的分析 ORB-SlAM2 的 g2o 库中位姿的顶点以及重投影误差边的实现，让读者能更深入地理解这些技术，并且能够了解怎么将不一样的场景中的优化问题通过自定义 g2o 的边和顶点解决。

如果大家发现文章有什么错误的地方，欢迎指正！

0.1 文章结构概览

本文将通过以下几个部分阐述：

1. 高斯牛顿法回顾
   • 主要公式
   • 步骤
2. 自定义边
   • 误差函数 computeError()
   • 雅各比函数 linearizeOplus()
3. 自定义顶点
   • 理解 oplusImpl 函数
4. 小结
5. 再举个例子巩固一下

1 高斯牛顿法回顾

由于本文是以高斯牛顿法为出发点来理解 g2o 的边与顶点，所以这里按照《视觉 SLAM 十四讲》第 129 页稍微回顾一下高斯牛顿法的步骤与公式，接下来的内容将会以高斯牛顿法的步骤为框架一步一步讲解。

主要公式：
高斯牛顿法最核心的公式如下，其中 $J(x)$ 为当前迭代的雅可比矩阵，$f(x)$ 为当前迭代的误差

$$\begin{array}{rl}& H\Delta x=g\\ & \\ H(x)& =J(x)J^T(x)\\ & \\ g(x)& =-J(x)f(x)\\ & \end{array}$$

步骤：

1. 确定误差函数 Error，给定初始值 $x_0$
2. 对于第 k 次迭代，根据误差函数 Error，J 求出当前的雅可比矩阵 $J(x_k)$ 和误差 $f(x_k)$
3. 求解增量方程：$H\Delta x_k=g$
4. 若 $\Delta x_k$ 足够小，则停止。否则，令 $x_{k+1}=x_k+\Delta x_k$，返回第二步

有了以上基础，就可以开始 g2o 的理解了。

2 自定义边

当我们要自定义边，核心其实就只有两个函数，computeError 和 linearizeOplus，对应着高斯牛顿法的第一步和第二步（在 g2o 中我们关心的是设定误差函数和设定雅可比矩阵），接下来以二元边 EdgeSE3ProjectXYZ 为例子进行理解。

2.1 误差函数 computeError()

对于误差，应该不需要过多介绍了，它是整个优化问题的核心，并且于函数 computeError() 中定义，实现的重点就是使用顶点类型来构造误差函数 Error ，因为只有利用顶点来构造误差函数，误差函数的计算结果才会随着每次优化迭代而变化。

void computeError()  {
  const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); //取出位姿顶点
  const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);//取出3D点顶点
  Vector2d obs(_measurement);
  _error = obs-cam_project(v1->estimate().map(v2->estimate()));//从顶点->estimate()中取出位姿和3D点
}

如代码所示，只需要通过类内变量 _vertices 取出顶点，然后再通过 顶点->estimata() 的方式取出具体的优化变量数值来构造误差函数 Error 即可。

大致的过程就是这样，但是有个问题需要注意：
在写理论公式的时候，一定要注意，误差是要包含顶点（优化变量）的。这听起来像是句废话，但是在实践的过程却很容易忽略，接下来我使用点到平面距离误差为例子说明：

点到平面距离的误差可以用如下公式表示，其中 $n_w$ 和 $d$ 表示世界坐标系下的平面参数（单位法向量和常数项），$T_{wc}$ 为相机坐标系到世界坐标系的变换矩阵，$p_c$ 为相机坐标系下的 3D 点。

$$Error=|n_w\cdot T_{wc}{p}_c+d|$$

也就是说，这个公式希望将点转换到世界坐标系下计算点到平面的距离。
如果我们希望把这个误差加入到 ORB_SLAM2 优化框架中进行优化的话（使用与其相同的顶点），首先得注意，在 ORB_SLAM2 中，优化变量为 $T_{cw}$，求雅可比的时候自然也是求误差对 $T_{cw}$ 的雅可比，而上述的公式中为 $T_{wc}$，所以我们需要将公式转换成包含 $T_{cw}$ 的式子：

$$Error=|n_w\cdot (T_{cw}{)}^{-1}{p}_c+d|$$

最后一定是通过这个带有顶点 $T_{cw}$ 的函数求雅可比，（对于包含 $T$ 的逆的雅可比计算可以参考 SLAM中李代数的扰动模型如何对逆变换矩阵，或者逆旋转矩阵求导？ - 知乎）。

2.2 雅可比函数 linearizeOplus()

在完成误差函数 Error 的定义以后，对于边的设置我们要做的只剩高斯牛顿法的第二步计算雅可比了。
雅可比的定义在函数 linearizeOplus() 中，与计算误差函数一样，雅可比同样要利用顶点来构造，确保每次迭代的雅可比能够随着优化变量的变化而变化。

void EdgeStereoSE3ProjectXYZ::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3d xyz = vi->estimate();
  Vector3d xyz_trans = T.map(xyz);

  const Matrix3d R =  T.rotation().toRotationMatrix();

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;

  _jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2;
  ...
  _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2;

  _jacobianOplusXj(0,0) =  x*y/z_2 *fx;
   ...
  _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2; }

注意 _jacobianOplusXi 对应顶点 _vertices[0]，_jacobianOplusXj 对应 _vertices[1]。

3 自定义顶点

在完成边的定义以后，相当于完成了误差 Error 和增量方程 $H\Delta x=g$ 的 $H$ 和 $g$ 部分，此时 g2o 内部会根据 $H$ 和 $g$ 计算 $\Delta x$ ，紧接着，高斯牛顿法就进行到步骤 4，对 $x$ 进行更新了，也就是 $x_{k+1}=x_k+\Delta x_k$。
在 g2o 中，对 $x$ 的更新是在顶点 VertexSE3Expmap 中的 oplusImpl 函数实现的，要注意这里对位姿的更新是采用李群左乘实现的，原因是在计算雅可比时，采用的左乘扰动模型。

virtual void oplusImpl(const double* update_)  {
  Eigen::Map<const Vector6d> update(update_);
  setEstimate(SE3Quat::exp(update)*estimate());//注意这里的更新是通过左乘完成的！
}

如果只是为了使用已定义好的顶点的话，了解到这里其实也已经足够了，但如果想要自定义顶点的话，还得再继续深入理解更新公式中的 $\Delta x$ 以及怎么实现增量。

3.1 理解 oplusImpl 函数

大家都知道，由于变换矩阵（李群 $SE3$）对加法不封闭等原因，所以就没办法直接对他们进行求导，也就没办法进行优化了，为了解决这个问题，也就引进了李代数 $se3$。
然而由于李群可以简化变换的表达（以及一些数值上的好处？），所以在 SLAM 系统的其他部分都是采用李群 $SE3$ 来表达变换矩阵 T，只有在优化问题上才会采用李代数 $se3$ 表达， 此时顶点就要涉及到李群和李代数的转换了（因为如果顶点为李群的话，在优化完以后可以直接通过 顶点->estimate() 的方式直接取出李群），那么 g2o 是怎么实现的呢？
答案是通过对顶点类型 SE3Quat 类进行李群和李代数操作的封装（可以将其理解为李群，但是里面带有一些运算的函数），而顶点就是通过更新 SE3Quat 类来实现优化的。有了这些前提，接下来让我们按照更新公式 $x_{k+1}=x_k+\Delta x_k$ 的角度去理解 oplusImpl 函数。

3.1.1 “$x_{k+1}=x_k+\Delta x_k$” 中的 “$\Delta x$”

首先，我们来探究一下 $x_{k+1}=x_k+\Delta x_k$ 中的 “$\Delta x$” 。
参考高博的 SLAM 圣经《视觉 SLAM 十四讲第二版》81 页可以看到，李代数的表现形式为 $\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]$ ，其中 $\rho$ 代表平移（严格意义上说，$J\rho$ 等于平移 t），$\varphi$ 为旋转。我们求雅可比时，也是求的误差对李代数扰动 $\delta \xi$ 的雅可比，而 $\Delta x$ 本质上是由雅可比矩阵指导的一个步长，这个步长的作用是为了减少整个系统的误差，有了 $\Delta x$，我们就可以明确每一步调整的方向和大小，以确保每次迭代我们都朝着减少总误差的方向前进。
所以，此时增量方程中的 $\Delta x$ 就是李代数的扰动，即 $\Delta x=\delta \xi =\left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right](\delta 代表微小扰动)$ ，也就是说，在顶点增量函数 oplusImpl 中更新的 update_ 实际上就是 $\delta \xi =\left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right]$（注意！在 g2o 中对于李代数的定义为先旋转后平移也就是 $\xi =\left[\begin{array}{c}\varphi \\ \rho \end{array}\right]$，与十四讲的先平移后旋转 $\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]$ 定义相反)。 从代码 Eigen::Map<const Vector6d> update(update_); 也可以看出，此时 update_ 被转换成为了一个六维向量，前三维为旋转 $\varphi$，后三维为 $\rho$。

3.1.2 “$x_{k+1}=x_k+\Delta x_k$” 中的 “+”

在了解了 $\Delta x$ 以后，就要考虑如何把这个步长 $\Delta x$ 更新到上一次迭代的变量上。所以接下来，再来研究一下 $x_{k+1}=x_k+\Delta x_k$ 中的“+”是怎么实现的。g2o 在这里采用了使用李群来更新，所以可以看到在代码 setEstimate(SE3Quat::exp(update)*estimate()) 中，先将 update 这个李代数通过指数映射 SE3Quat::exp 转换成了李群，然后再左乘上一次迭代的李群估计值 estimate()（这里对 SE3Quat 类进行了乘法的重载）。如果我们继续查看指数映射的代码：

static SE3Quat exp(const Vector6d & update)
      {
        Vector3d omega;
        for (int i=0; i<3; i++)
          omega[i]=update[i];
        Vector3d upsilon;
        for (int i=0; i<3; i++)
          upsilon[i]=update[i+3];

        double theta = omega.norm();
        Matrix3d Omega = skew(omega);//反对称运算

        Matrix3d R;
        Matrix3d V;
        if (theta<0.00001)
        {
          //TODO: CHECK WHETHER THIS IS CORRECT!!!
          R = (Matrix3d::Identity() + Omega + Omega*Omega);

          V = R;
        }
        else
        {
          Matrix3d Omega2 = Omega*Omega;

          R = (Matrix3d::Identity()
              + sin(theta)/theta *Omega
              + (1-cos(theta))/(theta*theta)*Omega2);

          V = (Matrix3d::Identity()
              + (1-cos(theta))/(theta*theta)*Omega
              + (theta-sin(theta))/(pow(theta,3))*Omega2);
        }
        return SE3Quat(Quaterniond(R),V*upsilon);
      }

可以发现，代码中的 R 和 V 其实就是对应着十四讲 79 的公式 4.22 和 80 页的公式 4.27。

$$\begin{array}{rl}& T(SE3)=exp({\xi }^{\wedge })=\left[\begin{array}{ccc}exp({\varphi }^{\wedge })& & J\rho \\ 0^T& & 1\end{array}\right]\\ & \\ & R=exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }(4.22)\\ & \\ & J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }(4.27)\end{array}$$

其中：

• omega 为 $\varphi$
• upsilon 为 $\rho$
• V 对应着 $J$ 也就是旋转矩阵的泰勒展开
• theta 为 $\varphi$ 的模长 $\theta$
• R 为 $exp({\varphi }^{\wedge })$。

如果各位想推导的话，这里需要知道一个性质 $a{a}^T=a^{\wedge }{a}^{\wedge }+I$，以及在公式中，$a$ 为 $\varphi$ 的长度为 1 的方向向量，而在代码中直接用了 $\varphi$ 来做反对称矩阵 Omega 放入公式计算，所以 Omega 实际上是包含了 $\varphi$ 的长度信息，而 Omega2 则包含了两次长度信息，所以在乘上 Omega/Omega2 之后需要再除以一次/两次 $\varphi$ 的长度信息 theta，这也是代码中会有 theta 两次方甚至三次方的原因。

另外，从以下这部分代码也可以看出，update 前三维为旋转 $\varphi$，后三维为平移 $\rho$

Vector3d omega;
for (int i=0; i<3; i++)
  omega[i]=update[i];
Vector3d upsilon;
for (int i=0; i<3; i++)
  upsilon[i]=update[i+3];

4 小结

根据上面的内容，我们其实可以将变换矩阵优化的问题抽象成为“优化用 A 表达，平时使用时又用 B 表达”的模式，对于这种模式的优化变量我们需要确认以下几件事情

1. 首先确认用什么方式来表达此变量的优化问题，下面称作 A。以及在其他部分使用的表达方式 B
2. 确认好误差 Error 的计算方法，并且填入自定义边中的 computeError 函数中
3. 求出 Error 对 A 的雅可比，填入自定义边的 linearizeOplus 函数中
4. 确认好顶点是否使用 B 表达，并且在此类型中最好包含着 A 与 B 之间的转换函数
5. 使用 A 表达方式作为 $\Delta x$，也就是顶点中的 update_
6. 确认好变量的更新方式，根据上面确认的内容写顶点中的 oplusImpl 函数

5 再举个例子巩固一下

在优化问题中，像变换矩阵这种“优化用 A 表达，平时使用时又用 B 表达”的模式其实有很多，下面通过平面参数的例子来稍微阐述一下对于一个新场景如何自定义 g2o。
对于平面参数 $Plane=\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]$，诸如此类使用 a,b,c,d 四个参数来表达平面的方式叫做 Hesse 形式。
这种形式大家都很熟悉，在 SLAM 中使用 T 变换他也很方便 （$plan{e}^{\prime }=(T^{-1}{)}^T\cdot plane$），但是在优化中，由于它存在过参数化的问题，会导致一些问题（参考：SLAM中面特征的参数化 - 知乎），如果您想要优化平面的话，就需要用球坐标 $Plane=\left[\begin{array}{c}\varphi \\ \theta \\ d\end{array}\right]$ 的形式表达平面。
此时这个问题就可以抽象为像变换矩阵一样的：“优化用 A 表达，平时使用时又用 B 表达”的模式。此时就可以套用小结中的步骤了。

1. 此时的 A 为球坐标 $Plane=\left[\begin{array}{c}\varphi \\ \theta \\ d\end{array}\right]$，B 为 Hesse 形式 $Plane=\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]$
2. 确认好误差为 $Error=f(Plan{e}_{球坐标})$，填入 computeError 函数中
3. 计算雅可比 $\frac{\partial Error}{\partial Plan{e}_{球坐标}}$，填入 linearizeOplus 当中
4. 顶点用自定义类 PLANE 类型，在这个类中包含函数 transfer 将球坐标转换为 Hesse，并且类内变量储存对应的 Hesse 形式的值
5. 使用球坐标作为顶点中的 update_，并且在 oplusImpl 函数中使用 transfer 函数将 update_ 的值转换为 Hesse，再自定义 PLANE 类内函数 UpdateValue 进行增量运算

此时一套自定义的边和顶点就算完成了核心部分，剩下的工作可以参照其他的 g2o 教程进行。