g2o学习（附带SLAM十四讲第二版 第七章作业3d3d代码）

学习完成高博第二版的第七章了，多多少少对slam有些认知，但是对于图优化这个玄妙而又有用的东西理解不深，因此想在这里集中学习一下图优化和g2o的使用。

本文包含两部分内容：

1. 图优化的理解。高博的参考链接：https://www.cnblogs.com/gaoxiang12/p/5244828.html
2. 第七章ICP作业改写的，将pose和point均作为优化对象的g2o代码改写。

下面开始：

0. 预备知识：优化

优化问题三要素：目标函数、优化变量、优化约束

对于一个简单的优化问题： min𝑥𝐹(𝑥)

x为优化变量，F(x)为优化函数，此问题称为无约束优化问题。因为我们没有给出任何约束形式。而slam问题中大多为无约束优化。

当𝐹(𝑥)有一些特殊性质时，对应的优化问题也可以用一些特殊的解法。例如，𝐹(𝑥)为一个线性函数时，则为线性优化问题（不过线性优化问题通常在有约束情形下讨论）。反之则为非线性优化。对于无约束的非线性优化，如果我们知道它梯度的解析形式，就能直接求那些梯度为零的点，来解决这个优化：𝑑𝐹/𝑑𝑥=0

但是有时候F(x)的形式太复杂，导致无法写出解析的形式，因此使用迭代求解。

即：从一个初值𝑥0出发，不断地导致当前值附近的，能使目标函数下降的方式（反向梯度），然后沿着梯度方向走出一步，从而使得函数值下降一点。这样反复迭代，理论上对于任何函数，都能找到一个极小值点。

迭代的策略主要体现在如何选择下降方向，以及如何选择步长两个方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法两种，它们的细节可以在高博的书上找到，也就是第六章非线性优化，如何求x的变化量δx的方法。

1. 图优化基础及顶点和边

讲优化问题用图表达。

接下来理解图到底是什么

图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。我们记一个图为𝐺={𝑉,𝐸}，其中𝑉为顶点集，𝐸为边集。

顶点就是顶点，在问题中代表一个结点。

边是什么呢？一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最常见的边连接两个顶点，也就是接下来要将的BA方法解3d3d问题，将空间点和位姿两个优化变量作为顶点，这样就是一个二元边。当一个图中存在连接两个及以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就可以表示成一个超图（在不引起歧义的情况下，后文直接以图指代超图）

怎么把SLAM问题表示成图呢？
　SLAM的核心是根据已有的观测数据，计算机器人的运动轨迹和地图。假设在时刻𝑘，机器人在位置𝑥𝑘处，用传感器进行了一次观测，得到了数据𝑧𝑘。传感器的观测方程为：

由于误差的存在，𝑧𝑘不可能精确地等于ℎ(𝑥𝑘)，于是就有了误差：

那么，如果我们以𝑥𝑘为优化变量，以

为目标函数，就可以求得𝑥𝑘的估计值，进而得到我们想要的东西了。这实际上就是用优化来求解SLAM的思路。

以3d3d问题为例（3d3d比3d2d更容易理解），我们得到的是两组相机坐标系下的观测点（为左相机和右相机，并且运动方向是左到右）。我们知道这两组点是已经匹配好的，也就是在某种意义上，他们是同一个点，只不过在不同的坐标系下。但实际上二者并不严格重合，因为，有观测误差。那么我们可以将观测值Zk理解为左相机的三维观测点，观测方程理解为一种动作，也就是如何妄图获得观测值的动作。在这里，我们已经定义了左相机的三维点坐标为观测值，那么我想用右相机，做出某个动作，复现左相机的观测值。

这个动作是什么呢？那就是 TP‘。

我们用P’来表达三维点在右相机中的坐标，用P表达三维点在左相机中的坐标。TP‘就是我们妄图用右相机的三维点坐标去复现左相机的三维点坐标。这就是观测方程（理解为一种，想要复现观测值的动作）。具体示意图：

此观测方程包含两部分：

1. 两个相机间pose的变换T
2. 右3D点坐标P’

那么我们优化的目标就是：

P - TP’

那么如何讲他们表达成图呢？

在图中，以顶点表示优化变量，以边表示观测方程。由于边可以连接一个或多个顶点，所以我们把它的形式写成更广义的 𝑧𝑘=ℎ(𝑥𝑘1,𝑥𝑘2,…)，以表示不限制顶点数量的意思。对于刚才提到的3d3d情境下的观测方程，顶点和边是什么形式呢？

先看顶点。

顶点就是优化变量。前面讲过，优化目标是P - TP’，那么我们来看看这中间有什么不知道。这个问题是在视觉里程计中引入的。我们在这个式子中要求的第一个要素就是T，即我们要获得两个相机间的位姿（其实是一个相机，之所以叫两个相机是因为从第一帧的位置，也就是左相机，变换到第二帧的位置，也就是右相机）。

因此顶点一，就是T，也就是相机的位姿。

如果我们讲右相机的三维观测点P‘也考虑为优化变量也是可以的。根据高博的书中所说：让整个问题更加“灵活”。

那么顶点二，就是P’

再看边。

前面说了，将观测方程表示为边。（或者将求误差的动作理解为边，或者将误差理解为边，我建议将动作理解为边）

在这个3d3d的问题中，P - TP’这个动作就可以理解为是边。那么在定义边的时候要注意几个问题：

1.P是边的要素，是观测值，也就是我们要传入的左相机原始三维点坐标数据。
2.既然要完成P - TP’的动作，因此有了P，还需要调用TP’，这就涉及到之前定义的两个顶点的调用。
3.雅克比矩阵的定义。因为你要告诉图，你的优化方向（也就是梯度），以便于实现对T和P’的更新。雅克比矩阵的事情以后再说。

至此定义完毕，简而言之，优化变量为顶点，求误差的动作为边。

那么在这个问题里，有几个匹配的三维点，就有几条边。

最终所有边和顶点，汇聚（summary）为一个优化问题，也就是将所有误差均考虑在内：

其中pi和pi’表示每个匹配好的左右3D点，对于每个误差取2范数（每一项的平方和）最终求和，构成了一个

最小二乘问题

那么图优化的目标函数，可以看做这个最小二乘问题

2. 图优化的进行

现在让我们来仔细看一看图优化是怎么做的。假设一个带有𝑛条边的图，其目标函数可以写成：

是不是和我们上面提及的最小二乘问题非常之像。

这里高博提供了几句话，对这个目标函数进行理解：

1.e(x, z)可以这么理解，z是观测值，xk是用展开的动作，实现对z的靠近；此外，Ω表示信息矩阵，在高博第二版的书中的第P124，表示我们对误差各分量的不一致性。。

2.信息矩阵 Ω实际上是协方差矩阵（在本例中为观测方程的协方差矩阵）的逆。可以看做是对误差的加权，最简单的是把Ω设成对角矩阵，这样只考虑观测值本身的误差，而不考虑相关性。

3.直接将e(x, z)写成e(x)，以简洁表示。因为z是已知的。（观测值嘛）

于是总体优化问题变为𝑛条边加和的形式：

高博原文：

这是一个典型的3d2d问题。通过字里行间应该能分析出来，zk是观测值，是观测的右相机的二维像素，而对其进行观测，这个观测动作是什么呢？也就是要靠近这个右相机的二维像素的动作。也就是将左相机的三维点进行R t K等的变换，投影到右相机的二维像素平面。误差就是前后两个二位像素坐标的差值。总误差即为很多个观测点构成的最小二乘问题。

在这梳理一下如何在一个问题中梳理出来观测值和观测动作（或者是产生误差的动作）。

首先确定观测值，观测值是不进行优化的部分，是为了和之后动作产生的结果进行做差值的基本数据。3d2d问题中，我们首先选定了右相机的二维坐标为观测值。

接下来确定观测动作（或者是产生误差的动作）。也就是边。简而言之，就是我们如何用我们已有的东西（待优化的顶点，或者是顶点和已知数据的组合）去靠近之前的观测值。那么3d2d问题中，我们选定了右相机二维像素作为观测值，若想逼近观测值，那么就要用左相机的3D点，通过旋转平移和内参的关系，先转换到右相机坐标系，再转换到右相机像素坐标系，最终，与之前的观测值形成误差。

大致就是这么个过程。

此外，在定义边的时候，要定义雅克比矩阵。这里就不详细说了，雅克比矩阵如何求，就是误差e对不同的待优化变量求偏导数，这个在高博的书上都有很多，比如如何对位姿T的李代数δξ求导，如何对三维点P求导，这个在3d2d和3d3d的例程中都有详细的说明。

3. 流形

由于定义梯度的时候，待优化变量使用的加法δx，但是要注意，有可能待优化变量是没有加法定义的。（如T，即SE(3)）

回想我们先前讲过的李代数知识。虽然李群 𝑆𝐸(3) 和 𝑆𝑂(3) 是没有加法的，但是它们对应的李代数 𝔰𝔢(3),𝔰𝔬(3) 有啊！ 数学一点地说，我们可以求它们在正切空间里的流形上的梯度！如果读者觉得理解困难，我们就说，通过指数变换和对数变换，先把变换矩阵和旋转矩阵转换成李代数，在李代数上进行加法，然后再转换到原本的李群中。这样我们就完成了求导。

这也就是为什么我们在定义位姿pose顶点的时候，

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>

6就代表了目标待优化状态变量Sophus::SE3d的流形是一个含有6个元素的东西。因为SE3的李代数就是6个元素的向量。
g2o中，这6个元素以double存储

4. 3d3d代码详解

见另外一片博客