总结一下SLAM中关于非线性优化的知识。

先列出参考:

http://jacoxu.com/jacobian%E7%9F%A9%E9%98%B5%E5%92%8Chessian%E7%9F%A9%E9%98%B5/

http://blog.csdn.net/dsbatigol/article/details/12448627

http://www.cnblogs.com/rongyilin/archive/2012/12/21/2827898.html

《视觉SLAM十四讲》。

1. 雅克比矩阵 && 海森矩阵

  雅克比矩阵(Jacobian)是一阶偏导数以一定方式排列成的矩阵,其行列式称为雅克比行列式。它体现了一个可微方程与给出点的最优线性逼近,类似与多元函数的导数。

  假设F:Rn→Rm是一个从欧式n维空间转换到欧式m维空间的函数,这个函数由m个实函数组成:u1(x1,...,xn),...,um(x1,...,xn)。假设这些函数的偏导数存在,则雅克比矩阵可以写成:

  

  海森矩阵(Hessian)是一个自变量为向量的实值函数的二阶偏导数组成的方块矩阵,设函数为:f(x1,x2,...,xn),并假设f的二阶导数粗在,则其海森矩阵为:

  海森矩阵常被应用于牛顿法解决大规模的优化问题,具体可参考:http://jacoxu.com/jacobian%E7%9F%A9%E9%98%B5%E5%92%8Chessian%E7%9F%A9%E9%98%B5/。

2.最小二乘问题

  最小二乘法(又称最小平方法)是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。

2.1 线性最小二乘

  已知M个N+1维空间点:

其中,

如何求的函数f(x):

可以构建最小二乘问题估计f(x)的参数:

写成矩阵形式为:

其中:

为了最小化上式,对w求导,并另导数等于0:

上式的获取需要先展开平方项,然后进行矩阵的求导。只要求出w矩阵就可以了,一般线性最小二乘需要用到Cholesky的分解,具体的求解过程就不展开了。

2.2 非线性最小的二乘

  非线性最小二乘指的是f(x)函数是一个非线性函数,导致导数的求解变的非常困难,因此需要求解非线性最小二乘问题的方法。而求解方法中就包括最速下降法,牛顿法,高斯-牛顿法(G-N),列文伯格-马夸尔特法(L-M)等,它们都是通过迭代的方式逼近最优解,下面分别做一个简单的介绍。

3. 1最速下降法

  它的思想是每次迭代选取一个步长λ,根据函数的梯度选择下降最快的方向,使得目标函数的值能够最大程度的下降。这里主要是理解算法思想,具体的算法操作还应该查阅相关书籍。我们假设f(x)是一个待优化的函数(其实对于最小二乘来说,它应该是一个残差函数,有的地方也叫cost founction):min1/2||f(x)||2,对其进行一阶泰勒展开:

其中的J(x)为雅克比矩阵,即函数平方关于x的导数,根据上式可以求取增量和步长λ,然后进入迭代循环。但是该方法收敛速度比较慢,直接使用最速下降法效果不好。

3.2 牛顿法

  牛顿法相比最速下降法更进一步,对待优化函数进行二阶泰勒展开:

则增量为:

它是一个关于雅克比矩阵和海森矩阵的问题,所以计算量稍大。

3.3 高斯-牛顿法

  可以看到上面的方法都是对残差函数的平方项直接进行泰勒展开,而高斯牛顿法则是对f(x)进行泰勒展开,然后对展开项进行平方和:

  

由于每次迭代都需要求解一个增量,现在可以构建关于增量的线性最小二乘问题:

因此只要对上式求导,并另导数为0,就能求解增量:

通常上式都写成如下形式:

获得这个增量方程,需要对上面的线性最小二乘问题进行矩阵的平方项计算以及相应的矩阵求导,其实这和前面说过的非线性最小二乘问题本质上是一样的,获得的数学结论也类似,但是关于上式的求解方法在SLAM系统里跟线性方法不太一样。

3.4 列文伯格-马夸尔特法

  L-M法是对高斯牛顿法的一个改进,给增量添加一个信赖区域:

分子是实际函数的下降值,分母是近似模型的下降值(分母的问题可以回忆一下微积分)。上式太小,说明实际下降值小于近视下降值,所以你的估计下降的太快了;如果上式太大,说明实际下降值大于近似下降值,所以你可以快点下降没关系。这样,高斯牛顿法的最小二乘问题变成:

用拉格朗日乘数法就是:

同样进行平方项的展开以及对增量的求导:

这就是L-M中的增量方程。

SLAM中的非线性优化的更多相关文章

  1. SLAM中的EKF,UKF,PF原理简介

    这是我在知乎上问题写的答案,修改了一下排版,转到博客里.   原问题: 能否简单并且易懂地介绍一下多个基于滤波方法的SLAM算法原理? 目前SLAM后端都开始用优化的方法来做,题主想要了解一下之前基于 ...

  2. SLAM中的优化理论(二)- 非线性最小二乘

    本篇博客为系列博客第二篇,主要介绍非线性最小二乘相关内容,线性最小二乘介绍请参见SLAM中的优化理论(一)-- 线性最小二乘.本篇博客期望通过下降法和信任区域法引出高斯牛顿和LM两种常用的非线性优化方 ...

  3. 视觉SLAM中的数学基础 第四篇 李群与李代数(2)

    前言 理解李群与李代数,是理解许多SLAM中关键问题的基础.本讲我们继续介绍李群李代数的相关知识,重点放在李群李代数的微积分上,这对解决姿态估计问题具有重要意义. 回顾 为了描述三维空间里的运动,我们 ...

  4. SLAM中的优化理论(一)—— 线性最小二乘

    最近想写一篇系列博客比较系统的解释一下 SLAM 中运用到的优化理论相关内容,包括线性最小二乘.非线性最小二乘.最小二乘工具的使用.最大似然与最小二 乘的关系以及矩阵的稀疏性等内容.一方面是督促自己对 ...

  5. 视觉SLAM中相机详解

    视觉SLAM中,通常是指使用相机来解决定位和建图问题. SLAM中使用的相机往往更加简单,不携带昂贵的镜头,以一定的速率拍摄周围的环境,形成一个连续的视频流. 相机分类: 单目相机:只是用一个摄像头进 ...

  6. SLAM中的变换(旋转与位移)表示方法

    1.旋转矩阵 注:旋转矩阵标题下涉及到的SLAM均不包含位移. 根据同一点P在不同坐标系下e(e1,e2,e3)e'(e1',e2',e3')的坐标a(a1,a2,a3)a'(a1',a2',a3') ...

  7. 视觉SLAM中的数学基础 第二篇 四元数

    视觉SLAM中的数学基础 第二篇 四元数 什么是四元数 相比欧拉角,四元数(Quaternion)则是一种紧凑.易于迭代.又不会出现奇异值的表示方法.它在程序中广为使用,例如ROS和几个著名的SLAM ...

  8. 视觉SLAM中的数学基础 第三篇 李群与李代数

    视觉SLAM中的数学基础 第三篇 李群与李代数 前言 在SLAM中,除了表达3D旋转与位移之外,我们还要对它们进行估计,因为SLAM整个过程就是在不断地估计机器人的位姿与地图.为了做这件事,需要对变换 ...

  9. SLAM中的卡方分布

    视觉slam中相邻帧特征点匹配时,动辄上千个特征点,匹配错误的是难免的,而误匹配势必会对位姿精度以及建图精度造成影响,那么如何分辨哪些是误匹配的点对儿呢?如果已知两帧的的单应矩阵,假设单应矩阵是没有误 ...

随机推荐

  1. Java连载44-静态代码块、实例代码块、继承

    一.可以使用static关键字来定义“静态代码块” 1.语法规则 static { java语句: } 2.静态代码块在类加载时执行,并且只执行一次 3.静态代码块在一个类中可以编写多个,并且遵循自上 ...

  2. Tree-Shaking性能优化实践 - 原理篇

    Tree-Shaking性能优化实践 - 原理篇   一. 什么是Tree-shaking 先来看一下Tree-shaking原始的本意 上图形象的解释了Tree-shaking 的本意,本文所说的前 ...

  3. Python模块import本质是什么?import是什么

    ​ 写这篇文章主要是对Python的import模块或包的机制有了更深层级的一个理解,也在具体工作中得到了一点实践,这种思考是由上一篇文章<__main__内置模块预加载Shotgun接口的妙用 ...

  4. Android方法数超出限定的问题(multiDex,jumboMode)

    在Android项目开发中,项目代码量过大或通过引入很多jar导致代码量急剧增加,会出现错误: android.dex.DexIndexOverflowException: Cannot merge ...

  5. 【语义分割】Stacked Hourglass Networks 以及 PyTorch 实现

    Stacked Hourglass Networks(级联漏斗网络) 姿态估计(Pose Estimation)是 CV 领域一个非常重要的方向,而级联漏斗网络的提出就是为了提升姿态估计的效果,但是其 ...

  6. 天天向上的力量python(举一反三)

    天天向上的力量python实例(举一反三) 实例1: 一年365天,以第1天的能力值为基数,记为1.0,当好好学习时能力值相比前一天提高0.1%,没有学习实能力值相比前一天下降0.1%. 问:每天努力 ...

  7. Z从壹开始前后端分离【 .NET Core2.2/3.0 +Vue2.0 】框架之五 || Swagger的使用 3.3 JWT权限验证【必看】

    本文梯子 本文3.0版本文章 前言 1.如何给接口实现权限验证? 零.生成 Token 令牌 一.JWT ——自定义中间件 0.Swagger中开启JWT服务 1:API接口授权策略 2.自定义认证之 ...

  8. SSM框架之Mybatis(1)入门

    Mybatis(1)入门 1.mybatis的概述 mybatis是一个持久层框架,用java编写的. 它封装了jdbc操作的很多细节,使开发者只需要关注sql语句本身,而无需关注注册驱动,创建连接等 ...

  9. LFI (local file inclusion vulnerability)本地文件包含

    代码实例: <?php $file = $_GET['file']; if(isset($file)) { include("pages/$file"); } else { ...

  10. Python工具库分享

    漏洞及渗透练习平台: WebGoat漏洞练习平台: https://github.com/WebGoat/WebGoat webgoat-legacy漏洞练习平台: https://github.co ...