激光slam学习笔记——基于图优化的激光slam方法

1、slam基础

整体来说，在激光slam中，滤波器的误差要小于图优化的误差。
图优化通俗点说就是里程计计算的位姿与观测到的位姿之间会形成一个误差（都准的话位姿的差值为0），然后我们通过一些方法（最小二乘）不断调整位姿，使得这个误差最小，从而达到最优的位姿解。

2、非线性最小二乘

预测与观测之间形成一个误差，在表示成最小二乘的形式，不过这个最小二乘是一个非线性的最小二乘的问题，不能直接求导求解。

正常e的转置 * e ，但是这里我们加了一个权重，使得当观测值比较准的时候权重大些。

解决办法就是讲非线性最小二程线性化（局部线性化:泰勒展开）

首先线性化（泰勒展开）， 雅克比矩阵就是，对每个位姿求导数，然后带入到最小二程方程中

然后就是展开，因为线性化了所以可以求导

这里最后得到的实际就可以看做一个二次方程

求导之后，我们就能表示delte X，然后通过不断的迭代知道得到最小值

主要流程回顾，因为假设对200个位姿进行优化，那么雅克比矩阵要计算600项（一个位姿三个坐标x, y, z），海塞矩阵就是2400个次计算，计算量过于大，后面会介绍一些方法来加速计算

3、最小二乘在Slam中的应用

里程计测量两位位姿的关系能构成一个边
回环检测：机器人走过一段路然后回到之前走过的路，由于噪声不会再计算中完全重合，因此形成误差，也是一条边

Xi的逆 * Xj表示 j 位姿 在 i 位姿为原点坐标系的表示为 j 的预测值。然后Zij 的逆 * （Xi 的逆 * Xj ） 表示 i 对 j 的观测的逆 与 预测的 j 的坐标相乘，通常情况应该是 单位矩阵，但因为有噪声不准确，所以得到的就是误差。

首先表示成误差形式，然后对ti（位移） 与 斯塔i（转角）进行求导得到第二个式子，第三个式子就是对tj（位移）与 斯塔j（转角）进行求导。

进行线性化，也能看出雅克比只有对应 i 和 j 位置有值，所以能想到对应的海塞矩阵也是稀疏的

只有ij对应地方有值

通常我们会固定第一个位姿，不然优化的是整体的位姿，可能导致形状与轨迹是对应的，但是整体偏移