SLAM学习笔记——基础知识补充（牛顿法与ceres）

补充一些最优化的基础知识，现代的人工智能、控制理论一类基本都是基于优化理论的。想学好这门课需要扎实的线性代数基础。牛顿法是SLAM中使用最多的优化方法，利用ceres库可以进行求解。

牛顿法

雅可比矩阵与黑塞矩阵与梯度

先问一个问题，什么是方向导数？什么是梯度？
方向导数是一个数字，就是在某个方向下的变化速度，而梯度是什么，是一个方向，在这个方向下，方向导数是最大的。
雅可比和黑塞定义
雅可比矩阵
雅可比矩阵就是矩阵的导数。我个人如何判定是不是懂了的标准是，会不会因为坐标而困扰，我第一次学的时候总觉得懂了，但是又总觉得会不知道是先对x1求偏导还是对x2求偏导，如果你的思路很清晰的话，实际上根本不会有这个问题的。

通常我们理解过程中，y是一个值，x是一个向量，$y=f(x)$。高数里学过y对x求导（全微分）就是分别对每个x的方向求偏导，然后加起来（这个加起来是有方向的）
我们要有一个理解，一个向量代表的是各个方向的分量，而左乘一个方向矩阵$(i,j,k)$，就是上文说的加起来，也就是指定了一个空间的方向。
如果目标函数是多维的，那么J自然就从一个向量一行行拓展为一个矩阵。
黑塞矩阵

黑塞矩阵可以理解为2阶雅可比

雅可比是针对一个向量求的，第二次求雅可比确是针对一个矩阵求的，这个向量比较广义，就像vector里面可以push_back任何数据结构。你可以理解为把每一个列向量作为广义向量的一个元素。
从这个角度展开，就会知道坐标下标了。
PS：xy方向都连续，那么求偏导顺序是不影响的。看到过一个很有意思的例子，爷爷的爸爸和爸爸的爷爷是同一个人，而奶奶的妈妈和妈妈的奶奶却不是同一个人。原因是：二阶偏导次序不影响结果的前提是导数在区间连续。
黑塞矩阵的几何意义
梯度
梯度就是雅可比矩阵（向量）的转置，这里一般都会写成向量形式，即用f代表y1,y2,y3…
上面黑塞矩阵里面那个反三角f转置，其实就是雅可比矩阵，反三角f就是梯度。

泰勒展开

没见到展开三阶的，我也不会展开三阶，三阶也没啥物理含义。第三项就是标准的二次线性关系的表达式，具体怎么推导我也不会，但猜也是这么猜，1/2就是标准的系数。

梯度下降

就是略去了泰勒展开的二次部分，用一次线性逼近原函数，然后沿着梯度方向找最小值。

牛顿法和LM修正

保留了二次部分，逼近速度更快，但是牛顿法要求逆，复杂度是$O(n^3)$的，上面是O(n)的，算起来很慢，所以一般特征比较少的时候用牛顿法，机器学习里面，梯度用的很多，因为特征实在太多了，想想神经网络多少参数啊，
这是牛顿法迭代公式，就是对泰勒展开求导等于0推出来的。

非凸黑塞矩阵是不保证正定的，比如鞍点，于是有拟牛顿法。黑塞矩阵不正定，就强行正定，加一个单位矩阵乘一个系数，让他刚好正定。
这个系数表观上起了一个什么作用呢，如果这个系数很小，那么使用就是牛顿法，如果这个数字很大，那么起作用的就是梯度法，因为单位矩阵的逆还是单位矩阵，乘梯度还是梯度。

拟牛顿法

黑塞矩阵的逆太难算了，要找一个矩阵近似黑塞矩阵的逆，不记得咋做了，好像是利用梯度去凑了一个满足正定条件的矩阵。

牛顿法非线性最小二乘

高斯牛顿法

视觉SLAM解释推导：先做一阶展开，然后在梯度方向上求解最小值。
最优化推导：直接应用牛顿法，舍去小参数。
中间推导省略，可以得到可以利用$J^{T}J$来近似黑塞矩阵，同样有非正定的情况，需要LM修正。高翔的视觉SLAM上面还有另一种经验修正手段。

比较

ceres库简单应用

ceres是google推出的一个最后小二乘的求解库。例程就是SLAM14讲里面的例程

/*
 * @Author: Jimazeyu
 * @Date: 2022-01-26 22:19:34
 * @LastEditors: OBKoro1
 * @LastEditTime: 2022-01-26 23:58:46
 * @FilePath: /catkin_ws/src/multi_position_optimization/ceres_test.cpp
 */
#include"ros/ros.h"
#include"iostream"
#include"ceres/ceres.h"
#include"opencv2/core/core.hpp"
using namespace std;

//构建代价函数
struct cost_function{
    cost_function(double x,double y):_x(x),_y(y){}
    //残差（lost）计算
    template<typename T>
    //abc是abc的数组
    bool operator() (const T *const abc, T *residual)const{ 
        //error=y-exp(ax^2+bx+c)
        residual[0] =  T(_y) -ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]); 
        return true;
    }


    const double _x,_y;
};

int main(int argc,char **args)
{
    double  ar=1.0,br=2.0,cr =1.0; //groud_truth
    double ae=2.0,be=-1.0,ce=5.0; //initial param
    int N=100; //100 points
    double w_sigma=1.0;//方差
    double inv_sigma=1.0/w_sigma;
    cv::RNG rng; //random generator

    vector<double> 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));
    }

    //检测数据 
    // for(int i=0;i
    // {
    //     cout<
    // }

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

    //构建最小二乘问题
    ceres::Problem problem;
    for(int i=0;i<N;i++)
    {
        problem.AddResidualBlock(//向问题中添加误差项
            new ceres::AutoDiffCostFunction<cost_function,1,3>(
                new cost_function(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; //优化信息

    ceres::Solve(options,&problem,&summary); //开始优化

    //cout<
    for(auto param:abc)
    {
        cout<<param<<endl;
    }


    return 0;
}