g2o学习记录（2）官方文档的阅读及理解

g2o学习记录（2）官方文档的阅读及理解

前言

  本文是自己的理解，并不能保证准确性，如果有误，请大家指出。参考文献在后给出，最主要是根据新版本博文(1)中介绍安装的里面的doc进行阅读和使用，期间会穿插旧版本，比如高翔14讲中的g2o旧版本（无unit_test)的g2o的代码测试。本文仅做自己的学习记录，欢迎大家交流讨论。系列文以g2o下的doc为主，其他书籍是辅助作用。
  由于CSDN采用Latex渲染器KATEX，行内公式编号的tag命令不能过多指定，为了保证编号的准确性，我便舍弃了\begin{align}和\end{align}进行公式的优化，也舍弃了我原本在mardown语法中编写公式使用\begin{eqnarray}和\end{eqnarray}嵌套组，请大家阅读的时候知道=号其实是对齐的就成。

g2o描述和介绍

基本定义

  g2o是一个C++框架，用于执行非线性最小二乘问题的优化，它是一个图优化工具。 非线性最小二乘问题又恰恰是由许多误差项和组成，如果把最小二乘以图的形式表示，那么顶点是优化变量，而边是误差项。

SLAM而言的例子

  对于SLAM中使用而言，这里截取两本书的例子，希望有个大致的了解。

图1。GraphSLAM算法示例。摘自Probabilistic Robotics书的338页码。个人不太建议中文版，翻译得水准有失偏颇，术语有些存在不准确性。当然有条件和时间，可以中英文对照阅读。

图2。图优化例子。摘自高翔SLAM十四讲第六章第122页。三角形表示位姿，圆形是图像观察到的路标点。

  我本身不太希望，我写的就像是介绍graph slam，主要是理论部分对我现在来说，只需要了解大概，因为我是使用它而不是深入改造它。

  我们就说图1，对于图1来看，有四个机器人位置和两个地图特征。图中节点，是机器人位姿和特征的位置。比如$x_0,x_1,x_2,x_3,x_4$，以及$m_1,m_2$。实线就是机器人的位姿，虚线是表示在该位姿下看到的特征。对于graph slam来说，它会认为，每一个连接都是二次方非线性约束。这些约束会被表示为测量和运动模型的负对数似然，也被称为信息约束。对于graph slam来说，这些约束加入图后并不重要。

  它最小化的就是下面的这些约束，这构成了一个最小二乘问题。

  因为它要找到在这个约束下，产生最大似然地图和对应的一系列机器人位姿。为什么是最大似然？这种问题我不会过于展开，这在SLAM十四讲会讲到（你可以类比该书的第108页的公式$(6.12)$，求导过程我目前不会展示），因为我们要在已知的测量条件下，求出最大的可能性，概率机器人这本书还是以EKF讲解，所以这里面还涉及了贝叶斯公式，这里先不细说，而且我自己也还没看完，以后有时间再讨论。

  现在我们要知道的是，上面的最小二乘的问题被构建成了图优化问题，你主要是记住，要优化的是节点，误差项是边。节点是我们要估计的，误差项是我们定义的。比如我们估计一系列的数据，是否符号一元二次方程$f(x)=a{x}^2+bx+c$，我们要估计的是$a,b,c$则作为图的节点，而误差项，就是测量值和估量值之间的差值。相当于$y-f(x)$，我们要迭代求解得到这个测量值和估量值之间的差值最小的时候，也就是求到了满足这么多点的一元二次方程的$a,b,c$，这就是一个图求解的问题。

  而在SLAM中，我们要求解的是位姿，然后误差项是重投影误差，或者光度误差或者其他自定义的误差，最主要的思想是利用g2o.ceres-solver等工具，使得求出一组最佳的位姿，使得这些误差最小。

  好的，这些就让我们过了这个话题。我们只需要知道g2o是一个求解工具，它将我们定义的那种误差的最小二乘问题能求解出来，求得符合的最佳位姿就够了。我们主要还是在g2o的了解和使用上，现在我不会去过多考虑g2o由其扩展的Graph SLAM的理论部分。

超图

  在g2o文档中定义了一个超图（Hyper Graph），超图（Hyper Graph）是图的扩展，其中边可以连接多个节点而不仅仅是两个节点，甚至只连接一个都可以，比如下图。

SLAM十四讲，123页，曲线拟合对应的图优化模型。

  对于超边(Hyper Edge)和超图（Hyper Graph）的定义我也不是很喜欢，大体知道它就是一个图，只不过不是一般的图那样，边只能连接两个节点。

  在机器人和计算机视觉中的若干问题需要找到关于一组参数的误差函数的最佳优化。 例子包括SLAM和BA等流行应用。

g2o的目的

• 为了提供易于扩展且易于使用的图形优化通用库，可以轻松应用于不同的问题，
• 为想要了解SLAM或BA的人提供易于阅读的实现，该实现侧重于问题规范的相关细节。
• 尽可能地提供最先进的性能。

（超）图可嵌入优化问题

  Graph-Embeddable在理解起来可能大家和我第一次看的时候差不多想法，这个是什么东西？估计很多人知道Graph Embedding（图嵌入），比如这篇论文Exploring the Semantic Content of Unsupervised Graph Embeddings: An Empirical Study。

  这篇论文把Graph Embedding定义为：图嵌入是一系列机器学习模型，它们学习图中顶点的潜在表示，目的是将矢量空间中没有固有表示的复杂图转换为其元素的低维矢量表示。

  这个意思估计也不太好懂，我觉得这篇博文Graph Embedding 综述描述倒是比较清晰些，定义为：将网络的节点和向量之间建立一个一一对应的关系，并且保留网络的一些拓扑性质。

  这里有一篇关于Graph Embedding的论文综述，也描述了这部分的知识。在机器学习领域最近倒是很喜欢把降维以embedding来指代。

  我觉得这里已经扯远了，作者的意思很可能表达的可嵌入性超图，意味着，我们可以把一个多维度的优化问题转换成图问题，这减少了问题的求解难度（其实感觉意思表达也不错，我们把问题降低了维度一样，使得问题求解变得容易了）。

超图优化问题

  我们知道最小二乘最小化问题可以通过以下等式描述：

$$\mathrm{F}(\mathrm{x})=\sum _{\mathrm{k}\in \mathcal{C}}\underset{{\mathrm{F}}_{\mathrm{k}}}{\underbrace{{\mathrm{e}}_{\mathrm{k}}({\mathrm{x}}_{\mathrm{k}},{\mathrm{z}}_{\mathrm{k}}{)}^{\mathrm{T}}{\mathrm{\Omega }}_{\mathrm{k}}{\mathrm{e}}_{\mathrm{k}}({\mathrm{x}}_{\mathrm{k}},{\mathrm{z}}_{\mathrm{k}})}}\text{\hspace{1em}(1)}$$
$$x^{\ast }={\mathrm{argmin}}_{\mathrm{x}}\mathrm{F}(\mathrm{x})\text{\hspace{1em}(2)}$$

其中：

• $\mathrm{x}=({\mathrm{x}}_1^T,\cdots ,{\mathrm{x}}_n^T{)}^T$是参数向量，其中每一个${\mathrm{x}}_{\mathrm{i}}$表示通用参数块。
• ${\mathrm{x}}_k=({\mathrm{x}}_{k_1}^T,\cdots ,{\mathrm{x}}_{k_q}^T{)}^T\subset ({\mathrm{x}}_1^T,\cdots ,{\mathrm{x}}_n^T{)}^T$是第$k$个约束涉及的参数子集
• ${\mathrm{z}}_k$和${\Omega }_k$分布表示与${\mathrm{x}}_k$中参数相关的约束的均值和信息矩阵
• $e_k(x_k,z_k）$是一个向量误差函数，用于测量${\mathrm{x}}_k$中参数块满足约束${\mathrm{z}}_k$的程度。 当${\mathrm{x}}_k$和${\mathrm{x}}_j$与约束完全匹配时为0。 作为示例，如果具有测量函数${\hat{\mathrm{z}}}_k={\mathrm{h}}_k({\mathrm{x}}_k)$，其在给定${\mathrm{x}}_k$中的节点的实际配置的情况下生成合成测量${\hat{\mathrm{z}}}_k$。 然后直接的(straightforward)误差函数是${\mathrm{e}(\mathrm{x}}_k,{\mathrm{z}}_k)={\mathrm{h}}_k({\mathrm{x}}_k)-{\mathrm{z}}_k$。

为简单起见，定义如下：

$${\mathrm{e}}_k({\mathrm{x}}_k,{\mathrm{z}}_k)\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}.}{=}{\mathrm{e}}_k({\mathrm{x}}_k)\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}.}{=}{\mathrm{e}}_k(\mathrm{x})\text{\hspace{1em}(3)}$$

  请注意，每个参数块和每个误差函数都可以张成(span)不同的空间。 这种形式的问题可以通过有向超图来有效地表示。 图的节点$i$表示参数块${\mathrm{x}}_i\in {\mathrm{x}}_k$，并且节点${\mathrm{x}}_i\in {\mathrm{x}}_k$中的超边表示涉及${\mathrm{x}}_k$中的所有节点的约束。 在超边具有大小为2的情况下（个人理解是边只连接了两个图的节点），超图变为普通图。 下图显示了超图和目标函数之间的映射示例。

摘自g2o的doc文档，说明如何通过超图表示目标函数。

  上图说明一个小SLAM问题的一部分[1]。 在这个例子中，我们假设测量函数由一些未知的校准参数$K$控制。机器人位姿由变量${\mathrm{p}}_{1:n}$表示。 这些变量通过方框所描绘的约束${\mathrm{z}}_{ij}$连接。 例如，通过匹配在激光参考系（the laser reference frame）中的附近激光扫描来产生约束。 然而，激光匹配和机器人位姿之间的关系取决于传感器在机器人上的位置，其由校准参数$K$建模。相反，随后的机器人位姿通过由测距法测量（odometry measurements，实际上也是我们说的视觉里程计）引起的二元约束来连接。 这些测量是在移动机器人为基的坐标系（移动坐标系）所产生的（如果是vSLAM，你可以看成是相机拍摄到的物体的以相机坐标系得到的路标测量值）。

最小二乘优化

  如果已知参数的良好初始猜测 $\breve{\mathrm{x}}$，则公式2的数值解可以通过使用流行的Gauss-Newton（高斯牛顿）或Levenberg-Marquardt（莱文贝格-马夸特方法）算法获得。

作者推荐阅读numerical recipes的第二版15.5部分了解LM算法，不过目前已经出现第三版，而中文版C++版本也有源码可以下。

  这个想法【指的GN和LM】是通过围绕当前初始猜测 $\breve{\mathrm{x}}$的一阶泰勒展开来近似误差函数：

$$e_k({\breve{\mathrm{x}}}_k+\Delta x_k)=e_k(\breve{x}+\Delta x)\text{\hspace{1em}(4)}$$
$$\simeq e_k+J_k\Delta x\text{\hspace{1em}(5)}$$

  其中${\mathrm{J}}_k$是在$\breve{\mathrm{x}}$ 和${\mathrm{e}}_k\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}.}{=}{\mathrm{e}}_k(\breve{\mathrm{x}})$中计算的${\mathrm{e}}_k$的雅可比行列式。 用等式（1）中的${\mathrm{F}}_k$ 的误差项【这里描述应该是$e_k(x_k,z_k）$，前面有等式（3）说明了标记的同等性，这是下面7到8替换获得】由等式（5）代替。 我们获得

$${\mathrm{F}}_k(\breve{\mathrm{x}}+\Delta \mathrm{x})\text{\hspace{1em}(6)}$$
$$={\mathrm{e}}_k(\breve{\mathrm{x}}+\Delta \mathrm{x}{)}^T{\Omega }_k{\mathrm{e}}_k(\breve{\mathrm{x}}+\Delta x)\text{\hspace{1em}(7)}$$
$$\simeq {\left({\mathrm{e}}_k+{\mathrm{J}}_k\Delta \mathrm{x}\right)}^T{\Omega }_k\left({\mathrm{e}}_k+{\mathrm{J}}_k\Delta \mathrm{x}\right)\text{\hspace{1em}(8)}$$
$$=\underset{{\mathrm{c}}_k}{\underbrace{{\mathrm{e}}_k^T{\Omega }_k{\mathrm{e}}_k}}+2\underset{{\mathrm{b}}_k}{\underbrace{{\mathrm{e}}_k^T{\Omega }_k{\mathrm{J}}_k}}\Delta \mathrm{x}+\Delta {\mathrm{x}}^T\underset{{\mathrm{H}}_k}{\underbrace{{\mathrm{J}}_k^T{\Omega }_k{\mathrm{J}}_k}}\Delta \mathrm{x}\text{\hspace{1em}(9)}$$
$$={\mathrm{c}}_k+2{\mathrm{b}}_k\Delta \mathrm{x}+\Delta {\mathrm{x}}^T{\mathrm{H}}_k\Delta \mathrm{x}\text{\hspace{1em}(10)}$$

  通过这种局部近似，可以重写式（1）中给出的函数$\mathrm{F}(x)$如下所示：

$$\mathrm{F}(\breve{\mathrm{x}}+\Delta \mathrm{x})=\sum _{k\in \mathcal{C}}{\mathrm{F}}_k(\breve{\mathrm{x}}+\Delta \mathrm{x})\text{\hspace{1em}(11)}$$
$$\simeq \sum _{k\in \mathcal{C}}{\mathrm{c}}_k+2{\mathrm{b}}_k\Delta \mathrm{x}+\Delta {\mathrm{x}}^T{\mathrm{H}}_k\Delta \mathrm{x}\text{\hspace{1em}(12)}$$
$$=\mathrm{c}+2{\mathrm{b}}^T\Delta \mathrm{x}+\Delta {\mathrm{x}}^T\mathrm{H}\Delta \mathrm{x}\text{\hspace{1em}(13)}$$

  通过设置$c=\sum c_k$,$\mathrm{b}=\sum {\mathrm{b}}_k$和$\mathrm{H}=\sum {\mathrm{H}}_k$，就可以从等式（12）中获得等式（13）的二次形式。 它可以通过求解线性方程组在$\Delta \mathrm{x}$处被最小化

$$\mathrm{H}\Delta {\mathrm{x}}^{\ast }=-\mathrm{b}\text{\hspace{1em}(14)}$$

  矩阵$\mathrm{H}$是方程组的信息矩阵，并且通过构造是稀疏的，仅在通过约束连接的块之间具有非零。 其非零块的数量是约束数加上节点数的两倍。 这允许解决方程式(14)采用稀疏Cholesky分解或预处理共轭梯度（PCG）等有效方法。 稀疏Cholesky分解的高效实现可以在开源的软件包中找到，如CSparse或CHOLMOD。 然后通过将初始猜测添加到计算的增量来获得线性化解

$${\mathrm{x}}^{\ast }=\breve{\mathrm{x}}+\Delta {\mathrm{x}}^{\ast }\text{\hspace{1em}(15)}$$

  流行的Gauss-Newton算法迭代了等式（13）中的线性化， 等式（14）中的解和等式15中的更新步骤。 在每次迭代中，先前的解决方案用作线性化点和初始猜测。

  Levenberg-Marquardt（LM）算法是Gauss-Newton的非线性变体，它引入阻尼因子(damping factor)和备用动作（backup actions）来控制收敛。

  我个人不太喜欢这篇doc中描述的backup actions，显得不是很清晰。文中也就出现过一次。我们只需要知道LM是高斯牛顿的变体，不过是加入了阻尼系数，采用了一定的策略，在运行过程中更改，以便稳定收敛，后期我打算观看高斯牛顿和LM算法方面的书籍，进行自我总结学习，大体思路了解一下差不多就可以了。

  LM不是直接解决方程式14，而是解决了它的阻尼版本：

$$(\mathrm{H}+\lambda \mathrm{I})\mathrm{\Delta }{\mathrm{x}}^{\ast }=-\mathrm{b}\text{\hspace{1em}(16)}$$

  其中$\lambda$是一个阻尼因子：$\lambda$越大$\Delta \mathrm{x}$越小。 这对于在非线性表面的情况下控制步长是有用的。 LM算法背后的想法是动态控制阻尼因子。 在每次迭代时，都会监视新配置的error。 如果新error低于前一个error，则下一次迭代将减少$\lambda$。 否则，恢复解决方案并增加$\lambda$。【关于LM的描述，建议可以查阅相关博客进行了解，主要是判断实际减少和近似模型减少的比值，在SLAM十四讲第113页也有描述，相对描述得浅显易懂些】

  g2o中实现的LM算法，其参考SBA: A Software Package for Generic Sparse Bundle Adjustment。

  上述过程是多变量函数最小化的一般方法。 然而，一般方法假设参数$\mathrm{x}$的空间是欧几里德空间，这对于SLAM或BA等几个问题无效。 这可能导致次优解决方案。

关于线性化方程组结构的思考

  按照方程式(13)中，通过对一组矩阵和向量求和来获得矩阵$\mathrm{H}$和向量$\mathrm{b}$，每个约束对应一个矩阵和向量。 如果我们设置${\mathrm{b}}_k={\mathrm{J}}_k^T{\Omega }_k{\mathrm{e}}_k$和${\mathrm{H}}_k={\mathrm{J}}_k^T{\Omega }_k{\mathrm{J}}_k$，我们可以将$\mathrm{H}$和$\mathrm{b}$重写为

$$\mathrm{b}=\sum _{k\in \mathcal{C}}{\mathrm{b}}_{ij}\text{\hspace{1em}(17)}$$
$$\mathrm{H}=\sum _{k\in \mathcal{C}}{\mathrm{H}}_{ij}\text{\hspace{1em}(18)}$$

  每个约束都将通过加数项为方程组做出贡献。 此加数的结构取决于误差函数的雅可比行列式。 由于约束的误差函数仅取决于节点${\mathrm{x}}_i\in {\mathrm{x}}_k$的值，因此方程（5）中的雅可比行列式具有以下形式【这一部分的想要彻底理解，可以阅读SLAM十四讲第248页，关于稀疏性和边缘化问题，图文化显示得很不错】：

$${\mathrm{J}}_k=\left(0\cdots 0{\mathrm{J}}_{k_1}\cdots {\mathrm{J}}_{k_i}\cdots 0\cdots {\mathrm{J}}_{k_q}0\cdots 0\right)\text{\hspace{1em}(19)}$$
  其中${\mathrm{J}}_{k_i}=\frac{\partial \mathrm{e}({\mathrm{x}}_k)}{\partial {\mathrm{x}}_{k_i}}$是关于由第$k$个超边连接的节点的误差函数的导数，相对于参数块${\mathrm{x}}_{k_i}\in {\mathrm{x}}_k$。

  通过等式（9），我们得到块矩阵${\mathrm{H}}_{ij}$的结构如下：

$${\mathrm{H}}_k=\left(\begin{array}{ccccccc}\ddots & & & & & & \\ & {\mathrm{J}}_{k_1}^T{\Omega }_k{\mathrm{J}}_{k_1}& \cdots & {\mathrm{J}}_{k_1}^T{\Omega }_k{\mathrm{J}}_{k_i}& \cdots & {\mathrm{J}}_{k_1}^T{\Omega }_k{\mathrm{J}}_{k_q}& \\ & {\mathrm{J}}_{k_i}^T{\Omega }_k{\mathrm{J}}_{k_1}& \cdots & {\mathrm{J}}_{k_i}^T{\Omega }_k{\mathrm{J}}_{k_i}& \cdots & {\mathrm{J}}_{k_i}^T{\Omega }_k{\mathrm{J}}_{k_q}& \\ & {\mathrm{J}}_{k_q}^T{\Omega }_k{\mathrm{J}}_{k_1}& \cdots & {\mathrm{J}}_{k_q}^T{\Omega }_k{\mathrm{J}}_{k_i}& \cdots & {\mathrm{J}}_{k_q}^T{\Omega }_k{\mathrm{J}}_{k_q}& \\ & & & & & & \ddots \end{array}\right)\text{\hspace{1em}(20)}$$

$${\mathrm{b}}_k=\left(\begin{array}{c}⋮\\ {\mathrm{J}}_{k_1}{\Omega }_k{\mathrm{e}}_k\\ ⋮\\ {\mathrm{J}}_{k_i}{\Omega }_k{\mathrm{e}}_k\\ ⋮\\ {\mathrm{J}}_{k_q}{\Omega }_k{\mathrm{e}}_k\\ ⋮\end{array}\right)\text{\hspace{1em}(21)}$$

  为简单起见，我们省略了零块。 可能会注意到矩阵$\mathrm{H}$的块结构是超图的邻接矩阵。 另外，Hessian $\mathrm{H}$是对称矩阵，因为所有${\mathrm{H}}_k$都是对称的。 连接$q$个顶点的单个超边将在Hessian中引入$q^2$非零块，超边对应于连接的节点每对$<{\mathrm{x}}_{k_i},{\mathrm{x}}_{k_j}>$。

流形上的最小二乘法

  为了处理张成非欧几里德空间的参数块，通常在流形上应用误差最小化【实际上我想认为这说的是李群，这是一种流形】。 流形是一个数学空间，在全局尺度（global scale）内不一定是欧几里德，但可以在局部尺度（local scale）上看作欧几里德（Introduction to Smooth Manifolds，该书的相关页还有此处链接）。

  例如，在SLAM问题的上下文(context)中，每个参数块${\mathrm{x}}_i$由平移向量${\mathrm{t}}_i$和旋转分量${\alpha }_i$组成。 平移向量${\mathrm{t}}_i$明显形成欧几里德空间。 与此相反，旋转分量${\alpha }_i$张成非欧几里德2D或3D旋转组$SO(2)$或$SO(3)$。

SO(2)是二维旋转，SO（3）是三维旋转

   为了避免奇点，通常以过度参数化的方式描述这些空间，例如通过旋转矩阵或四元数。 这些过参数化的表示直接应用等式15破坏过参数化引起的约束。 过参数化导致额外的自由度，从而在解决方案中引入误差。 为了克服这个问题，可以使用旋转的极小表示（如3D中的欧拉角）。 然而，这将受到奇点的影响【万向锁请了解一下】。

  另一种想法是将underlying space（底空间）视为一个流形，并定义一个算子$⊞$将欧几里得空间中的局部变量(local variation)$\Delta \mathrm{x}$映射到流形上的变量,$\Delta \mathrm{x}\mapsto \mathrm{x}⊞\Delta \mathrm{x}$。

在Bourbaki, Nicolas的Elements of mathematics: Theory of sets 著作中定义"topological space" is an underlying structure of the “Euclidean space” structure，意思是拓扑空间是欧氏空间的 underlying structure。我可以认为底空间是拓扑空间的意思。这里面作者以underlying space描述，委实让我理解起来的时候却是有点难度，我只能理解为在欧氏空间中的拓扑空间的意思。

  此类流行空间优化的具体细节阅读A framework for sparse, non-linear least squares problems on manifolds的1.3部分【此文我还没得阅读】。 使用此运算符，可以将新的误差函数定义为

$${\breve{\mathrm{e}}}_k(\Delta {\tilde{\mathrm{x}}}_k)\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}.}{=}{\mathrm{e}}_k({\tilde{\mathrm{x}}}_k⊞\Delta {\tilde{\mathrm{x}}}_k)\text{\hspace{1em}(22)}$$
$$={\mathrm{e}}_k(\tilde{\mathrm{x}}⊞\Delta \tilde{\mathrm{x}})\simeq {\breve{\mathrm{e}}}_k+{\tilde{\mathrm{J}}}_k\Delta \tilde{\mathrm{x}}\text{\hspace{1em}(23)}$$

  其中 $\breve{\mathrm{x}}$ 张成原始的过度参数化空间，例如四元数。 $\Delta \tilde{\mathrm{x}}$项是初始位置周围$\breve{\mathrm{x}}$的小增量，并以最小表示来表示。 $SO(3)$的常见选择是使用单位四元数的向量部分。 更详细地说，可以将增量$\Delta \tilde{\mathrm{x}}$表示为6D向量 $\Delta {\tilde{\mathrm{x}}}^T=(\Delta \tilde{\mathrm{t}}{\tilde{\mathrm{q}}}^T)$，其中$\Delta \tilde{\mathrm{t}}$表示平移，而$\Delta {\tilde{\mathrm{q}}}^T$是单位四元数的向量部分表示3D旋转。 相反，$\breve{\mathrm{x}}=({\breve{\mathrm{t}}}^T{\breve{q}}^T)$使用四元数${\breve{q}}^T$来对旋转部分编码。 因此，算子$⊞$可以被表示为首先通过将$\Delta \tilde{\mathrm{q}}$转换为完全四元数$\Delta \mathrm{q}$然后将变换$\Delta {\mathrm{x}}^T=(\Delta \mathrm{t}\Delta {\mathrm{q}}^T)$应用于$\breve{\mathrm{x}}$【从22到23式】。

估计在上下标处会迷糊，我是这样认为，$\breve{\mathrm{x}}$这种表示是初始猜测的表示。$\tilde{\mathrm{x}}$的上标这种表示微小增量。后面也有需要这种表示的说明需要很好的了解才行。

  在描述误差最小化的等式中，这些操作可以很好地由$⊞$算子封装。 雅可比矩阵${\tilde{\mathrm{J}}}_k$可以表达为:

$${\tilde{\mathrm{J}}}_k={\frac{\partial {\mathrm{e}}_k(\breve{\mathrm{x}}⊞\Delta \tilde{\mathrm{x}})}{\partial \Delta \tilde{\mathrm{x}}}|}_{\Delta \tilde{\mathrm{x}}=0}\text{\hspace{1em}(24)}$$

  由于在前面的等式中，$\breve{\mathrm{e}}$仅依赖于$\Delta {\tilde{\mathrm{x}}}_{k_i}\in \Delta {\tilde{\mathrm{x}}}_k$，我们可以进一步扩展如下：

$${\tilde{\mathrm{J}}}_k={\frac{\partial {\mathrm{e}}_k(\breve{\mathrm{x}}⊞\Delta \tilde{\mathrm{x}})}{\partial \Delta \tilde{\mathrm{x}}}|}_{\Delta \tilde{\mathrm{x}}=0}\text{\hspace{1em}(25)}$$
$$=\left(0\cdots 0{\tilde{\mathrm{J}}}_{k_1}\cdots {\tilde{\mathrm{J}}}_{k_i}\cdots 0\cdots {\tilde{\mathrm{J}}}_{k_q}0\cdots 0\right).\text{\hspace{1em}(26)}$$

  通过简单的表示法扩展，我们设

$${\tilde{\mathrm{J}}}_{k_i}={\frac{\partial {\mathrm{e}}_k(\breve{\mathrm{x}}⊞\Delta \tilde{\mathrm{x}})}{\partial \Delta {\tilde{\mathrm{x}}}_{k_i}}|}_{\Delta \tilde{\mathrm{x}}=0}\text{\hspace{1em}(27)}$$

  通过简单的表示法扩展，我们可以在等式（8）和等式（11）中插入等式（23）。 这导致以下增量：

$$\tilde{\mathrm{H}}\Delta {\tilde{\mathrm{x}}}^{\ast }=-\tilde{\mathrm{b}}\text{\hspace{1em}(28)}$$

  由于增量$\Delta {\tilde{\mathrm{x}}}^{\ast }$是在初始猜测$\breve{\mathrm{x}}$的局部欧几里得环境中计算的，因此需要通过$⊞$操作符将它们重新映射到原始冗余空间中。 因此，等式（15）的更新规则变成了

$${\mathrm{x}}^{\ast }=\breve{\mathrm{x}}⊞\Delta {\tilde{\mathrm{x}}}^{\ast }\text{\hspace{1em}(29)}$$

  总之，在流形上形式化最小化问题包括首先由公式(28）计算围绕初始猜测的局部欧几里得近似的一组增量，然后通过公式（29）累加全局非欧几里得空间中的增量。

  注意，在流形表示上计算的线性方程组具有与在欧几里德空间上计算的线性方程组相同的结构。可以很容易地从非流形版本推导出图最小化的流形版本，只需要定义一个⊞算子及其雅克比矩阵 ${\tilde{\mathrm{J}}}_{k_i}$ 相应的参数块。g2o提供了用于在流型空间上数值计算雅可比的工具。这要求用户仅实现误差函数和⊞操作符。

  g2o没有解决非流型情况，因为该问题已经包含在流型中。但是，为了获得最大的性能和准确性，建议the user to implement analytic Jacobians, once the system is functioning with the numeric ones。
这句建议不知道怎么翻译才准确表达，个人理解这句话，作者的意思是，雅克比的矩阵，在已知一部分数值的时候，可以通过数学公式推导得到计算的雅克比的最终代数式子（可能仅需要几个数值就好），而不是用g2o帮你计算，可以加速性能和准确性。

稳健最小二乘

  可选地，最小二乘优化可以是稳健的。 请注意，方程式（1）中的误差项具有以下形式：

$${\mathrm{F}}_k={\mathrm{e}}_k^T{\Omega }_k{\mathrm{e}}_k={\rho }_2(\sqrt{{\mathrm{e}}_k^T{\Omega }_k{\mathrm{e}}_k})\text{\hspace{1em}(30)}$$

  其中${\rho }_2(x):=x^2$

  因此，误差向量${\mathrm{e}}_k$对$\mathrm{F}$具有二次影响，因此单个潜在异常值将具有主要的负面影响。 为了使更加离群的鲁棒性，二次误差函数${\rho }_2$可以用更稳健的成本函数代替，该成本函数对较小的误差进行较小的权重。 在g2o中，可以使用Huber成本函数${\rho }_H$。

$${\rho }_H:=\{\begin{array}{ll}{x}^2& \text{if}|x|<b\\ 2b|x|-b^2& \text{else},\end{array}\text{\hspace{1em}(31)}$$

  对于小$|x|$，它是二次的,但对于大$|x|$而言是线性的。 与其他更强大的成本函数相比，Huber内核的优势在于它仍然是凸的，因此不会在$\mathbf{F}$ 中引入新的局部最小值。

  下面是摘自多视图几何第二版的616页的一些成本函数的成本函数，概率密度函数，衰减系数。

  这些函数以后有时间再仔细说明。回到正题，在实践中，我们不需要修改等式（1）。 相反，应用以下方案。 首先，像往常一样计算误差${\mathrm{e}}_k$。 然后，${\mathrm{e}}_k$被加权版本$w_k{\mathrm{e}}_k$取代:

$$(w_k{\mathrm{e}}_k{)}^T{\Omega }_k(w_k{\mathrm{e}}_k)={\rho }_H\left(\sqrt{{\mathrm{e}}_k^T{\Omega }_k{\mathrm{e}}_k}\right)\text{\hspace{1em}(32)}$$

  其中，权重$w_k$如下计算

$$w_k=\frac{\sqrt{{\rho }_H(||{\mathrm{e}}_k|{|}_{\Omega })}}{||{\mathrm{e}}_k|{|}_{\Omega }}\text{\hspace{1em}(33)}$$

  其中$||{\mathrm{e}}_k|{|}_{\Omega }:=\sqrt{{\mathrm{e}}_k^T{\Omega }_k{\mathrm{e}}_k}$。

  在g2o中，用户具有细粒度控制，可以单独启用/禁用每个边的稳健成本函数（参见下文）。

库的概览

  从上面的部分可以清楚地看出，图优化问题完全由以下定义：

• 图中顶点的类型（即参数块 { x i } \{{\rm x}_i\} {xi​}。对于每个顶点的类型必须指定：
  1. 内部参数化(internal parameterization)的定义域(domain) $\text{Dom}({\mathrm{x}}_i)$，
  2. 增量$\Delta {\mathrm{x}}_i$的定义域 $\text{Dom}(\Delta {\mathrm{x}}_i)$,
  3. $⊞:\text{Dom}({\mathrm{x}}_i)\times \text{Dom}(\Delta {\mathrm{x}}_i)\to \text{Dom}({\mathrm{x}}_i)$，应用于增量$\Delta {\mathrm{x}}_i$到先前的方案${\mathrm{x}}_i$.
• 每种类型的超边${\mathrm{e}}_k$的误差函数：$\text{Dom}(\Delta {\mathrm{x}}_{k_1})\times \text{Dom}(\Delta {\mathrm{x}}_{k_2})\times \cdots \text{Dom}(\Delta {\mathrm{x}}_{k_q})\to \text{Dom}({\mathrm{z}}_k)$当扰动估计${\mathrm{x}}_k⊞\Delta {\mathrm{x}}_k$完全满足约束${\mathrm{z}}_k$时应该为零。

  默认情况下，Jacobians由g2o的框架以数字方式计算。 然而，为了在特定实现中实现最大性能，可以指定误差函数和流形运算符的雅可比行列式。

g2o类图

优化问题的表示

  g2o中利用通用的超图结构来表示问题实例（在hyper_graph.h中定义）。 此通用超图表专门用于表示OptimizableGraph类的优化问题，该类在optimizable_graph.h中定义。 在OptimizableGraph中，内部类OptimizableGraph :: Vertex和OptimizableGraph :: Edge用于表示通用超边和超顶点。 虽然可以通过直接扩展这些类来完成特定的实现，但g2o提供了一个模板特化，它自动实现了方程组(system)必须使用的大多数方法。

  这些类是BaseVertex和BaseUnaryEdge，BaseBinaryEdge和BaseMultiEdge。

• BaseVertex模拟参数块${\mathrm{x}}_i$和相应流形$\Delta {\mathrm{x}}_i$的维数，因此它可以使用其编译时（意味着效率）已知布局的存储块（blocks of memory）。 此外，它实现了一些映射运算符来存储Hessian和线性化问题的参数块，以及一堆可用于保存/恢复图部分的先前值。 将由$\mathrm{v}$表示的扰动$\Delta {\mathrm{x}}_i$应用于成员变量_estimate的方法oplusImpl（double * v）应该被实现。 这是$⊞$运算符。 此外，必须指定应将顶点的内部状态设置为$0$的setToOriginImpl()。

  template <int D, typename T>
    class BaseVertex : public OptimizableGraph::Vertex
  

• BaseUnaryEdge是一个模拟一元超边(unary hyper-edge)的模板类，可用于表示先验。 它通过linearizeOplus方法的实现免费提供雅可比行列式的计算。 它需要指定（单个）顶点${\mathrm{x}}_i$的类型，以及误差$\mathrm{e}({\mathrm{x}}_k)$的类型和维度作为模板参数。 将误差$\mathrm{e}({\mathrm{x}}_k)$的结果存储在成员Eigen :: Matrix _error中的函数computeError应该被实现。

    template <int D, typename E, typename VertexXi>
    class BaseUnaryEdge : public BaseEdge<D,E>
  

• BaseBinaryEdge是一个对二元约束的建模的模板类，即${\mathrm{e}}_k({\mathrm{x}}_{k_1},{\mathrm{x}}_{k_2})$形式的误差函数。 它提供与BaseUnaryEdge相同的功能，并且需要指定以下模板参数：节点${\mathrm{x}}_{k_1}$和${\mathrm{x}}_{k_2}$的类型以及测量的类型和维度。 同样，它通过linearizeOplus方法的默认实现实现了数值形式的雅可比行列式。 同样，computeError应该在派生类中实现。

  template <int D, typename E, typename VertexXi, typename VertexXj>
  class BaseBinaryEdge : public BaseEdge<D, E>

• BaseMultiEdge是一个模板类，它以${\mathrm{e}}_k({\mathrm{x}}_{k_1},{\mathrm{x}}_{k_2},\cdots ,{\mathrm{x}}_{k_q})$的形式模拟多顶点约束。 它提供与上述类型相同的功能，并且只需要将测量的类型和维度指定为模板参数。特化类（specialized class，应指已经制定了模板是何种类型的类）应该注意将连接的顶点的大小调整为正确的大小$q$。 这个类依赖于动态内存，因为太多参数是未知的，如果您需要针对特定问题的高效实现，可以自己编程。 数值化的雅可比矩阵是免费的（意思是g2o提供了默认的运算代码），但您应该像往常一样在派生类中实现computeError

  template <int D, typename E>
    class BaseMultiEdge : public BaseEdge<D,E>

  简而言之，定义新问题实例所需要做的就是从上面列出的那些类中派生一组类，每类参数块便是一个类，每种类型的（超）边便是一个类（意思是你需要定义节点和边，分别用类来表示）。 总是试着从最适合你的类派生出来。 如果你想看一个简单的例子，请看vertex_se2和edge_se2。 这两种类型定义了一个简单的2D图SLAM问题，就像许多SLAM论文中描述的那样。

  当然，对于构造的每种类型，您还应该定义read和write函数，以便将数据读取和写入流。 最后，一旦定义了新类型，要启用加载和保存新类型，您应该将其“注册”到工厂。 这可以通过registerType函数将字符串标记分配给新类型来轻松完成。 这应该在加载所有文件之前调用一次。

  为此，g2o提供了一个简单的宏来执行类到工厂的注册。 有关示例，请参见清单1，完整示例可在types_slam2d.cpp中找到。

  给宏G2O_REGISTER_TYPE的第一个参数指定了某个顶点/边已知的标签。 g2o将在加载文件时使用此信息并将当前图保存到文件中。 在清单1给出的示例中，分别使用VertexSE2和EdgeSE2类注册标记VERTEX_SE2和EDGE_SE2。

  此外，宏G2O_REGISTER_TYPE_GROUP允许声明类型组。 如果我们使用工厂来构造类型，那么这是必要的，我们必须强制我们的代码链接到特定的类型组。 否则链接器可能会删除我们的库，因为我们没有显式使用包含我们类型的库提供的任何符号。 声明使用特定类型库并因此强制执行链接是由G2O_USE_TYPE_GROUP命名的宏完成的。

线性化问题的构造与表示

  可以将构造和解决方案分成单独的步骤，这些步骤是迭代的。

• 初始化优化（仅在第一次迭代之前）。
• 计算每个约束的误差向量。
• 线性化每个约束。
• 构建线性方程组。
• 更新Levenberg-Marquardt阻尼系数。

  在以下部分中，我们将介绍这些步骤。

个人认为，这个步骤的流程将LM说得倒是蛮清楚了。

初始化

  SparseOptimizer类提供了几种初始化底层(underlying)数据结构的方法。

前面我有指出，underlying一般是对应于拓扑空间，所以这个意思我理解是初始化几种拓扑空间的基（关于基我不想太多描述，学过线性代数的人应该会基础）。

  initializeOptimization()方法采用顶点子集或边子集，这些边将被考虑用于下一次优化运行。 此外，可以考虑所有顶点和边进行优化。 我们分别引用当前被视为活动顶点(active vertices)和边的顶点和边。

  在初始化过程中，优化器（optimizer）为每个活动顶点分配一个临时索引(temporary index)。 该临时索引对应于Hessian中的顶点的块列/行。 在优化过程中，某些顶点可能需要保持固定，以解决任意自由度（规范自由度,gauge freedom)。 这可以通过设置顶点的_fixed属性来完成。

计算误差

  computeActiveErrors()函数获取活动顶点的当前估计值，并且对于每个活动边调用computeError()以计算当前误差向量。 使用前面描述的基本边缘类(个人理解，指的是BaseUnaryEdge, BaseBinaryEdge 和 BaseMultiEdge)，误差应缓存在成员变量_error中。

  如果对于特定活动边(particular active edge)，robustKernel()设置为true，则调用robustifyError()，正如前面“稳健最小二乘”的部分中的描述的，_error是健壮的【前面提到了g2o是采用Huber鲁棒成本函数】。

线性化方程组

  通过调用linearizeOplus()函数来线性化每个活动边。 Jacobians也可以通过前面描述的模板化基类提供的成员变量进行缓存。

  这句话有点含糊，其实应该说的是前面描述的模板化基类中其内部定义有成员变量可以缓存雅克比的计算结果，比如在BaseMultiEdge的类中，定义了成员变量_jacobianOplus， 它的类型是 std::vector<JacobianType, Eigen::aligned_allocator<JacobianType> >，在BaseBinaryEdge中，雅克比相关的成员有_jacobianOplusXi和_jacobianOplusXi。

  如果未重新实现linearizeOplus()函数，雅可比将按数字计算如下：

$${\tilde{\mathrm{J}}}_k^{\bullet l}=\frac{1}{2\delta }\left({\mathrm{e}}_k({\mathrm{x}}_k⊞\delta 1_l)-{\mathrm{e}}_k({\mathrm{x}}_k⊞-\delta 1_l)\right),\text{\hspace{1em}(34)}$$

  其中$\delta >0$是一个小常数（g2o的实现是$10^{-}9$），$1_l$是沿着维$l$的单位向量。 请注意，g2o只存储和计算初始化期间尚未确定的${\tilde{\mathrm{J}}}_k$的非零元素。

构建线性方程组

  对于每个活动边，等式（18）的加数项可以通过将雅可比的对应块与边的信息矩阵相乘来计算。 通过调用constructQuadraticForm()在每个边上计算加数项。

更新Levenberg-Marquardt

  如公式16所示。 Levenberg-Marquardt算法需要更新线性方程组。 但是，只需要修改沿主对角线的元素。 为此，Solver类的方法updateLevenbergSystem（double lambda）和recoverSystem（double lambda）通过分别沿$\mathbf{H}$的主对角线加或减$\lambda$来应用修改。

求解器(Solvers)

  这些最小二乘方法的核心组成部分是线性方程组$\tilde{\mathbf{H}}\Delta {\tilde{\mathrm{x}}}^{\ast }$的解。 为此，有几种方法可用，其中一些方法利用某些问题的已知结构来减少方程组的中间计算过程的时间，例如将Schur补(舒尔补)应用于变量子集。

扩展知识：舒尔补

  在线性代数和矩阵理论中，矩阵块的舒尔补被如下定义：

  假设有四个矩阵，分别是$\mathrm{A},\mathrm{B},\mathrm{C},\mathrm{D}$,维度分别为$p\times p,p\times q,q\times p,q\times ,q$，其中$\mathrm{D}$是可逆矩阵。那么如果让一个矩阵$\mathrm{M}$分别由这几个矩阵组成，即

$$\mathrm{M}=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$$

  则$M$是$(p+q)\times (p\times q)$的矩阵。

  矩阵$M$的块$D$的舒尔补是$p\times p$矩阵，定义为：

$$M/D:=A-B{D}^{-1}C$$

  矩阵$M$的块$A$的舒尔补是$q\times q$矩阵,定义为：

$$M/A:=D-C{A}^{-1}B$$

  如果$A$或$D$是奇异矩阵（非可逆矩阵），则用广义逆代替$M/A$和$M/D$上的逆，则得到的是广义舒尔补。

  一般在求解线性方程组的时候舒尔补会有用到，比如下面线性方程组

$$Ax+By=a$$

$$Cx+Dy=b$$

  其中$x，a$是$p$维列向量，$y,b$是$q$维列向量，$A,B,C,D$就如前面定义的一样，我们把第二个方程左乘以$B{D}^{-1}$，然后再拿第一个方程减去它得到：

$$(A-B{D}^{-1}C)x=a-B{D}^{-1}b$$

  因此，如果可以知道$D$的逆矩阵以及$D$的舒尔补，就可以求解出$x$，然后代入第二个等式直接求解出$y$，这样便将$(p+q)\times (p+q)$矩阵求逆问题转换成求$p\times p$和$q\times q$的逆矩阵问题，降低了求解的复杂度。实践中，需要$D$是良态的矩阵。

这里的良态指的是低条件数的。在数值分析领域，条件数被定义为在输入参数微小变化时输出参数变换大小幅度的衡量标准。这里有一篇博文讲述了矩阵的良态和病态，讲得稍微粗显。

具体你可以理解这里的意思是，比如$Ax=b$,当$A$或$b$有微小扰动时，求解$x$的误差很小，则可以称矩阵$A$是良态矩阵。

  在g2o中，我们不选择任何特定的求解器，但我们依赖于外部库。 为此，我们从线性方程组的解决方案中解耦这些结构操作（如Schur补）。

  雅可比矩阵的线性问题和超图元素中的误差向量的构造由所谓的Solver类控制。要使用方程组的特定分解，用户必须扩展Solver类，并实现虚函数。即解算器应该能够从超图中提取线性方程组，并返回解决方案。

  这是通过几个步骤完成的：

  在优化开始时调用函数initializeStructure，以分配将在后续操作中被覆盖的必要内存。这是可能的，因为方程组的结构在迭代之间不会改变。然后，用户应该通过函数b()和x()提供访问增量向量$\Delta \tilde{\mathrm{x}}$和$\tilde{\mathrm{b}}$的方法。为了支持Levenberg-Marquardt，还应该实现一个用$\lambda \mathrm{I}$项干扰Hessian的函数。此函数称为setLambda（double lambda），需要由特定解算器实现。

  我们提供了一个解析器类的模板化实现，BlockSolver <>将线性方程组存储在SparseBlockMatrix <>类中。 BlockSolver <>也实现了Schur补，并依赖于另一个抽象类LinearSolver来解决方程组问题。 线性求解器的实现完成了求解（简化）线性方程组的实际工作，并且必须实现一些方法。 在这个版本的g2o中，我们提供了线性求解器，它们分别使用预处理梯度下降，CSparse和CHOLMOD。

这个版本的说法不太明确，但不影响使用

Actions

  在g2o的范围内，存储在超图中的实体具有纯数学意义。 它们要么代表要优化的变量（顶点），要么代表优化约束。 然而，通常这些变量通常与更多“具体(concrete)”对象有关，例如激光扫描，机器人位姿，相机参数等。 某些变量类型可能仅支持可行操作的子集。 例如，可以“绘制”机器人位姿，但是不可能“绘制”校准参数。 更一般地说，我们不能事先知道g2o的用户类型将支持的操作类型。 但是，我们希望设计一组依赖于某些操作的工具和函数。 这些包括例如查看器或以特定格式保存/加载图形的函数。

  这样做的可能性是用多个虚函数“重载”超图元素（顶点和边）的基类，每个虚函数对应我们想要支持的每个函数。 这当然不是很优雅，因为每次添加新内容时我们都需要使用新函数修补（patch）基类。 另一种可能性是利用C ++的多重继承，并定义一个抽象的“可绘制”对象，观察者在其上操作。 这个解决方案有点好，但是我们不能为每个对象提供多个“绘图”函数。

  g2o中使用的解决方案包括创建一个函数对象库，该函数对象对图的元素（顶点或边）进行操作。 其中一个函数对象由函数名称和它运行的类型标识。 这些函数对象可以注册到action library中。 在action library中加载这些对象后，可以在图上调用它们。 这些函数在hyper_graph_action.h中定义。 在定义边和顶点的类型时，通常会注册并创建actions。 您可以在type_*/* .h中看到许多示例。

这里的action library我想翻译成动态库好像不太准确，因为后文的actions就不好翻译了（其实我想翻译actions成函数，即像库一样提供的某种功能的函数），为了保持语义的流畅性，最后还是原英文。

g2o工具

  g2o附带两个工具，可以处理存储在文件中的数据。 数据可以从文件加载并在处理后再次存储。 在下文中，我们将简要介绍这些工具，即命令行界面和图形用户界面。

g2o命令行界面

  g2o是g2o中包含的命令行界面。 它允许优化存储在文件中的图形并将结果保存回文件。 这允许快速原型化优化问题，因为它只需要实现新类型或求解器。 g2o分发包括数据文件夹，其包括可以应用g2o的一些数据文件。

g2o Viewer

  下图中描绘的图形用户界面允许可视化优化问题。 另外，可以控制算法的各种参数。

g2o的图形界面。 GUI允许选择不同的合适优化器并执行优化。

g2o增量(incremental)

  g2o包括用于以增量方式执行优化的实验二进制，即，在插入一个或多个节点及其测量之后进行优化。 在这种情况下，g2o在Hessian矩阵上执行ranke更新以更新线性方程组。 有关其他信息，请参阅g2o_incremental子文件夹中的README。

g2o_incremental的文件夹应该指的是当前g2o目录下的，g2o/examples/interactive_slam/g2o_incremental文件夹

  该README文件的内容如下：

g2o_incremental implements a variant of the iSAM algorithm.
In contrast to iSAM the update of the Cholesky factor is done
using rank updates as provided via the CHOLMOD library.

“iSAM: Incremental Smoothing and Mapping”
by M. Kaess, A. Ranganathan, and F. Dellaert.
IEEE Trans. on Robotics, vol. 24, no. 6, Dec. 2008, pp. 1365-1378

Furthermore, it implements a simple protocol to control the operation of the
algorithm. See protocol.txt for more details or
http://slameval.willowgarage.com/workshop/submissions/ for an evaluation system
as well as data sets.

In the standard configuration g2o_incremental solves after inserting 10 nodes.
This behavior can be modified via a command line option. Furthermore, by
specifying -g on the command line a gnuplot instance is created to visualize
the graph.

Please note that both the visualization via Gnuplot and the verbose output
affect the timing results.

g2o_incremental is licensed under GPLv3.

  快速看一遍后大概知道的是g2o_incremental是实现了iSAM的变体，它通过CHOLMOD库使用rank updates而不是Cholesky factor，而且实现了简单控制算法操作的协议。好了这部分看上面的文件内容，大致能了解许多。

g2o_incremental使用示例

  Manhattan3500数据集的示例：

Plug-in Architecture(插件架构)

  这两个工具都支持在动态库中从运行时加载类型和优化算法。

这两个工具老实说我还没搞明白作者表达的意思，我觉得可能是指g2o Viewer和g2o命令行

  这实现如下。 这些工具从libs文件夹加载所有匹配* types *和* solver *的库，分别用于注册类型和优化算法。 我们假设通过加载库，类型和算法通过它们各自的构造函数注册到方程组。 清单1显示了如何向系统注册类型，清单2是一个示例，它显示了如何通过插件架构注册优化算法。

  对于加载包含类型或优化算法的动态库，我们支持两种不同的方法：

• 这些工具识别命令行开关-typeslib和-solverlib以加载特定库。
• 您可以指定在开始时扫描的环境变量G2O_TYPES_DIR和G2O_SOLVER_DIR，并自动加载与* types *和* solver *匹配的库。

2D SLAM:一个例子

  SLAM是机器人技术中众所周知的问题，这个缩写代表“同步定位与建图”。问题可以表述如下：给定一个配备有传感器的移动机器人，我们想要从传感器测量值估计环境中机器人的地图和姿势。
  通常，传感器可以分为外部感受型或本体感受型。外部感知传感器是一种测量相对于机器人移动环境的数量的装置。这些传感器的例子可以是在特定位置获取世界图像的相机，测量机器人周围的一组距离的激光扫描仪或存在重力的加速度计，其测量重力矢量或通过观察已知卫星的星座得出位姿估计的GPS。
  相比之下，本体感受器传感器测量相对于先前机器人位置的机器人状态（位置）的变化。示例包括里程表，其测量机器人在两个时间步长或陀螺仪之间的相对运动。在传统的SLAM方法中，与EKF一样，这两个传感器在系统中扮演着截然不同的角色。本体感受测量用于演化一组状态变量，而外感测量用于通过反馈测量误差来校正这些估计。这不是平滑方法（如可以用g2o实现的方法）的情况，其中所有测量都以基本相似的方式处理。

  SLAM的完整解决方案通常相当复杂，涉及处理原始传感器数据并确定先前看到的环境部分与实际测量（数据关联）之间的对应关系。 描述问题的完整解决方案超出了本文的范围。 但是，请注意，我们将提供一个简化但有意义的问题版本，其中包含所有相关元素，非常适合用g2o实现。

  场景(scenario)是机器人在平面上移动。 机器人配备有测距传感器，能够测量机器人在两个时间帧之间的相对运动，以及“地标”传感器，其能够测量机器人参考系中机器人附近的一些环境标志的位置。 例如，可以通过从激光扫描中提取角点或通过从立体图像对中检测相关特征的位置来实现该地标检测器。 我们在本节中进行的简化是地标是唯一可识别的。 换句话说，每当机器人看到一个地标时，它就可以判断它是否是一个新的，或者它已经知道在何时是否已经看到它。

  显然，里程计和地标传感器都受到噪音的影响。 原则上，如果测距不会受到噪声的影响，则可以简单地通过连接测距测量来重建机器人的轨迹。 然而，情况并非如此，并且集成测距法导致增加的定位误差，当机器人重新进入已知区域时该定位误差变得明显【回环检测能约束该误差】。 以类似的方式，如果机器人具有无限的感知范围，则它可以一次获取所有地图并且可以通过简单的几何构造来检索位置。 同样情况并非如此，并且机器人感知位于最大范围内的地标的位置。 这些测量受到噪声的影响，噪声通常随着机器人的地标距离而增加。

下面将介绍在g2o中表征问题所需的所有必要步骤。 这些是：

• 识别状态变量${\mathrm{x}}_i$及其定义域，
• 约束和图结构的识别的表征，
• 增量$\Delta {\tilde{\mathrm{x}}}_i$的参数化，以及$⊞$操作符的定义。
• 误差函数${\mathrm{e}}_k({\mathrm{x}}_k)$的构造

确定状态变量

  下图示出了SLAM图的片段。

SLAM过程的图形表示。 用圆形节点描绘的图的顶点表示机器人位姿${\mathrm{x}}_{\ast }^{\mathrm{s}}$或地标${\mathrm{x}}_{\ast }^{\mathrm{l}}$。 通过约束${\mathrm{z}}_{\ast }^{\mathrm{l}}$捕获来自机器人位姿的地标的测量，并且通过约束${\mathrm{z}}_{\ast }^{\mathrm{s}}$对连接后续机器人姿势的测距测量进行建模。

  机器人位置由节点${\mathrm{x}}_t^{\mathrm{s}}$表示，而地标由节点${\mathrm{x}}_i^{\mathrm{l}}$表示。 我们假设我们的地标传感器只能检测地标的2D位姿，而不能检测其方向。 换句话说，地标"活"在${\mathfrak{R}}^2$，机器人位姿由平面上的机器人位置$（x，y）$及其方向$\theta$参数化，因此它们属于2D变换群$SE(2)$。 更正式地说，2D SLAM图的节点有两种类型

• 机器人位置：${\mathrm{x}}_t^{\mathrm{s}}={\left(\begin{array}{ccc}{x}_t^{\mathrm{s}}& y_t^{\mathrm{s}}& {\theta }_t^{\mathrm{s}}\end{array}\right)}^T\in SE(2)$
• 地标位置：${\mathrm{x}}_i^{\mathrm{l}}={\left(\begin{array}{cc}{x}_i^{\mathrm{l}}& y_i^{\mathrm{l}}\end{array}\right)}^T\in {\mathfrak{R}}^2$

约束的建模

  两个随后的机器人位姿${\mathrm{x}}_t^{\mathrm{s}}$和${\mathrm{x}}_{t+1}^{\mathrm{s}}$通过测距法测量相关联，其表示通过测距法测量的机器人从${\mathrm{x}}_t^{\mathrm{s}}$到${\mathrm{x}}_{t+1}^{\mathrm{s}}$的相对运动。 由于影响传感器的噪声，该测量通常与两个位姿之间的实际变换略有不同。 作为里程测量，欧氏变换，它也是$SE(2)$群的成员。 假设影响测量的噪声是白噪声和高斯噪声，它可以通过$3\times 3$对称正定信息矩阵建模。 在实际应用中，该矩阵的元素取决于机器人的运动。 即，运动越大，不确定性就越大。 因此，节点${\mathrm{x}}_t^{\mathrm{s}}$和${\mathrm{x}}_{t+1}^{\mathrm{s}}$之间的测距边缘由这两个实体组成：

• ${\mathrm{z}}_{t,t+1}^{\mathrm{s}}\in SE(2)$表示节点之间的运动以及
• ${\Omega }_{t,t+1}^{\mathrm{s}}\in {\mathfrak{R}}^{3\times 3}$表示测量的逆协方差(the inverse covariance of the measurement)，因此是对称的和正定的。

  如果机器人从位置${\mathrm{x}}_i^{\mathrm{l}}$感测到地标${\mathrm{x}}_t^{\mathrm{s}}$，则相应的测量将建模为由从机器人位姿到路标的边。 关于地标的测量包括在机器人系中感知的x-y平面中的点。 因此，具有里程碑意义的测量值就像地标一样存在于${\mathfrak{R}}^2$中。 同样，在白高斯噪声假设下，噪声可以通过其逆协方差来建模。 因此，机器人位姿和地标之间的边以这种方式被参数化：

• ${\mathrm{z}}_{t,i}^{\mathrm{l}}\in {\mathfrak{R}}^2$表示由${\mathrm{x}}_t^{\mathrm{s}}$表示的坐标系中的地标的位置和
• ${\Omega }_{t,i}^{\mathrm{l}}\in {\mathfrak{R}}^{2\times 2}$表示测量的逆协方差，是SPD。

SPD:Symmetric positive definite,对称且正定。作者这个缩写真的是秀了。

增量参数化的选择

  到目前为止，我们已经定义了使用g2o实现2D SLAM算法所需的大部分元素。 即，我们描述了变量的定义域和测量的定义域。 剩下要做的是为我们系统中的两种边定义误差函数，并确定增量的（可能是智能的）参数化。

  地标位置参数化为${\mathfrak{R}}^2$，它已经是欧几里德空间。 因此，增量$\Delta {\tilde{\mathrm{x}}}_i^{\mathrm{l}}$可以存在于同一空间中，并且可以安全地选择$⊞$运算符作为向量和：

$${\mathrm{x}}_i^{\mathrm{l}}⊞\Delta {\tilde{\mathrm{x}}}_i^{\mathrm{l}}\doteq {\mathrm{x}}_i^{\mathrm{l}}+\Delta {\tilde{\mathrm{x}}}_i^{\mathrm{l}}$$

  相反，这些位姿活在(live in,你可以认为是属于)非欧几里德空间$SE(2)$中。 此空间允许许多参数化。 示例包括：旋转矩阵$\mathbf{R}(\theta )$和平移向量$(xy{)}^T$或角度$\theta$和平移向量$(xy{)}^T$。

  作为增量的参数化，我们选择最小的一个，即平移向量和角度。 选择此参数化后，我们需要在位姿和位姿增量之间定义$⊞$运算符。 一种可能的选择是将三个标量参数$x$，$y$和位姿的$\theta$视为矢量，并将$⊞$定义为矢量和。 这是一个糟糕的选择有很多原因。 其中之一是角度不是欧几里德，人们需要在每次添加后重新归一化(re-normalize)它们。

  更好的选择是将位姿和位姿增量之间的$⊞$定义为运动合成运算符。 即，给定机器人位姿${\mathrm{x}}_t^{\mathrm{s}}={\left(\begin{array}{ccc}x& y& \theta \end{array}\right)}^T$和增量$\Delta {\tilde{\mathrm{x}}}_t^{\mathrm{s}}={\left(\begin{array}{ccc}\Delta x& \Delta y& \Delta \theta \end{array}\right)}^T$，其中$\Delta x$是纵向位移（即，在机器人的航向方向上），$\Delta y$是横向位移和$\Delta \theta$是旋转变化 ，运算符可以定义如下：

$${\mathrm{x}}_t^{\mathrm{s}}⊞\Delta {\tilde{\mathrm{x}}}_t^{\mathrm{s}}\doteq \left(\begin{array}{c}x+\Delta x\cos \theta -\Delta y\sin \theta \\ y+\Delta x\sin \theta +\Delta y\cos \theta \\ \mathrm{n}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{A}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}(\theta +\Delta \theta )\end{array}\right)\text{\hspace{1em}(36)}$$
​ $$={\mathrm{x}}_t^{\mathrm{s}}\oplus \Delta {\tilde{\mathrm{x}}}_t^{\mathrm{s}}\text{\hspace{1em}(37)}$$

  在前面的等式中，我们引入了运动合成运算符$\oplus$与$\oplus$类似，有$\ominus$运算符执行相反的操作，定义如下：

$${\mathrm{x}}_a^{\mathrm{s}}\ominus {\mathrm{x}}_b^{\mathrm{s}}\doteq \left(\begin{array}{c}(x_a-x_b)\cos {\theta }_b+(y_a-y_b)\sin {\theta }_b\\ (x_a-x_b)\sin {\theta }_b+(y_a-y_b)\cos {\theta }_b\\ \mathrm{n}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{A}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}({\theta }_b-{\theta }_a)\end{array}\right)\text{\hspace{1em}(38)}$$

误差函数的设计

  形式化问题的最后一步是设计“合理”的误差函数$\mathrm{e}({\mathrm{x}}_k)$。 一种常见的方法是定义一个所谓的测量函数${\mathbf{h}}_k({\mathrm{x}}_k)$，给定集合${\mathrm{x}}_k$中顶点的先验，它能“预测”测量${\hat{\mathrm{z}}}_k$。 定义此函数通常相当简单，可以通过直接实现误差模型来完成。 随后，可以将误差矢量计算为预测${\hat{\mathrm{z}}}_k$和实际测量之间的矢量差。 这是构造误差函数的一般方法，当误差的空间位于测量原点周围的局部欧几里得时，它就起作用。 如果不是这种情况，可能想要用更“常规”的其他运算符替换向量差。

  我们现在将构建连接机器人位姿${\mathrm{x}}_t^{\mathrm{s}}$和地标${\mathrm{x}}_i^{\mathrm{l}}$的边的误差函数。 第一步是构建计算“虚拟测量”的测量预测${\mathbf{h}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}}，{\mathrm{x}}_i^{\mathrm{l}})$。 该虚拟测量是从机器人位姿${\mathrm{x}}_t^{\mathrm{s}}$看到的地标${\mathrm{x}}_i^{\mathrm{l}}$的位置。 ${\mathbf{h}}_{t,i}^{\mathrm{l}}(\cdot )$的等式如下：

$${\mathbf{h}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_i^{\mathrm{l}})\doteq \left(\begin{array}{c}(x_t^{\mathrm{s}}-x_i)\cos {\theta }_t^{\mathrm{s}}+(y_t^{\mathrm{s}}-y_i)\sin {\theta }_t^{\mathrm{s}}\\ -(x_t^{\mathrm{s}}-x_i)\sin {\theta }_t^{\mathrm{s}}+(y_t^{\mathrm{s}}-y_i)\cos {\theta }_t^{\mathrm{s}}\end{array}\right)\text{\hspace{1em}(39)}$$

  它将地标的位置转换为机器人的坐标系。

  由于地标位于欧几里德空间中，因此将误差函数计算为法向量差是合理的。 这导致以下关于地标的误差函数的定义：

$${\mathrm{e}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_i^{\mathrm{l}})\doteq {\mathrm{z}}_{t,i}-{\mathrm{h}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_i^{\mathrm{l}})\text{\hspace{1em}(40)}$$

  以类似的方式，我们可以定义连接两个机器人位姿${\mathrm{x}}_t^{\mathrm{s}}$和${\mathrm{x}}_{t+1}^{\mathrm{s}}$的测距边的误差函数。 如前所述，测距法测量存在于$SE(2)$中。 通过使用$\oplus$运算符，我们可以编写一个综合测量函数：

$${\mathrm{h}}_{t,t+1}^{\mathrm{s}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_{t+1}^{\mathrm{s}})\doteq {\mathrm{x}}_{t+1}^{\mathrm{s}}\ominus {\mathrm{x}}_t^{\mathrm{s}}\text{\hspace{1em}(41)}$$

  简而言之，此函数返回机器人从${\mathrm{x}}_t^{\mathrm{s}}$到${\mathrm{x}}_{t+1}^{\mathrm{s}}$的运动，即“理想”的测距。 而且可以再次获得误差，误差作为测量和预测之间的差异。 但是，由于我们的测量不存在于欧几里德空间，我们可以使用$ \ominus$而不是矢量差:

$${\mathrm{e}}_{t,t+1}^{\mathrm{s}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_{t+1}^{\mathrm{s}})\doteq {\mathrm{z}}_{t,t+1}\ominus {\mathrm{h}}_{t,t+1}^{\mathrm{s}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_{t+1}^{\mathrm{s}})\text{\hspace{1em}(42)}$$

整理(Putting things together)

  在这里，我们总结了先前问题定义的相关部分，并为实现做好了准备。

变量	符号	定义域	维数	$\Delta \mathrm{x}$的参数化	$⊞$运算符
相机位姿	${\mathrm{x}}_t^{\mathrm{s}}$	$SE(2)$	$3$	$\left(\begin{array}{ccc}\Delta \mathrm{x}& \Delta \mathrm{y}& \Delta \theta \end{array}\right)$	${\mathrm{x}}_t^{\mathrm{s}}\oplus \Delta {\mathrm{x}}_t^{\mathrm{s}}$
地标位姿	${\mathrm{x}}_i^{\mathrm{l}}$	$\mathfrak{R}(2)$	2	$\left(\begin{array}{cc}\Delta \mathrm{x}& \Delta \mathrm{y}\end{array}\right)$	${\mathrm{x}}_i^{\mathrm{l}}+\Delta {\mathrm{x}}_i^{\mathrm{l}}$

测量	符号	定义域	维数	涉及的变量集${\mathrm{x}}_k$	误差函数
里程计	$={\mathrm{z}}_{t,t+1}^{\mathrm{s}}$	$SE(2)$	3	{ x t s , x t + 1 s } \{{\rm x}_t^{\rm s},{\rm x}_{t+1}^{\rm s}\} {xts​,xt+1s​}	${\mathrm{e}}_{t,t+1}^{\mathrm{s}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_{t+1}^{\mathrm{s}})\doteq {\mathrm{z}}_{t,t+1}\ominus {\mathrm{h}}_{t,t+1}^{\mathrm{s}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_{t+1}^{\mathrm{s}})$
路标	$={\mathrm{z}}_{t,i}^{\mathrm{l}}$	$\mathfrak{R}(2)$	2	{ x t s , x i l } \{{\rm x}_t^{\rm s},{\rm x}_i^{\rm l}\} {xts​,xil​}	${\mathrm{e}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_i^{\mathrm{l}})\doteq {\mathrm{z}}_{t,i}-{\mathrm{h}}_{t,i}^{\mathrm{l}}({\mathrm{x}}_t^{\mathrm{s}},{\mathrm{x}}_i^{\mathrm{l}})$

  我们要做的第一件事是实现一个表示$SE(2)$群元素的类。 我们通过使用旋转矩阵和平移向量表示，通过Eigen :: Geometry中定义的类型在内部表示这些元素。 因此，我们定义了一个实现运动合成运算符$\oplus$的operator *（...），以及一个返回转换逆的inverse()函数。 为方便起见，我们还实现了一个转换2D点的operator*。 为了将元素转换为使用Eigen :: Vector3d的最小表示，我们定义了fromVector（...）和toVector（...）方法。

  构造函数将此类初始化为原点中的面向0度的一个点。

原句是：The constructor initializes this class as a point in
the origin oriented at 0 degrees.这句翻译起来和理解起来让我有点理解困难，大体是是构造函数初始化该类为一个点。然后这个点在原点，朝向0°的方向。其实意思在代码中比较明显，就是$x,y=0，\theta =0^{\circ }$

  请注意，在g2o中，为群创建单独的类不是必需的，但使代码更具可读性和可重用性。 清单3中报告了相应的C ++类。

  一旦我们定义了很好的$SE(2)$群，我们就可以实现顶点了。为此，我们扩展了BaseVertex <>类，我们派生了VertexSE2类来表示机器人位姿，并使用VertexPointXY来表示平面中的点地标。清单4中报告了机器人姿势顶点的类定义。

  位姿顶点(pose-vertex)扩展了BaseVertex <>的模板特化。我们应该对g2o说内部类型的维度为$3$，估计值为SE2。这意味着VertexSE2的成员_estimate是SE2类型。然后我们需要做的就是重新定义setToOriginImpl()方法，将估计重置为已知配置，方法为oplusImpl（double *）。该方法应该应用一个增量，用增量参数化表示（即向量${\left(\begin{array}{ccc}\Delta \mathrm{x}& \Delta \mathrm{y}& \Delta \theta \end{array}\right)}^t$到当前估计。为此，我们首先将作为参数传递的向量转换为SE2，然后我们将此增量相乘在之前的估计的右边。之后我们应该对流实现读写函数，但这很简单，你可以在代码中自己查找。

    bool VertexSE2::read(std::istream& is)
    {
      Eigen::Vector3d p;
      is >> p[0] >> p[1] >> p[2];
      _estimate.fromVector(p);
      return true;
    }

    bool VertexSE2::write(std::ostream& os) const
    {
      Eigen::Vector3d p = estimate().toVector();
      os << p[0] << " " << p[1] << " " << p[2];
      return os.good();
    }

所示的例子在g2o目录下的g2o/example/tutorial_slam2d文件夹下

  下一步是实现顶点来描述地标位置。 由于地标在$\mathfrak{R}(2)$中被参数化，我们不需要为此定义任何群，并且我们直接使用在Eigen中定义的向量类。 清单5中报告了此类。

  现在我们完成了顶点。 我们应该来完成边。 由于两个边都是二元边，我们扩展BaseBinaryEdge<>类。 为了表示连接两个VertexSE2的测距边缘（参见清单6），我们需要扩展BaseBinaryEdge <>，专门用于连接顶点的类型（顺序很重要），其中测量本身由维度为$3$的SE2表示 。

  第二个模板参数是用于成员变量_measurement的参数。 第二步是通过重新定义computeError()方法来构造误差函数。computeError()应该将误差向量放在具有类型Eigen :: Vector <double，3>的成员变量error中。 这里的3来自指定维度的模板参数。 同样，可以在代码中查找读写函数。

    bool EdgeSE2::read(std::istream& is)
    {
      Vector3d p;
      is >> p[0] >> p[1] >> p[2];
      _measurement.fromVector(p);
      _inverseMeasurement = measurement().inverse();
      for (int i = 0; i < 3; ++i)
        for (int j = i; j < 3; ++j) {
          is >> information()(i, j);
          if (i != j)
            information()(j, i) = information()(i, j);
        }
      return true;
    }

    bool EdgeSE2::write(std::ostream& os) const
    {
      Vector3d p = measurement().toVector();
      os << p.x() << " " << p.y() << " " << p.z();
      for (int i = 0; i < 3; ++i)
        for (int j = i; j < 3; ++j)
          os << " " << information()(i, j);
      return os.good();
    }

  现在我们完成了边的构造。Jacobians用g2o数值计算（默认实现）。 但是，如果您希望在一切正常后加快代码执行速度，则会可以重新定义linearizeOplus方法。

  剩下要做的最后一件事是定义一个类来表示一个地标的测量。 在清单7中被显示。

  我们再次扩展了BaseBinaryEdge <>的特化，我们告诉系统它将VertexSE2与VertexPointXY连接，测量由具有维度大小为$2$的Eigen :: Vector2d表示。

  我们应该做的最后一步是让我们的系统运行，就是注册类型让g2o知道有新类型准备就绪。 但是，如果您打算手动构建图而不在磁盘上执行任何$\mathrm{i}/\mathrm{o}$操作，则甚至不需要执行此步骤。

总结

  通过阅读g2o的官方文献，基本大体知道了应该这么使用，下一节运行和调试example,如果有时间，可能看看g2o适不适合写教程。不过目前来说，我时间不算很多。只能尽力看情况，能不能把自己学习过程记录下来了。

---

[1]: g2o: A general Framework for (Hyper) Graph Optimization，March 11, 2017

[2]: SLAM 十四讲，第六章

[3]: Probabilistic Robotics，chapter 11，2005

​