视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合

 专栏系列文章如下:

 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

6.3 实践:曲线拟合问题

6.3.1手写高斯牛顿法

考虑一条满足以下方程的曲线:

其中 a,b,c 为曲线的参数,w 为高斯噪声,满足 w ∼ (02)。假设有 N 个关于 x,y 的观测数据点,根据这些数据点求出曲线的参数。那么,可以求解下面的最小二乘问题来估计曲线参数:

在这个问题中,待估计的变量是 a,b,c,而不是 x。程序里先根据模型生成 x,y 的真值,然后在真值中添加高斯分布的噪声。随后,使用高斯牛顿法来从带噪声的数据(x,y)拟合参数模型。定义误差为:

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

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_第1张图片

于是

高斯牛顿法的增量方程为:

也可以选择把所有的Ji 排成一列,将这个方程写成矩阵形式,它的含义与求和形式是一致的。下面的代码演示了这个过程是如何进行的:slambook2/ch6/gaussNewton.cpp

在这个例子中演示了如何对一个简单的拟合问题进行迭代优化。该程序输出每一步迭代的目标函数值和更新量,如下:

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_第2张图片

整个问题的目标函数在迭代 9 次之后趋近收敛,更新量趋近于零。最终估计的值与真值接近,函数图像如下:

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_第3张图片

蓝色点为100个数据点,黑色线为理论模型,红色线为拟合的模型。

6.3.2使用 Ceres 进行曲线拟合

Ceres 简介

Google Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中,只需按照一定步骤定义待解的优化问题,然后交给求解器计算即可。Ceres 求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘):

在这个问题中,x1,··· ,xn 为优化变量,又称参数块(Parameter blocks),fi 称为代价函数(Cost function),也称为残差块(Residual blocks),在 SLAM 中也可理解为误差项。lj 和 uj 为第 j 个优化变量的上限和下限。在最简单的情况下,取 lj = −∞,uj = ∞(不限制优化变量的边界)。此时,目标函数由许多平方项经过一个核函数 ρ(·) 之后求和组成。取 ρ 为恒等函数,那么目标函数即为许多项的平方和,就得到了无约束的最小二乘问题。为了让 Ceres 求解这个问题,需要做以下几件事:

  1. 定义每个参数块。参数块通常为向量,但是在SLAM里也可以定义成四元数、李代数这种特殊的结构。如果是向量,那么需要为每个参数块分配一个 double 数组,来存储变量的值。
  2. 然后定义残差块的计算方式。残差块通常关联若干个参数块,对它们进行一些自定义的计算,然后返回残差值。Ceres 对它们求平方和之后,作为目标函数的值。
  3. 残差块往往也需要定义雅可比的计算方式。在 Ceres 中,可以使用自动求导功能,也可以手动指定雅可比的计算过程。如果要使用自动求导,那么残差块需要按照特定的写法来书写:残差的计算过程是一个带模板的括号运算符。
  4. 最后,把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中,调用 Solve 函数求解即可。求解之前,可以传入一些配置信息,例如迭代次数、终止条件等,也可以使用默认的配置。

安装 Ceres

Ceres 的 github 地址为:https://github.com/ ceres-solver/ceres-solver,

安装依赖项

sudo apt−get install liblapack−dev libsuitesparse−dev libcxsparse3  libgflags−dev libgoogle−glog−dev libgtest−dev  

然后进入 Ceres 库目录下,使用 cmake 编译并安装。安装完成后,在/usr/local/include/ceres 下找到 Ceres 的头文件,并在/usr/local/lib/下找到名为 libceres.a 的库文件。有了这些文件,就可以使用 Ceres 进行优化计算了。

使用 Ceres 拟合曲线

下面的代码演示了如何使用 Ceres 求解同样的问题:slambook/ch6/ceresCurveFitting.cpp

利用 OpenCV 的噪声生成器生成了 100 个带高斯噪声的数据,随后利用 Ceres 进行拟合。这里Ceres用法有如下几项:

  1. 定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的 () 运算符,这样该类就成为了一个拟函数(Functor),或者叫仿函数。这种定义方式使得 Ceres 可以像调用函数一样,对该类的某个对象(比如 a)调用 a() 方法。Ceres 会把雅可比矩阵作为类型参数传入此函数,从而实现自动求导的功能。
  2. 程序中的 double abc[3] 即为参数块,而对于残差块,对每一个数据构造 CURVE_FIT-TING_COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,有若干种选择:(1)使用 Ceres 的自动求导(Auto Diff);(2)使用数值求导(Numeric Diff);(3)自行推导解析的导数形式,提供给 Ceres。
  3. 自动求导需要指定误差项和优化变量的维度。这里的误差是标量,维度为 1;优化的是 a,b,c 三个量,维度为 3。于是,在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
  4. 设定好问题后,调用 Solve 函数进行求解。可以在options里配置优化选项。例如,可以选择使用 Line Search 还是 Trust Region、迭代次数、步长,等等。可以查看 Options 的定义,看看有哪些优化方法可选,一般默认的配置已经可用于很广泛的问题了。

最终的优化值和手写的基本相同,但运行速度上 Ceres 要相对慢一些。

Ceres的优点是提供了自动求导工具,不必去计算很麻烦的雅可比矩阵。Ceres的自动求导是通过模板元实现的,在编译时期就可以完成自动求导工作,不过仍然是数值导数。此外,Ceres的优化过程配置也很丰富,使其适合很广泛的最小二乘优化问题,包括 SLAM 之外的各种问题。

注:自动求导也是用数值导数实现的。

6.3.3使用 g2o 进行曲线拟合

g2o(General Graphic Optimization,G2O)是在SLAM领域广为使用的优化库。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论。

图优化理论简介

前面介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而,仅有一组优化变量和许多个误差项,并不清楚它们之间的关联。比如,某个优化变量xj存在于多少个误差项中呢?能保证对它的优化是有意义的吗?进一步,希望能够直观地看到该优化问题长什么样。于是,就引出了图优化。

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

如下图:

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_第4张图片

用三角形表示相机位姿节点,用圆形表示路标点,它们构成了图优化的顶点;同时,实线表示相机的运动模型,虚线表示观测模型,它们构成了图优化的边。最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题,可以利用图模型的某些性质做更好的优化。

g2o 是一个通用的图优化库,可以在g2o里求解任何能够表示为图优化的最小二乘问题,包括曲线拟合问题。

g2o 的编译与安装

安装依赖:

sudo apt−get install qt5−qmake qt5−default libqglviewer−dev−qt5 libsuitesparse−dev libcxsparse3 libcholmod3

从GitHub下载并cmake安装:https://github.com/RainerKuemmerle/g2o

安装完成后, g2o 的头文件将位于/usr/local/g2o 下,库文件位于/usr/local/lib/下

使用 g2o 拟合曲线

首先要将曲线拟合问题抽象成图优化。这个过程中,节点为优化变量, 边为误差项。

在曲线拟合问题中,整个问题只有一个顶点:曲线模型的参数 a, b, c;而各个带噪声的数据点, 构成了一个个误差项,也就是图优化的边。这里的边是一元边(Unary Edge),即只连接一个顶点。事实上,图优化中一条边可以连接一个、两个或多个顶点,这主要反映每个误差与多少个优化变量有关。

主要步骤:

  1. 定义顶点和边的类型。
  2. 构建图。
  3. 选择优化算法。
  4. 调用g2o进行优化,返回结果。

程序:slambook/ch6/g2oCurveFitting.cpp

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

  1. 顶点的更新函数:oplusImpl。优化过程最重要的是增量∆x的计算,而该函数处理的是 xk+1 = xk + ∆x 的过程。在曲线拟合过程中,由于优化变量(曲线参数)本身位于向量空间中,这个更新计算就是简单的加法。但是当优化变量不在向量空间中时,比如x是相机位姿,它本身不一定有加法运算。这时就需要重新定义增量如何加到现有的估计上的行为了。
  2. 顶点的重置函数:setToOriginImpl。即把估计值置零即可。
  3. 边的误差计算函数:computeError。该函数需要取出边所连接的顶点的当前估计值,根据曲线模型,与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
  4. 边的雅可比计算函数:linearizeOplus。这个函数里计算了每条边相对于顶点的雅可比。
  5. 存盘和读盘函数:read、write。

定义了顶点和边之后,在main函数里声明了一个图模型,然后按照生成的噪声数据,往图模型中添加顶点和边,最后调用优化函数进行优化。g2o会给出优化的结果。

6.4 小结

非线性优化问题:由许多个误差项平方和组成的最小二乘问题。讨论了两种主要的梯度下降方式:高斯牛顿法和列文伯格 —马夸尔特方法。在实践部分中,分别使用了手写高斯牛顿法、Ceres 和 g2o 两种优化库求解同一个 曲线拟合问题,发现结果相似。 特别地,如果用g2o来拟合曲线,必须先把问题转换为图优化,定义新的顶点和边。相比之下,Ceres 定义误差项求曲线拟合问题则自然了很多,因为它本身即是一个优化库。然而,在 SLAM 中更多的问题是,一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地,当相机位姿以李代数表示时,误差项关于相机位姿的导数如何计算。g2o提供了大量现成的顶点和边,非常便于相机位姿估计问题。而在 Ceres 中, 不得不自己实现每一个Cost Function,有一些不便。

Ceres 库提供了基于模板元的自动求导和运行时的数值求导,而 g2o 只提供了运行时数值求导这一种方式。但是对于大多数问题,如果能够推导出雅可比矩阵的解析形式并告诉优化库,就可以避免数值求导中的诸多问题。

习题

  1. 证明线性方程 Ax = b 当系数矩阵 A 超定时,最小二乘解为 x = (ATA) −1ATb。
  2. 调研最速下降法、牛顿法、高斯牛顿法和列文伯格—马夸尔特方法各有什么优缺点。
  3. 为什么高斯牛顿法的增量方程系数矩阵可能不正定?不正定有什么几何含义?为什么在这种情况下解就不稳定了?
  4. DogLeg 是什么?它与高斯牛顿法和列文伯格—马夸尔特方法有何异同?
  5. 阅读 Ceres 的教学材料(http://ceres-solver.org/tutorial.html)以更好地掌握其用法。
  6. 阅读 g2o 自带的文档 。
  7. 更改曲线拟合实验中的曲线模型,并用 Ceres 和 g2o 进行优化实验。

习题在空余时间学习

你可能感兴趣的:(算法,书籍学习笔记,计算机视觉,算法,人工智能)