非线性优化与g2o--学习笔记

学习高翔g2o：https://www.bilibili.com/video/BV1b441177kr/

做个笔记，描述可能有误。

图1

图2

P（x,y|z）表示在已知观测值的情况下，去求状态量x，y，如图1左下角。在SLAM中z可以是相机中特征点的坐标，x为相机pose，y为路标点。

通过贝叶斯公式可以理解为P（x,y|z）正比于p（z|x，y）*p（x，y）。p（z|x，y）为在这个xy情况下，最容易产生怎样的z。

最大似然是在怎样的x，y的情况下最容易产生现在的观测数据。

一般情况下求最大似然来求P（x,y|z），因为一般情况下不知道机器人的先验位姿和路标的先验位姿。最大似然可以变为-ln（）求最小值。

z服从高斯分布，求高斯分布求反对数，如图右侧。

我们可以定义一个误差：1/2*（x-u）T∑-1*（x-u），求他的极小值，就类似求p（z|x，y）的最大似然。这就理解了为什么误差函数中间这样设计了。

图2右下角部分∑为z高斯分布的协方差矩阵，∑的逆被称之为信息矩阵。

图3

求了目标函数的最小值，相当于求了x，y的最大似然。也就是说误差分布服从高斯分布后，整个问题就变成了一个最小二成问题，slam就变成了最小二乘问题。

图4

图5

x是一个pose时，J怎么算。

如果Δx的量非常大时，不能用H求逆的方式求解。怎样用稀疏性的方式求解。

使用G2O,用户自定义顶点和边，每个边对应一个误差项，和误差项相对于优化变量的雅可比