视觉SLAM⑥---非线性优化

目录

6.0 主要目标

6.1 SLAM问题的数学表达 

6.2 状态估计问题 

6.2.1 批量状态估计与最大后验估计

6.1.2 最小二乘的引出 

6.1.3 例子:批量状态估计

6.2 非线性最小二乘

6.2.1 一阶和二阶梯度法 

6.2.2 高斯牛顿法 

6.2.3 列文伯格—马夸尔特方法

6.3 实践 ：曲线拟合问题

6.3.1 手写高斯牛顿法

6.3.2 使用 Ceres进行曲线拟合

6.3.3 使用g2o进行曲线拟合

6.4 小结 

---

6.0 主要目标

主要目标：
1.理解最小二乘法的含义和处理方式。
2.理解高斯牛顿法(Gauss-Newton's method)、列文伯格—马夸尔特方法(Levenburg-
-Marquadt's method）等下降策略。
3.学习Ceres库和g2o库的基本使用方法。

        在前面几讲，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。于是，我们已经弄清了经典SLAM模型在视觉情况下的具体表达。
        然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的
即使我们有高精度的相机，运动方程和观测方程也只能近似成立。所以，与其假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。
        解决状态估计问题需要一定程度的最优化背景知识。本节将介绍基本的无约束非线性优化方法，同时介绍优化库g2o和Ceres的使用方式。

6.1 SLAM问题的数学表达 

        假设小萝卜正携带着某种传感器在未知环境里运动，怎么用数学语言描述这件事呢？

        首先，由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。这就把一串连续时间的运动变成了离散时刻当中发生的事情。在这些时刻，用表示小萝卜自身的位置。于是各时刻的位置就记为，它们构成了小萝卜的轨迹。

        地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有个，用 表示它们。
        在这样的设定中，“小萝卜携带着传感器在环境中运动”，由如下两件事情描述：
①什么是运动？        我们要考察从时刻到时刻，小萝卜的位置是如何变化的。
②什么是观测？        假设小萝卜在时刻于处探测到了某一个路标，我们要考察如何用
数学语言来描述这件事情。
        先来看运动。通常，机器人会携带一个测量自身运动的传感器。这个传感器可以测量有关运动的读数，但不一定直接就是位置之差，还可能是加速度角速度等信息。有时我们也给小萝卜发送指令，例如“前进1米”“左转90°”，或者“油门踩到底”“刹车”等。无论是何种情况，我们都能使用一个通用的、抽象的数学模型来说明此事：

                                                        

        这里，是运动传感器的读数或者输入，为该过程中加入的噪声。注意，我们用一个一般函数于来描述这个过程，而不指明具体的作用方式。这使得整个函数可以指代任意的运动传感器/输入，成为一个通用的方程，而不必限定于某个特殊的传感器上。我们把它称为运动方程。
        噪声的存在使得这个模型变成了随机模型。换句话说，即使我们下达“前进1米”的命令,也不代表小萝卜真的前进了1米。如果所有指令都是准确的，也就没必要估计了。事实上，小萝卜可能某次只前进了0.9米，另一次前进了1.1米，再一次可能由于轮胎打滑，干脆没有前进。于是，每次运动过程中的噪声是随机的。如果我们不理会这个噪声，那么只根据指令来确定的位置可能与实际位置相差十万八千里。
        与运动方程相对应，还有一个观测方程。观测方程描述的是，当小萝卜在位置上看到某个路标点时，产生了一个观测数据。同样，用一个抽象的函数来描述这个关系：

                                                        

        这里，是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据及观测方程也有许多不同的形式。
        我们用的函数，似乎并没有具体地说明运动和观测是怎么回事？同时，这里的又是什么呢？事实上，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化(Parameterization)方式。

        什么叫参数化呢？举例来说，假设小萝卜在平面中运动，那么，它的位姿由两个位置和一个转角来描述，即，其中是两个轴上的位置而为转角。同时，输入的指令是两个时间间隔位置和转角的变化量，于是，此时运动方程就可以具体化为：

                                                   

        这是简单的线性关系。不过，并不是所有的输人指令都是位移和角度的变化量,例如“油门”或者“控制杆”的输入就是速度或加速度量，所以也存在着其他形式更加复杂的运动方程，那时我们可能需要进行动力学分析。
        关于观测方程，以小萝卜携带着的一个二维激光传感器为例。我们知道激光传感器观测一个2D路标点时，能够测到两个量:路标点与小萝卜本体之间的距离和夹角。记路标点为，位姿为，观测数据为，那么观测方程就写为：

                                        

        考虑视觉SLAM时，传感器是相机，那么观测方程就是“对路标点拍摄后，得到图像中的像素”的过程。这个过程牵涉相机模型的描述，在第5讲中详细介绍。
        可见，针对不同的传感器，这两个方程有不同的参数化形式。如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程：

                                        

                                         

        其中是一个集合，记录着在哪个时刻观察到了哪个路标（通常不是每个路标在每个时刻都能看到的——我们在单个时刻很可能只看到一小部分)。

        这两个方程描述了最基本的SLAM问题：当知道运动测量的读数，以及传感器的读数时，如何求解定位问题（估计）和建图问题（估计）?这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量?

        状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关。按照运动和观测方程是否为线性、噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

        其中线性高斯系统(Linear Gaussian，LG系统)是最简单的，它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter，KF)给出。

        而在复杂的非线性高斯系统 (Non-Linear Non-Gaussian，NLNG系统) 中，我们会使用以扩展卡尔曼滤波器(Extended Kalman Filter，EKF)和非线性优化两大类方法去求解。直至21世纪早期，以EKF为主的滤波器方法在SLAM中占据了主导地位。我们会在工作点处把系统线性化，并以预测—更新两大步骤进行求解(见第10讲)。最早的实时视觉SLAM系统就是基于EKF2开发的。随后，为了克服 EKF的缺点(例如线性化误差和噪声高斯分布假设)，人们开始使用粒子滤波器(Particle Filter)等其他滤波器，乃至使用非线性优化的方法。时至今日，主流视觉SLAM使用以图优化(Graph Optimization)为代表的优化技术进行状态估计。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法（见第10讲和第11讲)。

6.2 状态估计问题 

6.2.1 批量状态估计与最大后验估计

1.没有噪声时的运动与观测模型

        由6.1可知，经典SLAM模型由一个运动方程和一个观测方程构成：   

式6-1 SLAM问题的数学表达

        通过第4讲，是相机的位姿，可以用来描述。

        观测方程，第5讲已经说明，即针孔相机模型。

        为了对它们有更深的印象，我们不妨讨论其具体参数化形式。

        首先，位姿变量可以由表达。其次，运动方程与输入的具体形式有关，但在视觉SLAM中没有特殊性(和普通的机器人、车辆的情况一样)，我们暂且不谈。

        观测方程则由针孔模型给定。假设在处对路标进行了一次观测，对应到图像上的像素位置，那么，观测方程可以表示成：

                                                        

式6-2 观测方程的参数化表示

         其中为相机内参，为像素点的距离，也是的第三个分量。如果使用变换矩阵描述位姿，那么路标点必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。

图6-1 像素点的距离s指什么

2.加入噪声后出现的状态估计问题

         现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项i满足零均值的高斯分布，像这样：

                                                

        其中表示高斯分布，0表示零均值，为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据和推断位姿和地图(以及它们的概率分布)，这构成了一个状态估计问题。（即根据像素点位置和终端输入推断机器人的位置以及路标点的位置）

3.两种处理状态估计的办法(如何根据像素点估计出图像原来的位置)

        处理这个状态估计问题的方法大致分成两种。由于在SLAM过程中，这些数据是随时间逐渐到来的，所以，我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量/渐进(incremental)的方法，或者叫滤波器。在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器及其衍生方法求解它。

        另一种方式，则是把数据“攒”起来一并处理，这种方式称为批量(batch)的方法。例如，我们可以把0到k时刻所有的输入和观测数据都放在一起。问？在这样的输入和观测下，如何估计整个0到k时刻的轨迹与地图呢？

        (给出所有的 or 部分)  

        这两种不同的处理方式引出了很多不同的估计手段。

        大体来说，增量方法仅关心当前时刻的状态估计​​​​​​​，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。

        极端情况下，我们可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这也正是SfM (Structure from Motion)的主流做法。

        当然，这种极端情况显然是不实时的，不符合SLAM的运用场景。所以在SLAM中，实用的方法通常是一些折衷的手段。例如，我们固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，这是后面要讲到的滑动窗口估计法。

4.批量法进行状态估计

        理论上，批量方法更容易介绍。同时，理解了批量方法也更容易理解增量的方法。所以，本节我们重点介绍以非线性优化为主的批量优化方法，将卡尔曼滤波器及更深入的知识留到介绍后端的章节再进行讨论。由于讨论的是批量方法，考虑从1到N的所有时刻，并假设有M个路标点。定义所有时刻的机器人位姿和路标点坐标为:

式6-3 关于位姿和观看的所有信息

         同样地，用不带下标的表示所有时刻的输入，表示所有时刻的观测数据。那么我们说，对机器人状态的估计，从概率学的观点来看，就是已知输入数据和观测数据的条件下，求状态的条件概率分布：（机器人位姿，路标点）

        注：这里为什么说概率分布而不是概率？因为我们是想通过像素图片得到近似具体的物体的坐标，如果没有误差的话用公式就能算出具体位置，但因为有误差，我们不可能100%确定物体的确定位置，因此我们需要估计每个点是该点的概率，得到像素点落在世界坐标系下所有点的概率后，取概率最大的为真实估计位置。

式6-4 本章核心问题

         特别地，当我们不知道控制输入，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计的条件概率分布，此问题也称为SfM，即如何从许多图像中重建三维空间结构。
        为了估计状态变量的条件分布，利用贝叶斯法则，有

式6-5 贝叶斯公式

 

         贝叶斯法则左侧称为后验概率，右侧的称为似然(Likehood )，另一部分称为先验(Prior)。

        直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，则是可行的：

        注：直接求后验概率不好求，转化为似然和先验的乘积，即求他俩的概率分布，他俩概率最大那么后验概率也是最大

式6-6 最大后验估计

         请注意贝叶斯法则的分母部分与待估计的状态无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积。当然，我们也可以说，对不起，我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计(MaximizeLikelihood Estimation，MLE)：

式6-7 将问题转化为求解最大似然估计

        直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

6.1.2 最小二乘的引出 

1.最大似然估计的求解步骤

        那么，如何求最大似然估计呢？

        我们说，在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

式6-8 预测像素坐标（根据路标点 + 自己位姿）+噪声影响

         由于我们假设了噪声项，所以观测数据的条件概率是

式6-9 观测数据的条件概率

        注：这个式子的意义是已知机器人位姿和观测到的路标下求像素坐标系下像素坐标，正常情况下是能直接求的，但是此时有噪声，需要将噪声考虑进去；但是怎么将噪声考虑进去呢？我们引入上述分布，已知其符合正太分布，求在初始值条件下最可能的像素坐标系坐标，这时用到最大似然估计。

         它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。
        我们知道高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布，它的概率密度函数展开形式为：

        （现在要求,当对P(x)求最大化时，化为对数）

式6-10 高维高斯分布概率密度函数展开形式

         对其取负对数，则变为：

        

式6-11 高维高斯分布概率密度函数展开形式取负对数

         

        因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。

        在最小化上式的时，第一项与无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。

        如果读者对上述推导不明所以，博主下面图片的思考可能会帮助你

       

图6-2 解释最大似然估计求解

2.将最大似然估计的思想带入SLAM模型

        代入SLAM的观测模型，相当于在最小化

        所有量加在一起：（这就是最小二乘）（均值有观测模型描述，方差由噪声描述）

        我们把状态最大似然估计变成了最小二乘问题

式6-12 最大似然估计的转换形式（只取右侧）

        问题转化逻辑：原本要求 

        我们发现，该式等价于最小化噪声项(即误差)的一个二次型。这个二次型称为马哈拉诺比斯距离(Mahalanobis distance )，又叫马氏距离。它也可以看成由加权之后的欧氏距离(二范数)，这里也叫作信息矩阵，即高斯分布协方差矩阵之逆。
        现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解:

式6-13 联合分布进行因式分解

         这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

        

式6-14 运动误差，观测误差

        那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：

        

式6-15 定义各次输入和观测数据与模型之间的误差 最小化误差的二范数

        （中间的矩阵是协方差的逆矩阵称为信息矩阵，调整各分量噪声关系，比如哪个分量上噪声比较小，认为这部分测量是正确的，噪声小，协方差小，信息求逆之后大，信息就更加看重些，权值就高一些） 

        这样就得到了一个最小二乘问题(Least Square Problem )，它的解等价于状态的最大似然估计。直观上看，由于噪声的存在，当我们把估计的轨迹与地图代人SLAM的运动、观测方程中时，它们并不会完美地成立。这时怎么办呢？

        我们对状态的估计值进行微调，使得整体的误差下降一些。当然，这个下降也有限度，它一般会到达一个极小值。这就是一个典型的非线性优化的过程。

        仔细观察式(6.13)，我们发现SLAM中的最小二乘问题具有一些特定的结构:
        ·首先，整个问题的目标函数由许多个误差的(加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与有关，观测误差只与有关。这种关系会让整个问题有一种稀疏的形式，我们将在介绍后端的章节中看到。
        ·其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入 s.t. 且det(R)=1这样令人头大的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。
        ·最后，我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如,某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。我们之后也会看到它存在一些问题，但是目前先不讨论。
        现在，我们介绍如何求解这个最小二乘问题，这需要一些非线性优化的基本知识。特别地,我们要针对这样一个通用的无约束非线性最小二乘问题，探讨它是如何求解的。在后续几讲中,我们会大量使用本讲的结果，详细讨论它在SLAM前端、后端中的应用。

6.1.3 例子:批量状态估计

        考虑一个非常简单的离散时间系统
        这可以表达一辆沿轴前进或后退的汽车。第一个公式为运动方程,为输入，为噪声;第二个公式为观测方程，为对汽车位置的测量。取时间k= 1,...,3，现希望根据已有的(噪声，路标)进行状态估计。设初始状态已知。下面来推导批量状态的最大似然估计：

               ​​​​​​​        ​​​​​​​        ​​​​​​​      ​​​​​​​       ​​​​​​​

                                                          

        首先，令批量状态变量为，令批量观测为，按同样方式定义。按照先前的推导，我们知道最大似然估计为:

                                        

        对于具体的每一项,比如运动方程，我们知道：

        观测方程也是类似的：

        根据这些方法，我们就能够实际地解决上面的批量状态估计问题。根据之前的叙述，可以构建误差变量：

        于是最小二乘的目标函数为：
        此外，这个系统是线性系统，我们可以很容易地将它写成向量形式。定义向量，那么可以写出矩阵，使得：

        那么： 

        且，整个问题可以写成

                
        之后我们将看到,这个问题有唯一的解：

                

6.2 非线性最小二乘

        一个简单的最小二乘问题：

                                                        

        其中，自变量，是任意标量非线性函数。注意这里的系数，是无关紧要的，有些文献上带有这个系数，有些文献则不带，它不会影响之后的结论。

        下面讨论如何求解这样一个优化问题。显然，如果是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解的最优值，就和求二元函数的极值一样：

                                                       

         解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要逐个比较它们的函数值大小即可。

        但是，这个方程是否容易求解呢？这取决于导函数的形式。

        如果为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质，而通常这是不大可能的。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

图6-1 迭代方式求解极值的方法

图6-2 迭代方式求解极值的方法

        这让求解导函数为零的问题变成了一个不断寻找下降增量的问题，我们将看到，由于可以对进行线性化，增量的计算将简单很多。

        当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。在这个过程中，问题在于如何找到每次迭代点的增量，而这是一个局部的问题，我们只需要关心f在迭代值处的局部性质而非全局性质。这类方法在最优化、机器学习等领域应用非常广泛。
        接下来，我们考察如何寻找这个增量。这部分知识实际属于数值优化的领域，我们来看—些广泛使用的结果。

6.2.1 一阶和二阶梯度法 

        现在考虑第k次迭代，假设我们在处，想要寻到增量，那么最直观的方式是将目标函数在附近进行泰勒展开：

                      

        其中是关于的一阶导数〔也叫梯度、雅可比(Jacobian)矩阵〕，则是二阶导数〔海塞(Hessian)矩阵〕，它们都在处取值。我们可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。

        （一阶二阶是考虑用雅可比去确定下降梯度或者是两者结合确定下降梯度）

        如果保留一阶梯度，那么取增量为反向的梯度，即可保证函数下降：

                                                                

        当然这只是个方向，通常我们还要再指定一个步长。步长可以根据一定的条件来计算，在机器学习中也有一些经验性质的方法。这种方法被称为最速下降法。

        它的直观意义非常简单，只要我们沿着反向梯度方向前进，在一阶(线性)的近似下，目标函数必定会下降。

        注意，以上讨论都是在第k次迭代时进行的，并不涉及其他的迭代信息。所以为了简化符号，后面我们省略下标k，并认为这些讨论对任意一次迭代都成立。

        另外,我们可选择保留二阶梯度信息，此时增量方程为
                ​​​​​​​        ​​​​​​​        

        右侧只含的零次、一次和二次项。求右侧等式关于的导数并令它为零，得到：

                                               

        求解这个线性方程,就得到了增量。该方法又称为牛顿法：
        我们看到，一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。事实上，我们用一个一次或二次的函数近似了原函数，然后用近似函数的最小值来猜测原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的(这也是现实中的情形)。不过，这两种方法也存在它们自身的问题。

        最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。

图6-2 最速下降法问题

        牛顿法则需要计算目标函数的Hession矩阵，这在问题规模较大时非常困难，我们通常倾向于避免Hession的计算。

        对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更实用的方法：高斯牛顿法和列文伯格一马夸尔特方法。

6.2.2 高斯牛顿法 

        高斯牛顿法是最优化算法中最简单的方法之一。它的思想是将进行一阶的泰勒展开。请注意这里不是目标函数而是，否则就变成牛顿法了。

                                             

        这里为关于 的导数，为的列向量。根据前面的框架，当前的目标是寻找增量，使得 达到最小。为了求，我们需要解一个线性的最小二乘问题：

                                        

        这个方程与之前的有什么不一样呢？根据极值条件，将上述目标函数对求导，并令导数为零。为此，先展开目标函数的平方项：

        

        求上式关于的导数,并令其为零：

        

        可以得到以下方程组：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        

                                                                           

        这个方程是关于变量的线性方程组,我们称它为增量方程，也可以称为高斯牛顿方程
( Gauss-Newton equation)或者正规方程(Normal equation )。我们把左边的系数定义为、右边定义为、那么上式变为

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​         

        这里把左侧记作是有意义的。对比牛顿法可见，高斯牛顿法用作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H的过程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程,那么高斯牛顿法的算法步骤可以写成：

        从算法步骤中可以看到，增量方程的求解占据着主要地位。只要我们能够顺利解出增量，就能保证目标函数能够正确地下降。
        为了求解增量方程，我们需要求解，这需要矩阵可逆，但实际数据中计算得到
却只有半正定性。（为什么要算，）

        也就是说，在使用高斯牛顿法时，可能出现为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。

        更严重的是，就算我们假设非奇异也非病态，如果我们求出来的步长太大，也会导致我们采用的局部近似式不够准确，这样一来我们甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。
        尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法，值得我们学习。在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过自己的改进修正其缺点。例如，一些线搜索方法 (Line Search Method )，加入了一个步长，在确定了后进一步找到使得达到最小，而不是简单地令。
        列文伯格一马夸尔特方法在一定程度上修正了这些问题。一般认为它比高斯牛顿法更为健壮，但它的收敛速度可能比高斯牛顿法更慢，被称为阻尼牛顿法（Damped Newton Method )。

6.2.3 列文伯格—马夸尔特方法

        （增加H矩阵的正定性）

        高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果,所以我们很自然地想到应该给添加一个范围，称为信赖区域(Trust Region)。

        这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法(Trust Region Method)。在信赖区域里，我们认为近似是有效的；出了这个区域，近似可能会出问题。
        那么，如何确定这个信赖区域的范围呢?一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，我们扩大近似的范围；反之，如果差异大，就缩小近似的范围。我们定义一个指标来刻画近似的好坏程度：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​              

        的分子是实际函数下降的值，分母是近似模型下降的值。

        如果接近于1，则近似是好的。如果太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
        于是，我们构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

          这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。在图中式中，我们把增量限定于一个半径为的球中，认为只在这个球内才是有效的。带上之后，这个球可以看成一个椭球。

        在列文伯格提出的优化方法中，把取成单位阵，相当于直接把约束在一个球中。随后，马夸尔特提出将取成非负数对角阵——实际中通常用的对角元素平方根，使得在梯度小的维度上约束范围更大一些。
        无论如何，在列文伯格—马夸尔特优化中，我们都需要解式(6.24)那样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中构成拉格朗日函数：

        ​​​​​​​        

        这里为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于的导数为零，它的核心仍是计算增量的线性方程：

                                                   

        可以看到，相比于高斯牛顿法，增量方程多了一项。如果考虑它的简化形式，即，那么相当于求解：

                                                         

        我们看到，一方面，当参数比较小时，占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格---马夸尔特方法更接近于高斯牛顿法。

        另一方面，当比较大时，占据主要地位，列文伯格—马夸尔特方法更接近于一阶梯度下降法（即最速下降)，这说明附近的二次近似不够好。列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量。
        在实际中，还存在许多其他的方式来求解增量，例如等方法。我们在这里所介绍的，只是最常见而且最基本的方法，也是视觉SLAM 中用得最多的方法。实际问题中，我们通常选择高斯牛顿法或列文伯格—马夸尔特方法中的一种作为梯度下降策略。

        当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格—马夸尔特方法。

6.3 实践 ：曲线拟合问题

6.3.1 手写高斯牛顿法

        接下来，我们用一个简单的例子来说明如何求解最小二乘问题。我们将演示如何手写高斯牛顿法，然后介绍如何使用优化库求解此问题。对于同一个问题，这些实现方式会得到同样的结果，因为它们的核心算法是一样的。
        考虑一条满足以下方程的曲线：

                                                        

        其中，为曲线的参数，为高斯噪声，满足。我们

        选择了这样一个非线性模型。现在，假设我们有个关于的观测数据点，想根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题以估计曲线参数：

                                              

        请注意，在这个问题中，待估计的变量是，而不是。我们的程序里先根据模型生成的真值，然后在真值中添加高斯分布的噪声。随后，使用高斯牛顿法从带噪声的数据拟合参数模型。定义误差为：

                                                     

        那么，可以求出每个误差项对于状态变量的导数：

                                                   

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​   

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​            

        于是，，高斯牛顿法的增量方程为：                                        

        当然，我们也可以选择把所有的排成一列，将这个方程写成矩阵形式，不过它的含义与求和形式是一致的。下面的代码演示了这个过程是如何进行的。
 

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {

    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration time_used = chrono::duration_cast>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

        在这个例子中，我们演示了如何对一个简单的拟合问题进行迭代优化。通过自己手写的代码，很容易看清楚整个优化的流程。该程序输出每一步迭代的目标函数值和更新量，如下：

        测试结果如下：

liuhongwei@liuhongwei-virtual-machine:~/桌面/ch6/build$ ./gaussNewton 
total cost: 3.19575e+06, 		update: 0.0455771  0.078164 -0.985329	estimated params: 2.04558,-0.921836,4.01467
total cost: 376785, 		update:  0.065762  0.224972 -0.962521		estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6, 		update: -0.0670241   0.617616  -0.907497	estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01, 		update: -0.522767   1.19192 -0.756452		estimated params: 1.52155,1.11267,1.3882
total cost: 174.853, 		update: -0.537502  0.909933 -0.386395		estimated params: 0.984045,2.0226,1.00181
total cost: 102.78, 		update: -0.0919666   0.147331 -0.0573675	estimated params: 0.892079,2.16994,0.944438
total cost: 101.937, 		update: -0.00117081  0.00196749 -0.00081055	estimated params: 0.890908,2.1719,0.943628
total cost: 101.937, 		update:   3.4312e-06 -4.28555e-06  1.08348e-06	estimated params: 0.890912,2.1719,0.943629
total cost: 101.937, 		update: -2.01204e-08  2.68928e-08 -7.86602e-09	estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.000312603 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629

        易见整个问题的目标函数在迭代9次之后趋近收敛，更新量趋近于零。最终估计的值与真值接近。

6.3.2 使用 Ceres进行曲线拟合

        本节向大家介绍两个C++的优化库：来自谷歌的Ceres库及基于图优化的g2o库。

        因为使用g2o还需要介绍一点图优化的相关知识，所以我们先来介绍Ceres，然后介绍一些图优化理论，最后来讲g2o。

        因为优化算法在之后的“视觉里程计”和“后端”中都会出现。

Ceres简介：
        Ceres是一个广泛使用的最小二乘问题求解库。在Ceres 中，我们作为用户，只需按照一定步骤定义待解的优化问题，然后交给求解器计算。Ceres求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘)：                                                

                                                

        在这个问题中，为优化变量，又称参数块（Parameter blocks )，称为代价函数( Cost function )，也称为残差块（Residual blocks )，在 SLAM中也可理解为误差项。和 为第个优化变量的上限和下限。在最简单的情况下，取(不限制优化变量的边界)。此时，目标函数由许多平方项经过一个核函数之后求和组成。同样，可以取​​​​​​​为恒等函数，那么目标函数即为许多项的平方和，我们就得到了无约束的最小二乘问题，和先前介绍的理论是一致的。

        为了让 Ceres帮我们求解这个问题，我们需要做以下几件事:
①定义每个参数块。参数块通常为平凡的向量，但是在SLAM里也可以定义成四元数、李
代数这种特殊的结构。如果是向量，那么我们需要为每个参数块分配一个 double数组来存储变量的值。
②定义残差块的计算方式。残差块通常关联若干个参数块，对它们进行一些自定义的计算，
然后返回残差值。Ceres对它们求平方和之后，作为目标函数的值。
③残差块往往也需要定义雅可比的计算方式。在Ceres中，你可以使用它提供的“自动求
导”功能，也可以手动指定雅可比的计算过程。如果要使用自动求导，那么残差块需要按照特定的写法书写:残差的计算过程应该是一个带模板的括号运算符。这一点我们通过例子来说明。
④把所有的参数块和残差块加入Ceres定义的Problem对象中，调用Solve函数求解即可。
求解之前，我们可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认的配置。
        下面，我们来实际操作如何用Ceres求解曲线拟合问题，理解优化的过程。

code：

//
// Created by xiang on 18-11-19.
//

#include 
#include 
#include 
#include 

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  template
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
    return true;
  }

  const double _x, _y;    // x,y数据
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction(
        new CURVE_FITTING_COST(x_data[i], y_data[i])
      ),
      nullptr,            // 核函数，这里不使用，为空
      abc                 // 待估计参数
    );
  }

  // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  ceres::Solver::Summary summary;                // 优化信息
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 开始优化
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration time_used = chrono::duration_cast>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出结果
  cout << summary.BriefReport() << endl;
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

         程序中需要说明的地方均已加注释。可以看到，我们利用OpenCV的噪声生成器生成了100个带高斯噪声的数据，随后利用Ceres进行拟合。这里演示的Ceres用法有如下几项:
①定义残差块的类。方法是书写一个类（或结构体)，并在类中定义带模板参数的()运算
符，这样该类就成为了一个拟函数（Functor )。这种定义方式使得Ceres可以像调用函数一样，对该类的某个对象（比如 a）调用a()方法。事实上，Ceres会把雅可比矩阵作为类型参数传入此函数，从而实现自动求导的功能。
②程序中的double abe[3]即参数块，而对于残差块，我们对每一个数据构造CURVE FIT-
TING_COST对象，然后调用AddResidualBlock将误差项添加到目标函数中。由于优化需要梯度，我们有若干种选择：

(1）使用Ceres的自动求导（Auto Diff )

(2）使用数值求导(Numeric Diff )R

(3）自行推导解析的导数形式，提供给Ceres。因为自动求导在编码上是最方便的，于是我们使用自动求导。
③自动求导需要指定误差项和优化变量的维度。这里的误差是标量，维度为1;优化的是
a,b, c三个量,维度为3。于是,在自动求导类AutoDiffCostFunction的模板参数中设定变量维度为1、3。
④设定好问题后，调用Solve 函数进行求解。你可以在 options 里配置（非常详细的）优化
选项。例如，可以选择使用Line Search还是Trust Region、迭代次数、步长，等等。读者可以查看Options的定义，看看有哪些优化方法可选,当然默认的配置已经可用于很广泛的问题了。
最后，我们来看看实验结果。调用build/ceresCurveFitting查看优化结果：

liuhongwei@liuhongwei-virtual-machine:~/桌面/ch6/build$ ./ceresCurveFitting 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.597873e+06    0.00e+00    3.52e+06   0.00e+00   0.00e+00  1.00e+04        0    2.60e-05    1.08e-04
   1  1.884440e+05    1.41e+06    4.86e+05   9.88e-01   8.82e-01  1.81e+04        1    1.02e-04    3.52e-04
   2  1.784821e+04    1.71e+05    6.78e+04   9.89e-01   9.06e-01  3.87e+04        1    2.29e-05    3.92e-04
   3  1.099631e+03    1.67e+04    8.58e+03   1.10e+00   9.41e-01  1.16e+05        1    2.10e-05    4.28e-04
   4  8.784938e+01    1.01e+03    6.53e+02   1.51e+00   9.67e-01  3.48e+05        1    2.10e-05    4.61e-04
   5  5.141230e+01    3.64e+01    2.72e+01   1.13e+00   9.90e-01  1.05e+06        1    2.10e-05    4.94e-04
   6  5.096862e+01    4.44e-01    4.27e-01   1.89e-01   9.98e-01  3.14e+06        1    1.98e-05    5.26e-04
   7  5.096851e+01    1.10e-04    9.53e-04   2.84e-03   9.99e-01  9.41e+06        1    2.00e-05    5.51e-04
solve time cost = 0.000578694 seconds. 
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890908 2.1719 0.943628 

        最终的优化值和我们上一节的实验结果基本相同，但运行速度上Ceres要相对慢一些。在笔者的计算机上Ceres约使用了1.3毫秒，这比手写高斯牛顿法慢了约六倍。
希望读者通过这个简单的例子对Ceres的使用方法有一个大致了解。它的优点是提供了自动求导工具，使得不必去计算很麻烦的雅可比矩阵。Ceres的自动求导是通过模板元实现的，在编译时期就可以完成自动求导工作，不过仍然是数值导数。本书大部分时候仍然会介绍雅可比矩阵的计算，因为那样对理解问题更有帮助，而且在优化中更少出现问题。此外，Ceres的优化过程配置也很丰富，使其适合很广泛的最小二乘优化问题，包括SLAM之外的各种问题。

6.3.3 使用g2o进行曲线拟合

        本讲的第2个实践部分将介绍另一个(主要在SLAM领域)广为使用的优化库：g2o (GeneralGraphic Optimization，G2O)。它是一个基于图优化的库。图优化是种将非线性优化与图论结合起来的理论，因此在使用它之前，我们花一点篇幅介绍图优化理论。

1.图优化理论简介

         我们已经介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而，目标函数仅描述了优化变量和许多个误差项，但我们尚不清楚它们之间的关联。例如，某个优化变量存在于多少个误差项中呢？我们能保证对它的优化是有意义的吗？

        进一步，我们希望能够直观地看到该优化问题长什么样。于是,就牵涉到了图优化。

        图优化，是把优化问题表现成图的一种方式。这里的图是图论意义上的图。一个图由若干个顶点(Vertex)，以及连接着这些顶点的边(Edge) 组成。进而，用顶点表示优化变量，用边衣示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。
        我们可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。

        下是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；

图6-3 图优化的例子

        同时，实线表示相机的运动模型，虚线表示观测模型，它们构成了图优化的边。

        此时，虽然整个问题的数学形式仍是式(6.13)那样，但现在我们可以直观地看到问题的结构了。如果希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大)的顶点这样的改进。但是最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质做更好的优化。

        g2o是一个通用的图优化库。“通用”意味着你可以在g2o里求解任何能够表示为图优化的最小二乘问题，显然包括上面谈的曲线拟合问题。下面我们来演示这个过程。

2.使用g2o拟合曲线

为        了使用g2o，首先要将曲线拟合问题抽象成图优化。这个过程中，只要记住节点为优化变量，边为误差项即可。曲线拟合对应的图优化模型可以画成下图所示的形式。
         

 图6-4 曲线拟合对应的图优化模型

         在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数是；而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一样，它们是一元边（Unary Edge)，即只连接一个顶点——因为整个图只有一个顶点。所以在图6-3中，我们只能把它画成自己连到自己的样子。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映每个误差与多少个优化变量有关。在稍有些玄妙的说法中，我们把它叫作超边( Hyper Edge),整个图叫作超图（Hyper Graph)。

        弄清了这个图模型之后，接下来就是在g2o中建立该模型进行优化。作为g2o的用户，我们要做的事主要包含以下步骤:
①定义顶点和边的类型

②构建图。
③选择优化算法。
④调用g2o进行优化，返回结果。

3.code

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {
    const CurveFittingVertex *v = static_cast (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique(g2o::make_unique()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);
  }

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration time_used = chrono::duration_cast>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

 执行结果：

liuhongwei@liuhongwei-virtual-machine:~/桌面/ch6/build$ ./g2oCurveFitting 
start optimization
iteration= 0	 chi2= 376785.128234	 time= 3.5967e-05	 cumTime= 3.5967e-05	 edges= 100	 schur= 0
iteration= 1	 chi2= 35673.566018	 time= 1.0961e-05	 cumTime= 4.6928e-05	 edges= 100	 schur= 0
iteration= 2	 chi2= 2195.012304	 time= 9.517e-06	 cumTime= 5.6445e-05	 edges= 100	 schur= 0
iteration= 3	 chi2= 174.853126	 time= 9.978e-06	 cumTime= 6.6423e-05	 edges= 100	 schur= 0
iteration= 4	 chi2= 102.779695	 time= 1.0139e-05	 cumTime= 7.6562e-05	 edges= 100	 schur= 0
iteration= 5	 chi2= 101.937194	 time= 9.85799e-06	 cumTime= 8.642e-05	 edges= 100	 schur= 0
iteration= 6	 chi2= 101.937020	 time= 9.55799e-06	 cumTime= 9.5978e-05	 edges= 100	 schur= 0
iteration= 7	 chi2= 101.937020	 time= 9.678e-06	 cumTime= 0.000105656	 edges= 100	 schur= 0
iteration= 8	 chi2= 101.937020	 time= 9.80801e-06	 cumTime= 0.000115464	 edges= 100	 schur= 0
iteration= 9	 chi2= 101.937020	 time= 9.79799e-06	 cumTime= 0.000125262	 edges= 100	 schur= 0
solve time cost = 0.000738584 seconds. 
estimated model: 0.890912   2.1719 0.943629

        在这个程序中，我们从g2o派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex和CurveFittingEdge，这实质上扩展了g2o的使用方式。这两个类分别派生自BaseVertex和 BaseU-naryEdge类。在派生类中,我们重写了重要的虚函数：

①顶点的更新函数：oplusImpl。我们知道优化过程最重要的是增量的计算，而该函数
处理的是 的过程。
        读者也许觉得这并不是什么值得一提的事情，因为仅仅是个简单的加法而已，为什么g2o不帮我们完成呢？在曲线拟合过程中，由于优化变量(曲线参数)本身位于向量空间中，这个更新计算确实就是简单的加法。但是，当优化变量不在向量空间中时，例如z是相机位姿，它本身不一定有加法运算。这时，就需要重新定义增量如何加到现有的估计上的行为了。按照第4讲的解释，我们可能使用左乘更新或右乘更新，而不是直接的加法。

②顶点的重置函数:setToOriginlmpl。这是平凡的，我们把估计值置零即可。
③边的误差计算函数:computeError。该函数需要取出边所连接的顶点的当前估计值，根据
曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
④边的雅可比计算函数：linearizeOplus。这个函数里我们计算了每条边相对于顶点的雅可比。

⑤存盘和读盘函数：read、write。由于我们并不想进行读/写操作，所以留空。
        定义了顶点和边之后,我们在main 函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。g2o就会给出如上优化的结果。

        我们使用高斯一牛顿方法进行梯度下降，在迭代了9次后得到优化结果，与Ceres和手写高斯牛顿法相差无几。从运行速度来看，我们的实验结论是手写快于g2o，而g2o快于Ceres。这是一个大体符合直觉的经验，通用性和高效性往往是互相矛盾的。但是本实验中Ceres使用了自动求导，且求解器配置与高斯牛顿还不完全一致,所以看起来慢一些。

6.4 小结 

        本节介绍了SLAM中经常碰到的一种非线性优化问题：由许多个误差项平方和组成的最小二乘问题。我们介绍了它的定义和求解，并且讨论了两种主要的梯度下降方式：高斯牛顿法和列文伯格—马夸尔特方法。在实践部分中，分别使用了手写高斯牛顿法、Ceres和 g2o两种优化库求解同一个曲线拟合问题,发现它们给出了相似的结果。
        由于还没有详细谈Bundle Adjustment,我们在实践部分选择了曲线拟合这样一个简单但有代表性的例子，以演示一般的非线性最小二乘求解方式。特别地，如果用g2o来拟合曲线，必须先把问题转换为图优化，定义新的顶点和边，这种做法是有一些迂回的——g2o的主要目的并不在此。相比之下,Ceres定义误差项求曲线拟合问题则自然了很多,因为它本身即是一个优化库。然而，在SLAM中更多的问题是，一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地，当相机位姿以李代数表示时，误差项关于相机位姿的导数如何计算,将是一件值得详细讨论的事。我们将在后续内容中发现，g2o提供了大量现成的顶点和边，非常便于相机位姿估计问题。而在 Ceres 中，我们不得不自己实现每一个Cost Function，有一些不便。
        在实践部分的两个程序中，我们没有去计算曲线模型关于三个参数的导数，而是利用了优化库的数值求导，这使得理论和代码都更简洁。Ceres库提供了基于模板元的自动求导和运行时的数值求导，而g2o只提供了运行时数值求导这一种方式。但是，对于大多数问题,如果能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。
最后，希望读者能够适应Ceres和g2o这些大量使用模板编程的方式。也许一开始会看上去比较吓人（特别是Ceres 设置残差块的括号运算符，以及 g2o初始化部分的代码)，但是熟悉之后，就会觉得这样的方式是自然的，而且容易扩展。我们将在 SLAM后端一讲中继续讨论稀疏性、核函数、位姿图等问题。