视觉SLAM——非线性优化问题

SLAM位姿求解问题根本上是一个不断进行非线性优化到最优值的问题。

1. 最大似然问题。位姿估计本身是求解最大似然估计，即在什么样的状态下xk，最可能产生现在观测到的数据。
运动方程：; 表示从k-1时刻到k时刻的运动,表示k-1时刻位姿，是传感器读数，是噪声。
观测方程：;在位置观测到某个点,产生一个观测数据,是噪声,是观测值。
位姿是可以使用变换矩阵或李代数表示，则在处对点进行一次观测，对应到图像上的像素位置为,观测方程可以表示为：,其中d为该点深度，K为相机内参（表达意义是像素坐标=KT*世界坐标）。
补充相机坐标相素坐标关系如下：像素坐标=K*相机坐标 ;
                                                     相机坐标=T*世界坐标 ;
                                                     像素坐标=KT*世界坐标;  
其中为相机内参, 为相机外参即位姿。
在状态估计中，位姿x是一个概率分布，它的概率分布为, 其中观测值z和传感器数据u已知。只考虑观测数据，相当于估计的概率分布，利用贝叶斯法则，P(x|z)称为后验概率，P(z|x)称为似然，P(x)为先验。直接求后验分布困难，但求一个状态最优估计，使得在该状态下后验概率最大化是可行的：
, .
没有了先验后就成了最大似然估计（Maximize Likehood Estimation, MLE）,意思为在什么样的状态（位姿）下，最可能产生现在观测的数据。

2. 最小二乘问题：平方损失函数的极值点
对于观测方程,假设噪声项符合高斯分布vk~N(0,Qk,j)(u=0),所以观测数据的条件概率.(u=h(yj,xk))，也符合高斯分布。对这个高斯分布取概率密度，再取负对数后，略去无关项，则概率最大值.这个公式等价于最小化噪声项（即误差，v_{k,j}=z-h）的平方，定义 则整体误差（两个噪声项的平方和）为：

该公式就是一个总体意义下的最小二乘问题。它的最优解就是状态的最大似然估计。将状态值x进行微调,使S(x)值即整体误差下降一些，达到极小值，这就是一个典型非线性优化的过程。
最小二乘法（最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法的解法，就是求得平方损失函数的极值点。最小二乘法还可用于曲线拟合。其他一些优化问题也可通过最小化能量或最大化熵用最小二乘法来表达。（百科）

3.非线性最小二乘求解过程：迭代优化
第2部分最小二乘问题就求平方损失函数的极值，对于不便直接求解(求导)的平方损失函数的求解即非线性最小二乘问题，使用迭代方式，从一个初始值出发，不断更新当前的优化变量使目标函数下降，具体步骤如下:
a.给定某个初始值，
b.对于第k次迭代，寻找一个增量使得 达到极小值，
c.若足够小，则停止。否则令,返回b.
这个步骤使求导问题转换成一个不断寻求覆盖率并下降的过程，直到某个时刻增量非常小，无法再使函数降，算法收敛，目标达到一个极小，完成寻找极小值的过程，在这个过程中，只要找到迭代点梯度方向即可，无需寻找全局导函数为0的情况。在求解位姿过程中，x就是位姿，其误差函数的表达式为 。接下来就是确定，求解方法如下：
3.1 最直观方法：一阶二阶梯度法
求增量的最直观方式是将它在x附近进行泰勒展开：
.其中J是关于x的导数（Jacobi矩阵，是一阶偏导数以一定方式排列成的矩阵，其行列式称为Jacobi行列式。Jacobi矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数),H是二阶导（Hessian矩阵）。保留展开的一阶或二阶项对应的求解方法即为一阶梯度或二阶梯度法。保留一阶梯度，增量解为,它的意义直观就是沿着反向梯度方向前进，通过计算时设置一个步长求得最快下降方式，称为最速下降法。保留二阶梯度增量解为，这种方法称为牛顿法.
这种方法十分直观，只需把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可。由于泰勒展开后变成多项式，求解增量时只需解线性方程即可。缺点就是最速下降法过于贪心，增加迭代次数;牛顿法需要计算目标函数的H矩阵，大规模问题求解时非常困难。以下两种是常用方法：高斯牛顿法和列文伯格-马夸尔特法。
3.2最简单方法：Gauss-Neutron(GN)法
思想是将f(x)进行一阶泰勒展开，而不是平方损失函数f(x)^2:,其中J(x)是f(x)关于x的导数，是一个m*n的Jacobi矩阵。当前目标是为了寻找下降矢量，使得达到最小。为了求解需要解以下的一个线性最小二乘问题：,根据极值条件，将上述目标函数对求导，并令导数为0。先展开上述公式右边平方项：,求上式关于的导数，并令其为零得到：
.对于变量这是一个线性方程组，也称为增量方程，或者Gauss-Neutron方程，正规方程。左边系数定义为H,右边定义为g,那么式变为：.对比牛顿法，GN法用作为牛顿法中的二阶Hession矩阵近似，省略计算H的过程。求解增量方程是整个优化问题的核心所在。GN的算法步骤如下：
a.给定某个初始值x0
b.对于第k次迭代，求出当前的Jacobi矩阵J(xk)和误差f(x)
c.求解增量方程：
d.若足够小，则停止。否则令,返回b
这种方法原则上要求H是可逆的正定的，实际计算通常半正定。尽管有这些缺点但仍值得学习，因为好多算法都是它的修正，比如接下来的LM法。
3.3效果最好方法：Levenberg-Marquadt(LM)法
由于GN法采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以给添加一个信赖区域。使用来判断泰勒近似是否够好，分子是实际函数下降的值，分母是近似模型下降的值，如果接近1，则近似是好的。这样构建改良版非线性优化框架，具体步骤如下：
a.给定初始值x0及初始优化半径
b.对于第k次迭代，求解,这里是信赖区域半径，D通常取的对角元素平方根。
c.计算，若>3/4,则=2;若<1/4,则=0.5;
d.如果大于某阈值，认为近似可行，令
e.判断算法是否收敛，如果不收敛则返回b,否则结束。
其中近似范围扩大倍数和阈值都是经验值，可以替换成别的数。使用Lagrange乘子将它转化为一个无约束化问题：
,其中为Lagrange乘子，类似GN法，核心仍是计算增量的线性方程：.当H占主民地位，说明二次近似模型较好，若后者占主导地位说明一阶近似较好，可以一定程度上提供更稳定更准确的增量。
求解方法总结：在三种方法求解最优值的时候都需要提供变量的初始值，准确说是一个良好的初始值，视觉LSAM中使用ICP、PnP之类的算法提供优化初始值。求解线性增量方程组通常使用矩阵分解法。

4.非线性优化在orbslam中的实践：g2o方法(参考：https://blog.csdn.net/weixin_42905141/article/details/93850592)
在SLAM中广为使用的库g2o(General Graphic Optimization)是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论。图优化是把优化问题表现成图的一种方式。一个图由若干个顶点以及连接着这些点的边组成，用顶点表示优化变量，用边表示误差项。它是一个重度模板类的c++项目，其中矩阵数据结构多来自Eigen，bundle adjustment，ICP，数据拟合，都可用g2o做。

图的核心:SparseOptimizer是整个图的核心，SparseOptimizer它是一个Optimizable Graph，从而也是一个超图(HyperGraph)
顶点和边:这个超图包含了许多顶点(HyperGraph::Vertex)和边(HyperGraph::Edge)。而这些顶点顶点继承自Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边)，它们都叫做OptimizableGraph::Edge.
配置SparseOptimizer的优化算法和求解器:整个图的核心SparseOptimizer 包含一个优化算法(OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从GN法,LM法, Powell’s dogleg 三者中间选择一个(常用GN和LM).
如何求解:OptimizationWithHessian内部包含一个求解器(Solver)这个Solver实际是由一个BlockSolver组成的.这个BlockSolver有两个部分,一个是SparseBlockMatrix,用于计算稀疏的雅可比和Hessian矩阵;一个是线性方程的求解器(LinearSolver),它用于计算迭代过程中最关键的一步HΔx=−b,LinearSolver有几种方法可以选择:PCG,CSparse,Choldmod.
代码如下：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1 
// 第1步：创建一个线性求解器
LinearSolver Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); 
// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化 
Block* solver_ptr = new Block( linearSolver ); 
// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化 
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); // 第4步：创建终极大boss 稀疏优化器（SparseOptimizer） 
g2o::SparseOptimizer optimizer; // 图模型 
optimizer.setAlgorithm( solver ); // 设置求解器 
optimizer.setVerbose( true ); // 打开调试输出 
// 第5步：定义图的顶点和边。并添加到SparseOptimizer中 
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点 
v->setEstimate( Eigen::Vector3d(0,0,0) ); 
v->setId(0); 
optimizer.addVertex( v ); 
for ( int i=0; isetId(i); 
edge->setVertex( 0, v ); // 设置连接的顶点 
edge->setMeasurement( y_data[i] ); // 观测数值 
edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆 
optimizer.addEdge( edge ); } 
// 第6步：设置优化参数，开始执行优化 
optimizer.initializeOptimization(); 
optimizer.optimize(100);

上述代码的具体详细过程如下：
1.创建一个线性求解器LinearSolver
增量方程的形式是：H△X=-b,通常情况下想到的方法就是直接求逆,即△X=-H.inv*b。看起来简单,但前提是H的维度较小，此时只需要矩阵的求逆就能解决。但是当H的维度较大时,矩阵求逆变得很困难,求解问题也变得很复杂。
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
2.创建BlockSolver。并用上面定义的线性求解器初始化。BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。BlockSolver有两种定义方式:
//固定变量的solver: p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
 using BlockSolverPL = BlockSolver< BlockSolverTraits >;
 //可变尺寸的solver:在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定
 using BlockSolverX = BlockSolverPL;
3.创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
4.创建终极大boss 稀疏优化器(SparseOptimizer),并用已定义求解器作为求解方法。
g2o::SparseOptimizer optimizer;//创建稀疏优化器
//用前面定义好的求解器作为求解方法：
optimizer.setAlgorithm( solver );   // 设置求解器
//setVerbose是设置优化过程输出信息用的
optimizer.setVerbose( true );       // 打开调试输出
5. 定义图的顶点和边。并添加到SparseOptimizer中。
g2o定义顶点：https://blog.csdn.net/weixin_42905141/article/details/100827638
g2o定义边：https://blog.csdn.net/weixin_42905141/article/details/100830126
6.设置优化参数，开始执行优化。
// 执行优化前的初始化SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
optimizer.initializeOptimization();
////开始执行优化,并设置迭代次数SparseOptimizer::optimize(int iterations, bool online)
optimizer.optimize(100);

5.滤波和非线性优化联系和区别
利用Bayesian推理框架可建立基于滤波和基于优化两种方间联系。基于优化方法迭代地找出测量的总概率最高的状态，可被看作最大似然ML思路。基于滤波的方法，其中平台姿态的先验分布由内部传感器的测量构建，并且似然分布由外部传感器测量建立，故被视为最大后验MAP思路。SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。目前SLAM研究的主流热点几乎都是基于图优化的。
滤波方法在当时计算资源受限、待估计量比较简单的情况下，EKF为代表的滤波方法比较有效，经常用在激光SLAM中。缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。图优化里，BA起到了核心作用，在视觉SLAM中，虽然包含大量特征点和相机位姿，但其实BA是稀疏的。

*******************************本文参考高翔SLAM十四讲内容。