专栏首页科学计算SLAM后端:非线性优化

SLAM后端:非线性优化

非线性优化

 假设有目标函数:

\min _{x} F(\boldsymbol{x})=\frac{1}{2}\|f(\boldsymbol{x})\|_{2}^{2}

 我们要求其最小值,当然是对目标函数进行求导,但通常目标函数是非线性的,因此我们需要通过以下步骤对目标函数进行求解:

  1. 给定初值
x_0

  1. 对于第
k

次迭代,寻找增量

\Delta x

,使

\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|_{2}^{2}

最小;

\Delta x

足够小,停止迭代;

  1. 否则,令
x_{k+1}=x_k+\Delta x

,返回步骤2;

 常见的寻找

\Delta x

的方法有:

 我们对上述目标函数进行泰勒展开:

F\left(\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_{k}\right) \approx F\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{T} \Delta \boldsymbol{x}_{k}+\frac{1}{2} \Delta \boldsymbol{x}_{k}^{T} \boldsymbol{H}\left(\boldsymbol{x}_{k}\right) \Delta \boldsymbol{x}_{k}

 其中,

J(x)

为一阶导数,即Jacobian矩阵,

H(x)

为二阶导数,即Hessian矩阵。我们将上式改写,则有下式:

\Delta \boldsymbol{x}=\underset{\Delta x}{\arg \min }\left(F(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{T} \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{T} \boldsymbol{H}(\boldsymbol{x}) \Delta \boldsymbol{x}\right)

1. 最速下降法

 我们将二阶导数忽略,只保留一阶导数,我们寻找最快下降方向,将导数取反,则可保证函数下降,则有:

\Delta \boldsymbol{x}=-\lambda \boldsymbol{J}(\boldsymbol{x})

 其中,

\lambda

称为步长,在深度学习中称为学习率。

 这种方法是最简单的非线性优化方法,但其需要进行很多次迭代。

2. 牛顿法

 我们将一阶导数,二阶导数全部保留,对增量

\Delta x

进行求导,并令其为0,则可以得到增量方程:

H \Delta x=-J^{T}

 则增量的解为:

\Delta x=-H^{-1}J^{T}

 这种方法比最速下降法迭代少,更精确,但其Hessian矩阵计算过于复杂。

3. 高斯牛顿法

 我们对

f(x)

进行一阶泰勒展开,则有:

f(x+\Delta x) \approx f(x)+J(x) \Delta x

 我们再对上式建立目标函数,如下所示:

\Delta x^{*}=\arg \min _{\Delta x} \frac{1}{2}\|f(x)+J(x) \Delta x\|_{2}^{2}

 我们对上式进行求导,并令导数为0,则可以得到下面方程:

J^{T} J \Delta x=-J^{T} f(x)

 将

J^TJ

记作

H

-J^Tf(x)

记作

g

,则有:

H \Delta x=g

 高斯牛顿法,使用

J^TJ

避免了对

H

矩阵直接计算,减少了复杂度,但是通常

H

是正定可逆的,一般情况下

J^TJ

却是不可逆的,此时方程陷入病态,导致无法求解,算法不收敛。继续改进。

4.LM算法

 高斯牛顿只有在展开点附件才会有比较好的近似效果,LM算法则给

\Delta x

增加了一个信赖域,来限制其大小,信赖域内部认为可信,否则不可信。目标函数如下:

\Delta \boldsymbol{x}=\underset{\Delta x}{\arg \min } \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{T} \Delta \boldsymbol{x}\right\|_{2}^{2}, \quad \text { s.t. }\|\boldsymbol{D} \Delta \boldsymbol{x}\| \leq \mu

 其中

\mu

为信赖域半径,

D

为系数矩阵,我们使用拉格朗日乘子将上式进行构造:

\mathcal{L}(\Delta \boldsymbol{x}, \lambda)=\frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{T} \Delta \boldsymbol{x}\right\|_{2}^{2}+\frac{\lambda}{2}\left(\|\boldsymbol{D} \Delta \boldsymbol{x}\|_{2}^{2}-\mu\right)

 其中

\lambda

为拉格朗日乘子,对上式进行求导,可得:

\Delta \boldsymbol{x}=-\left(\boldsymbol{J} \boldsymbol{J}^{T}+\lambda \boldsymbol{D}^{T} \boldsymbol{D}\right)^{-1} \boldsymbol{g}

 如果将信赖域当成球状,则有

D=I

,上式为:

\Delta \boldsymbol{x}=-\left(\boldsymbol{J} \boldsymbol{J}^{T}+\lambda I\right)^{-1} \boldsymbol{g}

 当

\lambda

较小时,LM算法接近高斯牛顿法,

\lambda

较大时,LM算法接近梯度下降法。

 此外信赖域大小我们可以通过实际模型和近似模型变化量的比值来确定,差异较小,我们就放大信赖域,差异较大,我们就缩小信赖域,比值定义如下:

\rho=\frac{f(\boldsymbol{x}+\boldsymbol{\Delta} \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x})^{T} \Delta \boldsymbol{x}}

 具体的算法流程为:

  1. 给定初值
x_0

,系数矩阵

D

和信赖域半径

\mu

;

k

次迭代求解增量

\Delta x_k

  1. 若增量足够小,停止迭代;
\rho > \frac{3}{4}

,则设置

\mu = 2\mu

,返回步骤2;

\rho<\frac{1}{4}

,则设置

\mu=\frac{1}{2}\mu

,返回步骤2;

\rho

大小合适,则

x_{k+1}=x_{k}+\Delta x_k

,返回步骤2;

5 Dog-Leg算法

 其结合了高斯牛顿法与最速下降法,我们先看最速下降法,有下式:

f\left(x+\alpha h_{s d}\right) \approx f(x)+\alpha J(x) h_{s d}

 则:

F\left(x+\alpha h_{s d}\right) \approx \frac{1}{2}\left\|f(x)+\alpha J(x) h_{s d}\right\|^{2}
=F(x)+\alpha h_{s d}^{T} J(x)^{T} f(x)+\frac{1}{2} \alpha^{2}\left\|J(x) h_{s d}\right\|^{2}

 我们对上式进行求导,令导数为0,可得:

\alpha=-\frac{h_{s d}^{T} J(x)^{T} f(x)}{\left\|J(x) h_{s d}\right\|^{2}}

 再看高斯牛顿法:

J^{T} J h_{gn}=-J^{T} f(x)

 至此我们有两个待选的解:

\alpha h_{sd}

h_{gn}

 Dog-Leg的迭代步长

h_{dl}

需要满足的关系式(trust region):

 if

\left\|h_{g n}\right\| \leq \Delta

h_{dl}=h_{gn}

 else if

\left\|\alpha h_{s d}\right\| \geq \Delta

h_{d l}=\frac{\Delta}{\left\|h_{g n}\right\| d} h_{s d}

 else

h_{d l}=\alpha h_{s d}+\beta\left(h_{g n}-\alpha h_{s d}\right)

,选择

\beta

使得

\left\|h_{d l}\right\|=\Delta

 信赖域半径计算与LM算法类似,只不过半径选择不一样:

\left\{\begin{array}{ll} \text { if } \rho>0.75 & \Delta=\max \left\{\Delta, 3 *\left\|h_{d l}\right\|\right\} \\ \text { if } \rho<0.25 & \Delta=\Delta / 2 \end{array}\right.

 具体的算法流程:

  1. 初始化
\Delta_0

  1. 求解最速下降法增量,如果太小,则退出;计算高斯牛顿法增量,如果太小,也退出;
  2. 计算信赖域半径,如果太小,则退出;
  3. 根据高斯牛顿法与最速下降法分别计算
h_{gn}

h_{sd}

,然后计算迭代步长

\alpha

  1. 根据3,4计算Dog-Leg步进值
h_{dl}

,若太小,则退出;

x_{k+1}=x_{k}+h_{dl}

,计算增益

\rho

更改信赖域半径,返回步骤2;

本文分享自微信公众号 - 科学计算technomania(Quant_Times),作者:大亮

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2021-04-29

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • VINS后端非线性优化目标函数

     vins在后端优化中,使用了滑动窗口,其状态向量包含窗口内的n+1个相机的状态(位置,旋转,速度,加速度计bias及陀螺仪bias)、相机到imu的外参、m+...

    猫叔Rex
  • 【优化3】非线性优化

    凸集和凸函数 ? ? ? SOCP ? Guideline ?

    用户1147754
  • SLAM初探(一)

    #经典SLAM过程 什么是SLAM SLAM (simultaneous localization and mapping),也称为CML (Concurren...

    Pulsar-V
  • 无人驾驶技术的灵魂——SLAM的现在与未来

    用户1737318
  • 详解|无人驾驶技术之魂——SLAM的现在与未来

    本文节选自图书《视觉SLAM十四讲:从理论到实践》,该书系统介绍了视觉SLAM(同时定位与地图构建)所需的基本知识与核心算法,既包括数学理论基础,又包括计算机视...

    AI科技大本营
  • 一文详解SLAM的主要任务和开源框架

    SLAM是Simultaneous localization and mapping缩写,意为“同步定位与建图”.

    3D视觉工坊
  • SLAM | 激光SLAM中开源算法对比

    好久没有更新SLAM系列的文章了,前面我们讲到了激光SLAM技术。基于激光雷达的同时定位与地图构建技术(simultaneous localization an...

    AI算法修炼营
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    SIGAI学习与实践平台
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    小白学视觉
  • 一文详解高精地图构建与SLAM感知优化建图策略

    高精度地图对自动驾驶系统功能研发的影响已经越来越明显,整体上来讲主要包含但不仅限于提升车端感知性能、拓展自动驾驶新功能、动态建图等相关应用。具体体现在如下几个重...

    3D视觉工坊
  • 2020年最新 iPad Pro上的激光雷达是什么?来激光SLAM技术中找答案

    日前,苹果公司正式发布了2020 iPad Pro。设备采用A12Z芯片,并包括Ultra Wide摄像头和液态视网膜显示屏,以及常规的摄像头、传感器和扬声器阵...

    CV君
  • 旷视SLAM组负责人刘骁:三维视觉与机器人

    ----大家好,我是旷视研究院SLAM组负责人刘骁,很高兴能和大家分享机器人领域一些有关三维视觉技术的思考。

    CV君
  • SLAM刚刚开始的未来之“工程细节”

    用户1737318
  • ICRA2021| Intensity-SLAM:基于强度辅助的大规模环境定位和建图

    论文、代码地址:在公众号「计算机视觉工坊」,后台回复「Intensity-SLAM」,即可直接下载。

    计算机视觉
  • 相机模型&非线性优化

    除内参外,相机坐标系与世界坐标系还差一个变换 先把P从世界坐标变到相机坐标系下 这里称为外参 右侧式子隐含了一次非齐次到齐次的变换

    小飞侠xp
  • 基于相交线的立体平面SLAM

    标题:Stereo Plane SLAM Based on Intersecting Lines

    点云PCL博主
  • 同时定位与地图创建综述

    SLAM包含两个主要任务,定位和建图。这是移动机器人自主完成作业任务需要解决的基本问题,特别是在未知环境的情况下,移动机器人既要确定自身在环境中的位姿,又要根据...

    3D视觉工坊
  • 经典视觉SLAM框架

    经典的视觉SLAM框架是过去十几年的研究成果。这个框架本身及其所包含的算法已经基本定型,并且已经在许多视觉程序库和机器人程序库中提供。依靠这些算法,我们能够构建...

    博文视点Broadview
  • 光场相机能否用于SLAM?

    本人研究生期间一直进行光场相机深度恢复的工作,深知其优势与不足。SLAM是我参加工作以来从事的研究方向,经过两年多的摸爬滚打算是入门了。目前视觉SLAM理论上虽...

    好好学SLAM

扫码关注云+社区

领取腾讯云代金券