视觉SLAM十四讲——第六讲非线性优化

@ 《视觉SLAM十四讲》知识点与习题

《视觉SLAM十四讲》第六讲知识点整理+习题

正在学习SLAM相关知识，将一些关键点及时记录下来。

知识点整理

方程的位姿可以用变换矩阵来表示，然后用李代数进行优化。
本讲主要将如何通过优化，来处理噪声数据，并使用图优化，来解决问题。
本章的逻辑很清晰，首先提出问题：得到的数据中都存在噪声；接下来引入卡尔曼过滤器和非线性优化，指出需要通过最大化似然函数来得到最有可能的当前位姿。最大化似然函数，又引出了可以使用迭代方式求解，就需要确定增量。最后就引出了高斯牛顿法和列文伯格-马夸尔特法

1. 运动方程和观测方程：通常认为两者的噪声项满足零均值的高斯分布
2. 卡尔曼过滤器和非线性优化：卡尔曼过滤器关心当前时刻的状态估计，而对之前的状态则不多考虑；非线性优化适用所有时刻采集到的数据进行状态估计
3. 非线性优化：把所有待估记的变量放在一个“状态变量”中。对状态的估计，就是已知输入数据u和观测数据z的条件下，求计算状态x的条件概率分布（这里的观测数据是指以位姿变量x，观测到路标y时得到的数据，其中包含噪声； 输入数据是指从测量数据的传感器中得到的数据）
4. 后验概率最大化求得状态最优估计：求解最大后验概率相当于最大化似然和先验的乘积。当不知道位姿时，就没有了先验。也即问题转换为求解x的最大似然估计，即在什么样的状态下，最可能产生现在观测到的数据
5. 最小二乘问题：最小化观测数据的高维高斯分布形式的误差的平方，就可以得到最优的位姿状态。对于不方便求解的最小二乘问题，可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。此时，只要找到迭代点的梯度方向即可，而无须寻找全局导函数为零的情况。（这个和神经网络里面的梯度下降方式一样欸！殊途同归的感觉）
6. 求解增量最直观的方式是将目标函数在x附近进行Taylor展开，可以选择保留Taylor展开式的一阶项或二阶项，从而为一阶梯度或二阶梯度法。对更新量做最小化
7. 高斯牛顿法：将f(x)进行一阶的泰勒展开，而不是对目标函数f(x)^2。将最小二乘问题展开，关于Δx求导。即可得增量方程，也即高斯牛顿方程
   
   优点：将原先的求解Hessian矩阵转化为求解，简化计算；缺点：由于半正定，所以增量的稳定性较差，导致算法不收敛
   
8. 列文伯格-马夸尔特方法：引入信赖区间方法，即在信赖区间中认为近似是有效的，出了信赖区间，就认为近似可能出现问题。即认为上述的高斯牛顿法仅在信赖区间内成里；并且设置一个量来表征信赖区间的好坏程度，依此对信赖区间进行调整。优点：避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量Δx
   
9. 在视觉SLAM中，会使用ICP,PnP等算法提供优化初始值。一个良好的初始值对最优化问题很重要！！
10. 图优化：将非线性优化和图论结合。 使用顶点表示优化变量，用边表示误差项，从而就可以知道每个优化变量存在于哪些误差项内。

实践

1. Ceres库：面向通用的最小二乘问题的求解，需要定义优化问题，然后设置一些选项，输入Ceres求解即可。具体的，将定义优化变量x和优化代价函数fx。优点是提供了自动求导工具，且其优化的过程配置也很丰富，使其适合于很广泛的最小二乘优化问题，包括SLAM中的各种问题
   书本上对代码的解释已经很详细了，在此就不再介绍
   主要注意下述几点：
   1. cost function中的残差计算为观测值减去符合分布概率的abc三个变量的函数，得到误差函数。注意，这里使用的仍然是观测值，在实际应用中是不存在真实值的，毕竟有噪声
   2. 对于每个数据点都要加入ceres::Problem中

      problem.AddResidualBlock (     // 向问题中添加误差项
          // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
              new ceres::AutoDiffCostFunction ( 
                  new CURVE_FITTING_COST ( x_data[i], y_data[i] )
              ),
              nullptr,            // 核函数，这里不使用，为空
              abc                 // 待估计参数
          );
      

   3. 使用这一句来求解上述最小二乘法

      ceres::Solve ( options, &problem, &summary ); 
      

      summary中会记录每一次求解过程的优化信息
2. g2o库。注意，对于非优化问题，把所有待估计的变量都放在一个“状态变量”中，所以在曲线拟合问题中，就只有一个顶点：即曲线模型的参数abc
   1. 自定义顶点类型继承自基础顶点。代表优化顶点

      class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
      

   2. 自定义边类型继承自基础一元边。代表误差项，所以在边的类中，需要定义computeError()函数来计算误差

      class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
      

   3. 对于图优化过程，如最前面讲到的“称每个误差项对应的优化变量为参数块”
   4. 需要设置g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > 设置每个误差项优化内容，g2o::OptimizationAlgorithmLevenberg* solver设置优化算法，g2o::SparseOptimizer optimizer设置优化器
   5. 使用下述代码执行优化过程

      optimizer.initializeOptimization();
      optimizer.optimize(100);