＜视觉SLAM十四讲＞ ch6 非线性优化

VIP文章贝宝老师

已于 2022-06-05 16:33:16 修改

阅读量1.1k

点赞数 1

文章标签： linux c++

于 2022-05-08 01:22:14 首次发布

本文链接：https://blog.csdn.net/weixin_40440816/article/details/124632108

版权

文章目录

前言：基础知识，但如果不看后面就不知道在干什么的基础知识。哎看太久i了。

一、状态估计问题

1.1 批量状态估计和最大后验估计

运动方程：
在这里插入图片描述观测方程：
观测方程由针孔模型给定，对应到图像上的像素位置，观测方程可以写成(from ch5):其中，K为相机内参，s为像素点的距离，xk是相机的位姿，yj是路标。w和v都是噪声分量，满足0均值的高斯分量：
在这里插入图片描述 在噪声的影响下，通过带噪声的数据z和u推断位姿x和地图y（以及概率分布），即状态估计问题。
处理状态贵问题有两种方法：滤波器（渐进式/增量式），非线性优化（批量式），SLAM使用得是滑动窗口估计法，对当前时刻附近的一些轨迹进行优化。
后验分布P(x,y|z,u): 已知输入数据u(所有时刻的输入), z(所有时刻的观测数据), 求状态x, y的条件概率分布。
贝叶斯法则：在这里插入图片描述直接求解后验概率很困难，办法是让似然与先验的乘积最大，使得后验的概率达到最大，以得到某个状态最优的估计。（我理解的是最有可能出现的状态）
补充：argmax f(x,y)是指当f(x,y)取得最大值时，变量x,y的取值
最大后验估计：
在这里插入图片描述最大似然估计：
似然的理解：在什么样的状态下，最有可能产生现在观测到的数据。
接下来求似然.

1.2 最小二乘法的引出

这里用了两个知识点：
①由于z, u相互独立可以将式子分解；
②利用数学推导求解；

首先讨论P（z|x, y)：
考虑高斯分布
在这里插入图片描述经过一些数学推导：
等价于最小化噪声项（重投影误差）的一个二次型。（二次型称为马氏距离）
据此引出并推导P（z, u | x, y):
注意右侧第一项没y，是因为运动方程本身就没y。
关于u的最大似然和关于z的最大似然是类似的，得到P（z, u | x, y)最大似然估计方法：
在这里插入图片描述即，将最大似然估计转换成最小二乘问题。
理解：当视觉里程计得到x和y，带入运动模型和观测模型时不会完美成立（存在误差），所做工作的目的是对数据进行调整，进而使得误差变小，也得到整体数据的最大似然估计。

1.3 举个例子

离散时间系统：
在这里插入图片描述按照上面的推导，最后的最小二乘目标函数为：
#pic_center =380x
这个系统是线性系统，可以写成向量的形式来求解。

二、非线性最小二乘

上一节是将SLAM问题转换成最大似然估计问题，然后又转换成最小二乘问题。本节来学习如何求解最小二乘问题。
求解某个函数的最小值，那高考教我们，求导数然后带进去算看哪个最小，当然啦现在不是高考题，此题非彼题。
首先来看个normal的迭代方法：在这里插入图片描述我的理解是就是找个增量让F(x)不断下降。

2.1 一阶和二阶梯度法

虽然我没学过数值分析，但把F(x) Taylor展开：
在这里插入图片描述一阶梯度法(最速下降法)：发现如果直接取增量x的方向为-J方向，就可以令其趋于最小；
二阶梯度法(牛顿法)：二阶导数保留：J + Hx = 0 --> Hx = -J。
这两种算法不得行，主要原因：最速下降法太贪心，反而容易增加迭代次数，收敛变慢；牛顿法得求出H，难度大。
于是有以下两种算法。

2.2 高斯牛顿法

上个是把F(x)展开，这个是把f(x)Taylor展开，再取范数的平方：
在这里插入图片描述
后和牛顿法一样，再对增量x求导，其实我觉得不需要展开也能求导，是这样的：
这里的H可不是海塞矩阵啦，是一阶导数雅可比矩阵的近似矩阵（也不晓得为啥整得名差不多）。然后不断迭代，和牛顿法类似。
缺点：还是需要求H的逆，但这个H能不能逆都不一定，即便能求出来，这个增量x也太大了。

2.3 列文伯格-马夸尔特方法（LM方法）

补充：Taylor展开只在离展开点近得地方近似比较好，于是划分出一个“信赖区域”，不让他跑到近似性不好的区域。
这个评判近似性是通过：
在这里插入图片描述我的理解是p是J(x)的真实程度，若p是1，则说明很真，很近似，很好；若p小于1, 则说明实际减小的值少于近似减小的值，需要缩小近似范围（以实际为准），反之，p比较大，实际比预计的大，则扩大近似范围。
在这里插入图片描述说明：上述阈值是经验值。列文伯格把D取为单位阵I，直接把增量x约束在秋内，马夸尔特把D取成非负对角阵（实际通常是J * J^T的对角元素平方根），这使得在梯度小的维度上约束范围大些。
步骤二的那个式子，右边有个不等式的约束，即用拉格朗日乘子把约束项放在目标函数中：
在这里插入图片描述求导，导数为0，得：
说明：这个H和高斯牛顿法的H是一样的，不是海塞矩阵哦。
增量x的系数矩阵比单纯H矩阵有更强的正定性，H在这个系数的占比权重越大，二阶近似效果越好，越接近高斯牛顿法；H在这个系数的占比权重越小，二阶近似差，越接近一阶（快速）牛顿法。（D^T * D 在列文伯格这等于I）
SLAM问题中，通常选择高斯牛顿或者列文伯格-马夸尔特方法中的一个，问题性质好就用高斯牛顿法，问题太病态了就用后者。

三、小结

1、科学问题的初值提供很重要，视觉SLAM主要用ICP，PnP之类的算法提供优化初始值；
2、求解线性增量方程，一般将矩阵分解，像之前ch3提到的QR和Cholesky等方法。

四、激动人心的英文符号

4.1 手写高斯牛顿

回一下高斯牛顿过程：将f(x)Taylor展开令导数为0，写出增量方程不断迭代。
待拟合的曲线： y = exp(ax^2 + bx + c) + w，已知x, y，估计a, b, c，w是噪声。
step:
① 给定初始值[2, -1, 5];
② 为了迭代求出雅可比矩阵和噪声误差（简单不需要Ceres / g2o)；
③ 求解增量方程，得到增量x；
④ 迭代直到次数达到上限或者增量x达到理想值。
这里有个需要注意的，这里的状态估计采用的范数是以信息矩阵为内积，具体推导见这篇，我觉得很容易懂啦，增量方程如下：
在这里插入图片描述
代码部分：
我和书里写得不太一样，主要是对M^{-1}的定义上，我只用一个inv，书里用了inv * inv。但结果一样。奇怪

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
   
   double aa = 1.0, ba = 2.0, ca = 3.0;//真实参数值
   double ae = 2.0, be = -1.0, ce = 5.0;//估计参数值
   int N = 100;  //数据点
   double w_sigma = 1.0;
   double inv_sigma = 1.0 / w_sigma; //set 1/Sigma = W^{-1}
   cv::RNG rng;
   
   //prepare data
   vector<double> x, y;
   for(int i = 0; i < N; i++){
   
	double temp = i / 100.0;
   	x.push_back(temp);
   	y.push_back(exp(aa * temp * temp + ba * temp + ca) + rng.gaussian(w_sigma));
   }
   
   //Gauss-Newton
   int iteration = 100; //time
   double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
   
   chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
   for(int i = 0; i < iteration; i++) {
   
   	Matrix3d H = Matrix3d::Zero(); // H = J^T * W^{-1} * J
   	Vector3d g = Vector3d::Zero(); // g = -J^T * W^{-1} * w
   	cost = 0;
   
   	for(int j = 0; j < N; j++){
   
		double xj = x[j], yj = y[j];
		double error = yj - exp