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

视觉SLAM十四讲从理论到实践高翔清华大学2016年冬第六讲非线性优化Chapter6:Non-linearOptimization2第六讲非线性优化•本讲目标•理解最小二乘法的含义和处理方式。•理解Gauss-Newton,Levenburg-Marquadt等下降策略。•学习Ceres库和g2o库的基本使用方法。3第六讲非线性优化•上讲回顾•三维世界中刚体运动的描述：旋转矩阵、旋转向量、欧拉角、四元数等。•观测：相机投影模型。4在给定模型和具体观测时，如何对估计进行优化？6.1状态估计问题56.1状态估计问题6𝑥𝑘=𝑇𝑘=exp𝜉𝑘•状态估计问题•最简单情况：线性系统，高斯噪声•复杂情况：非线性系统，非高斯噪声6.1状态估计问题•历史上很长一段时间使用滤波器求解状态估计•但近年非线性优化已成为主流•我们首先来分析一下如何从概率角度看待此问题76.1状态估计问题•状态变量（所有待求解的量）•状态估计等同于求解条件分布：•考虑更简单的情况：只有观测时，类似于StructurefromMotion（SfM）•贝叶斯法则：8类似的，这里z,u也是统称6.1状态估计问题•P(x|z)条件分布很难求解，但可以求：a)最大后验估计（MaximizeaPosterior，MAP）b)最大似然估计（MaximizeLikelihoodEstimation,MLE）“在哪种状态下，最容易产生当前的观测”96.1状态估计问题•从最大似然到最小二乘•例子：某次观测•由于噪声是高斯的：•于是•现在要求x,y的最大似然估计，怎么求？106.1状态估计问题•一般的高斯分布：•负对数形式：•原问题的最大化，相当于负对数最小化•因此，关于原问题的最大似然：•相当于最小化：11最小化x时，只和它有关这货就是所谓的最小二乘6.1状态估计问题•我们把状态最大似然估计变成了最小二乘问题•对于原问题：•最小化误差的二范数：min12定义误差：6.1状态估计问题•直观解释•由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美的成立。•此时就调整状态的估计，使得误差最小化•该问题有何结构？•由许多个误差的平方和（或Sigma范数和）组成。•虽然总体维度高，但每个项很简单，只关联2个变量。•如果用李代数表达位姿，那么是无约束优化问题。•如何求解？•下面先来介绍通用的非线性最小二乘问题。136.2非线性最小二乘146.2非线性最小二乘•先考虑简单的问题：•当f很简单时：•解：将得到极值点或鞍点，比较这些解即可•当f复杂时：•df/dx难求，或df/dx=0很难解•使用迭代方式求解15这里，f为任意函数6.2非线性最小二乘•迭代方式：16问题：如何确定这个增量？6.2非线性最小二乘•确定增量的方法（即梯度下降策略）：一阶的或二阶的•泰勒展开：•若只保留一阶梯度：•称为最速下降法（SteepestMethod）17雅可比海塞min𝛥𝑥‖𝑓𝑥‖22+𝐽𝛥𝑥增量的方向：（通常还需要计算步长）注意这里是对平方展开的6.2非线性最小二乘•若保留二阶梯度：•则得到（令上式关于的导数为零）：•该方法又称为牛顿法18𝛥𝑥6.2非线性最小二乘•最速下降法和牛顿法虽然直观，但实用当中存在一些缺点•最速下降法会碰到zigzag问题（过于贪婪）•牛顿法迭代次数少，但需要计算复杂的Hessian矩阵•能否回避Hessian的计算？•Gauss-Newton•Levenberg-Marquadt196.2非线性最小二乘•Gauss-Newton•一阶近似f(x)：•平方误差变为：•令关于导数为零：20𝛥𝑥6.2非线性最小二乘•G-N用J的表达式近似了H•步骤：21𝛥𝑥𝑘=𝐻−1𝑔6.2非线性最小二乘•Gauss-Newton简单实用，但当中无法保证H可逆（二次近似不可靠）•Levenberg-Marquadt方法一定程度上改善了它•G-N属于线搜索方法：先找到方向，再确定长度•L-M属于信赖区域方法（TrustRegion），认为近似只在区域内可靠•考虑近似程度的描述•若太小，则减小近似范围•若太大，则增加近似范围22𝛥𝑥𝑘=𝐻−1𝑔实际下降/近似下降6.2非线性最小二乘•改进版的G-N：23这些都是经验值Levenberg令D=I，即取一个球，Marquardt令D取非负对角阵，即椭球6.2非线性最小二乘•TrustRegion内的优化，利用Lagrange乘子转化为无约束：•仍参照G-N展开，增量方程为：•在Levenberg方法中，取D=I，则：246.2非线性最小二乘•LM相比于GN，能够保证增量方程的正定性•即，认为近似只在一定范围内成立，如果近似不好则缩小范围•从增量方程上来看，可以看成一阶和二阶的混合•参数控制着两边的权重25𝜆6.2非线性最小二乘•小结•非线性优化是个很大的主题，研究者们为之奋斗多年•主要方法：最速下降、牛顿、G-N、L-M、DogLeg等•与线性规划不同，非线性需要针对具体问题具体分析•问题非凸时，对初值敏感，会陷入局部最优•目前没有非凸问题的通用最优值的寻找办法•问题凸时，二阶方法通常一两步就能收敛265.3实践：CERES275.3实践：CERES•GoogleCeresSolver•通用最小二乘问题求解库•最一般的形式（带边界的核函数最小二乘）•f在Ceres中称为代价函数（CostFunction），x称为参数块（ParameterBlock）285.3实践：CERES•实验：曲线拟合•设曲线方程：•我们得到一些带噪声的样本数据：x,y•希望拟合（回归）曲线参数：a,b,c29305.4实践：G2O315.4实践：G2O•G2O中以图模型表达上述最小二乘问题32曲线拟合实验