图优化本质上是一个优化问题，所以我们先来看优化问题是什么。
优化问题有三个最重要的因素：目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下:
其中x为优化变量，而F(x)为优化函数。此问题称为无约束优化问题，因为我们没有给出任何约束形式。由于slam中优化问题多为无约束优化，所以我们着重介绍无约束的形式。
当F(x)有一些特殊性质时，对应的优化问题也可以用一些特殊的解法。例如，F(x)为一个线性函数时，则为线性优化问题（不过线性优化问题通常在有约束情形下讨论）。反之则为非线性优化。对于无约束的非线性优化，如果我们知道它梯度的解析形式，就能直接求那些梯度为零的点，来解决这个优化：
梯度为零的地方可能是函数的极大值、极小值或者鞍点。由于现在F(x)的形式不确定，我们只好遍历所有的极值点，找到最小的作为最优解。
但是我们为什么不这样用呢？因为很多时候F(x)的形式太复杂，导致我们没法写出导数的解析形式，或者难以求解导数为零的方程。因此，多数时候我们使用迭代方式求解。从一个初值x0出发，不断地导致当前值附近的，能使目标函数下降的方式（反向梯度），然后沿着梯度方向走出一步，从而使得函数值下降一点。这样反复迭代，理论上对于任何函数，都能找到一个极小值点。
迭代的策略主要体现在如何选择下降方向，以及如何选择步长两个方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法两种。
图优化所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。
图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。我们记一个图为G={ V,E }，其中V为顶点集，E为边集。
边一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最常见的边连接两个顶点。当一个图中存在连接两个以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就可以表示成一个超图（在不引起歧义的情况下，后文直接以图指代超图）。
SLAM的核心是根据已有的观测数据，计算机器人的运动轨迹和地图。假设在时刻k，机器人在位置xk处，用传感器进行了一次观测，得到了数据。传感器的观测方程为：
由于误差的存在，不可能精确地等于，于是就有了误差：
那么，如果我们以为优化变量，以minxFk()=∥ek∥为目标函数，就可以求得的估计值，进而得到我们想要的东西了。这实际上就是用优化来求解SLAM的思路。
x可以是一个机器人的Pose（6自由度下为 4×4的变换矩阵T 或者 3自由度下的位置与转角[x,y,θ]，也可以是一个空间点（三维空间的[x,y,z]或二维空间的[x,y]）。相应的，观测方程也有很多形式，如：
机器人两个Pose之间的变换；机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度；机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标；同样，它们的具体形式很多样化，这允许我们在讨论slam问题时，不局限于某种特定的传感器或姿态表达方式。
在图中，以顶点表示优化变量，以边表示观测方程。由于边可以连接一个或多个顶点，所以我们把它的形式写成更广义的zk=h(xk1,xk2,…)，以表示不限制顶点数量的意思。对于刚才提到的三种观测方程，顶点和边是什么形式呢？
机器人两个Pose之间的变换；——一条Binary Edge（二元边），顶点为两个pose，边的方程为T1=ΔT⋅T2。机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度；——Binary Edge，顶点为一个2D Pose：[x,y,θ]T和一个Point：[λx,λy]T，观测数据是距离r和角度b，那么观测方程为：机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标；——Binary Edge，顶点为一个3D Pose：T和一个空间点x=[x,y,z]T，观测数据为像素坐标z=[u,v]T。那么观测方程为：
C为相机内参，R,t为旋转和平移。
由于机器人可能使用各种传感器，故我们不限制顶点和边的参数化之后的样子。比如我既加了激光，也用相机，还用了IMU，轮式编码器，超声波等各种传感器来做slam。为了求解整个问题，我的图中就会有各种各样的顶点和边。但是不管如何，都是可以用图来优化的。
图优化怎么做现在让我们来仔细看一看图优化是怎么做的。假设一个带有n条边的图，其目标函数可以写成：
e 函数在原理上表示一个误差，是一个矢量，作为优化变量和符合程度的一个度量。它越大表示越不符合。但是，由于目标函数必须是标量，所以必须用它的平方形式来表达目标函数。最简单的形式是直接做成平方：e(x,z)Te(x,z)。进一步，为了表示我们对误差各分量重视程度的不一样，还使用一个信息矩阵 Ω 来表示各分量的不一致性。信息矩阵 Ω 是协方差矩阵的逆，是一个对称矩阵。它的每个元素作为的系数，可以看成我们对这个误差项相关性的一个预计。最简单的是把Ω设成对角矩阵，对角阵元素的大小表明我们对此项误差的重视程度。这里的可以指一个顶点、两个顶点或多个顶点，取决于边的实际类型。所以，更严谨的方式是把它写成，但是那样写法实在是太繁琐，我们就简单地写成现在的样子。由于是已知的，为了数学上的简洁，我们再把它写成的形式。于是总体优化问题变为n条边加和的形式：
重复一遍，边的具体形式有很多种，可以是一元边、二元边或多元边，它们的数学表达形式取决于传感器或你想要描述的东西。例如视觉SLAM中，在一个相机Pose 处对空间点xk进行了一次观测，得到，那么这条二元边的数学形式即为:
单个边其实并不复杂。
现在，我们有了一个很多个节点和边的图，构成了一个庞大的优化问题。我们并不想展开它的数学形式，只关心它的优化解。那么，为了求解优化，需要知道两样东西：一个初始点和一个迭代方向。为了数学上的方便，先考虑第k条边吧。
我们假设它的初始点为，并且给它一个Δx的增量，那么边的估计值就变为，而误差值则从 变为 。首先对误差项进行一阶展开：
这是的Jk是ek关于xk的导数，矩阵形式下为雅可比阵。我们在估计点附近作了一次线性假设，认为函数值是能够用一阶导数来逼近的，当然这在Δx很大时候就不成立了。
于是，对于第k条边的目标函数项，有：
在熟练的同学看来，这个推导就像一样简单（事实上就是好吧）。最后一个式子是个定义整理式，我们把和Δx无关的整理成常数项 ，把一次项系数写成 ，二次项则为 （注意到二次项系数其实是Hessian矩阵）。
请注意 实际就是该边变化前的取值。所以在发生增量后，目标函数项改变的值即为:
我们的目标是找到Δx，使得这个增量变为极小值。所以直接令它对于Δx的导数为零，有：
所以归根结底，我们求解一个线性方程组 ：
在每一步迭代中，我们都要求解一个雅可比和一个海塞。而一个图中经常有成千上万条边，几十万个待估计参数，这在以前被认为是无法实时求解的。但是SLAM构建的图，并非是全连通图，它往往是很稀疏的。例如一个地图里大部分路标点，只会在很少的时刻被机器人看见，从而建立起一些边。大多数时候它们是看不见的（就像后宫怨女一样）。体现在数学公式中，虽然总体目标函数有很多项，但某个顶点就只会出现在和它有关的边里面！
这会导致什么？这导致许多和无关的边，比如说，对应的雅可比就直接是个零矩阵！而总体的雅可比中，和有关的那一列大部分为零，只有少数的地方，也就是和顶点相连的边，出现了非零值。
相应的二阶导矩阵H中，大部分也是零元素。这种稀疏性能很好地帮助我们快速求解上面的线性方程。稀疏代数库包括SBA、PCG、CSparse、Cholmod等等。g2o正是使用它们来求解图优化问题的。要补充一点的是，在数值计算中，我们可以给出雅可比和海塞的解析形式进行计算，也可以让计算机去数值计算这两个阵，而我们只需要给出误差的定义方式即可。
流形我们在讨论给目标函数F(x)一个增量Δx时，直接就写成了F(x+Δx)。但是这个加法可能没有定义！由于我们不限制顶点的类型，x在参数化之后，很可能是没有加法定义的。最简单的就是常见的四维变换矩阵T或者三维旋转矩阵R。它们对加法并不封闭，因为两个变换阵之和并不是变换阵，两个正交阵之和也不是正交阵。它们乘法的性质非常好，但是确实没有加法，所以也不能像上面讨论的那样去求导。
但是，如果图优化不能处理SE(3)或SO(3)中的元素，那将是十分令人沮丧的，因为SLAM要估计的机器人轨迹必须用它们来描述啊。
回想我们先前讲过的李代数知识。虽然英俊的老师 SE(3) 和 SO(3) 是没有加法的，但是它们对应的李代数 se(3),so(3) 有啊！ 数学一点地说，我们可以求它们在正切空间里的流形上的梯度！如果读者觉得理解困难，我们就说，通过指数变换和对数变换，先把变换矩阵和旋转矩阵转换成李代数，在李代数上进行加法，然后再转换到原本的英俊的老师中。这样我们就完成了求导。
这样的好处是我们完全不用重新推导公式。这件事比我们想的更加简单。在程序里，我们只需重新定义一个优化变量x的增量加法即可。如果x是一个SE(3)里的变换矩阵，我们就遵守刚才讲的李代数转换方式。当然，如果x是其他什么奇怪的东东，只要定义了它的加法，程序就会自动去计算如何求它的雅可比。
核函数引入核函数的原因，是因为SLAM中可能给出错误的边。SLAM中的数据关联让科学家头疼了很长时间。出于变化、噪声等原因，机器人并不能确定它看到的某个路标，就一定是数据库中的某个路标。万一认错了呢？我把一条原本不应该加到图中的边给加进去了，会怎么样？
嗯，那优化算法可就慒逼了……它会看到一条误差很大的边，然后试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。
于是就有了核函数的存在。核函数保证每条边的误差不会大的没边，掩盖掉其他的边。具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没法求导啊！）。因为它们使得整个优化结果更为鲁棒（所谓鲁棒性，是指控制系统在一定（结构，大小）的参数摄动下，维持某些性能的特性），所以又叫它们为robust kernel（鲁棒核函数）。
很多鲁棒核函数都是分段函数，在输入较大时给出线性的增长速率，例如cauchy核，huber核等等。当然具体的我们也不展开细说了。
函数在许多优化环境中都有应用，博主个人印象较深的时当年有一大堆人在机器学习算法里加各种各样的核，我们现在用的svm也会带个核函数。
小结最后总结一下做图优化的流程。
选择你想要的图里的节点与边的类型，确定它们的参数化形式；往图里加入实际的节点和边；选择初值，开始迭代；每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；求解稀疏线性方程HkΔx=−bk，得到梯度方向；继续用GN或LM进行迭代。如果迭代结束，返回优化值。实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。
转载自：http://www.cnblogs.com/gaoxiang12/p/5244828.html