SLAM--相机图像与非线性优化

第5讲 相机与图像

1 相机模型

相机模型是用来描述将三维世界坐标系中的坐标点映射到二维图像平面的过程 
使用针孔和畸变两个模型来描述整个投影过程。
1.1 针孔相机模型

(1) 相机坐标系内中物理坐标点Pc~=[X,Y,Z]TPc~=[X,Y,Z]T, 
(2) 成像平面内成像坐标点P′=[X′,Y′,Z′]TP′=[X′,Y′,Z′]T, 焦距为ff, 
通过对称成像平面和归一化成像平面得到二者关系为 
X′=fXZY′=fYZ
X′=fXZY′=fYZ

(3) 像素坐标系内像素坐标为[u,v]T[u,v]T,像素坐标系和成像平面相差了一个缩放(u轴缩放α倍,v轴缩放β倍u轴缩放α倍,v轴缩放β倍)和一个原点的平移[cx,cy]T[cx,cy]T 
p′p′和像素坐标[u,v]T[u,v]T关系为 
u=αX′+cxv=βY′+cy
u=αX′+cxv=βY′+cy

代入上式得到像素坐标和相机坐标系内点的关系, 
u=fxXZ+cxv=fyYZ+cy
u=fxXZ+cxv=fyYZ+cy

其中,
fx=αf,fy=βf
fx=αf,fy=βf

齐次化写成矩阵形式, 
⎡⎣⎢uv1⎤⎦⎥=1Z⎡⎣⎢fx000fy0cxcy1⎤⎦⎥⎡⎣⎢XYZ⎤⎦⎥=1ZKPc~
[uv1]=1Z[fx0cx0fycy001][XYZ]=1ZKPc~
按照传统习惯将ZZ挪到左侧,表达的就是相机坐标系中点和像素坐标系中像素的关系 
Z⎡⎣⎢uv1⎤⎦⎥=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥⎡⎣⎢XYZ⎤⎦⎥=KPc~
Z[uv1]=[fx0cx0fycy001][XYZ]=KPc~

把中间量KK称作相机内参。
K=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥
K=[fx0cx0fycy001]

相机坐标PP是由它的世界坐标PwPw变换而来,相机位姿由R,tR,t决定。那么有下式成立, 
ZPuv=Z⎡⎣⎢uv1⎤⎦⎥=K(RPw+t)=K(TPw)(1:3)
ZPuv=Z[uv1]=K(RPw+t)=K(TPw)(1:3)

最后一个式子里面隐含着一次齐次坐标到非齐次坐标的转换(TPwTPw的最后一维是1),也就是
Pc~=K(RPw+t)=(TPw)(1:3)=⎡⎣⎢XYZ⎤⎦⎥
Pc~=K(RPw+t)=(TPw)(1:3)=[XYZ]

两边除以ZZ, 所以,
Puv=1ZK(TPw)(1:3)=1ZKPc~=K⎡⎣⎢⎢XZYZ1⎤⎦⎥⎥=KPc
Puv=1ZK(TPw)(1:3)=1ZKPc~=K[XZYZ1]=KPc

其中Pc=[XZ YZ 1]Pc=[XZ YZ 1]称为归一化坐标,二维的齐次坐标,它**位于相机前方z=1z=1的平面上,叫做归一化平面**。
变换过程总结: 
世界坐标系下点Pw−→−−−R,t或者TPc~=RPw+t=[X Y Z]−→−−−−−−−投影到归一化平面Pc=⎡⎣⎢⎢XZYZ1⎤⎦⎥⎥−→−−−−−经过相机内参Puv=KPcPw→R,t或者TPc~=RPw+t=[X Y Z]→投影到归一化平面Pc=[XZYZ1]→经过相机内参Puv=KPc
1.2 畸变

径向畸变 
透镜的形状引起,有桶形失真和枕形失真。
切向畸变 
相机组装过程不能使透镜和成像平面严格平行引起。
一般使用的去畸变处理方法:先对整张图像去畸变,得到去畸变后的图像,然后讨论图像上点的空间位置。

1.3 双目相机模型

根据三角形相似得到 
z−fz=b−uL+uRb
z−fz=b−uL+uRb

整理得到 
z=fbd,d=uL−uR
z=fbd,d=uL−uR
1.4 RGBD相机模型

原理

红外结构光
飞行时间法
2 图像的存储和访问

第六讲:非线性优化

非线性最小二乘问题

因为它非线性,所以df/dx有时候不好求,那么可以采用迭代法(有极值的话,那么它收敛,一步步逼近):

SLAM--相机图像与非线性优化_第1张图片

这样求导问题就变成了递归逼近问题,那么增量△xk如何确定?

这里介绍三种方法:

(1)一阶和二阶梯度法

将目标函数在x附近进行泰勒展开:

SLAM--相机图像与非线性优化_第2张图片

(2)高斯牛顿法

将f(x)一阶展开:(1)

这里J(x)为f(x)关于x的导数,实际上是一个m×n的矩阵,也是一个雅克比矩阵。现在要求下降矢量△x,使得||f(x+△x)|| 达到最小。

为求△ x,我们需要解一个线性的最小二乘问题:

这里注意变量是△ x,而非之前的x。所以是线性函数,好求。为此,先展开目标函数的平方项:

SLAM--相机图像与非线性优化_第3张图片

我们称为高斯牛顿方程。我们把左边的系数定义为H,右边定义为g,那么上式变为:

H △x=g

跟牛顿法对比可以发现,我们用J(x)TJ(x)代替H的求法,从而节省了计算量。所以高斯牛顿法跟牛顿法相比的优点就在于此,步骤列于下:

但是,还是有缺陷,就是它要求我们所用的近似H矩阵是可逆的(而且是正定的),但实际数据中计算得到的JTJ却只有半正定性。也就是说,在使用Gauss Newton方法时,可能出现JTJ为奇异矩阵或者病态(ill-condition)的情况,此时增量的稳定性较差,导致算法不收敛。更严重的是,就算我们假设H非奇异也非病态,如果我们求出来的步长△x太大,也会导致我们采用的局部近似(1)不够准确,这样一来我们甚至都无法保证它的迭代收敛,哪怕是让目标函数变得更大都是可能的。

所以,接下来的Levenberg-Marquadt方法加入了α,在△x确定了之后又找到了α,从而||f(x+α△x)||2达到最小,而不是直接令α=1。

(3)列文伯格-马夸尔特方法(Levenberg-Marquadt法)

由于Gauss-Newton方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果,所以我们很自然地想到应该给△x添加一个信赖区域(Trust Region),不能让它太大而使得近似不准确。非线性优化中有一系列这类方法,这类方法也被称之为信赖区域方法(Trust Region Method)。在信赖区域里边,我们认为近似是有效的;出了这个区域,近似可能会出问题。

那么如何确定这个信赖区域的范围呢?

一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围:如果差异小,我们就让范围尽可能大;如果差异大,我们就缩小这个近似范围。因此,考虑使用:来判断泰勒近似是否够好。ρ的分子是实际函数下降的值,分母是近似模型下降的值。如果ρ接近于1,则近似是好的。如果ρ太小,说明实际减小的值远少于近似减小的值,则认为近似比较差,需要缩小近似范围。反之,如果ρ比较大,则说明实际下降的比预计的更大,我们可以放大近似范围。

于是,我们构建一个改良版的非线性优化框架,该框架会比Gauss Newton有更好的效果。

1. 给定初始值x0,以及初始化半径μ。

2. 对于第k次迭代,求解:

 (2)

 这里面的限制条件μ是信赖区域的半径,D将在后文说明。

3. 计算ρ。

4. 若ρ>3/4,则μ=2μ;

5. 若ρ<1/4,则μ=0.5μ;

6. 如果ρ大于某阈值,认为近似可行。令xk+1=xk+△xk。

7. 判断算法是否收敛。如不收敛则返回2,否则结束。

这里近似扩大范围的倍数和阈值都是经验值,亦可替换成别的数值。在式(1)中,我们把增量范围限定在一个半径为μ的球中。带上D之后,这个球可以看成是个椭球。

(Levenberg提出的优化方法中,把D取成单位矩阵I,相当于直接把△x约束在球中。Marquart提出将D取成非负数对角阵---通常用的对角元素平方根。)

无论怎样,都需要解(2)这样一个问题来获得梯度,其是带不等式约束的优化问题,我们用Lagrange乘子将它转化为一个无约束优化问题:

λ为Lagrange乘子。展开后,化简得到:

当λ比较小时,H占主要地位,接近于牛顿高斯法;当λ比较大时,接近于最速下降法。

L-M的求解方式,可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题,提供更稳定更准确的增量△x。

 

 

小结:

SLAM--相机图像与非线性优化_第4张图片

SLAM--相机图像与非线性优化_第5张图片

SLAM--相机图像与非线性优化_第6张图片

           视觉SLAM里,这个矩阵往往有特定的稀疏形式,为实时求解优化问题提供了可能性。利用

稀疏形式的消元,分解,最后再求解增量,会让求解效率大大提高。(第十讲)

  

实践:

1.Ceres库(谷歌)

SLAM--相机图像与非线性优化_第7张图片

SLAM--相机图像与非线性优化_第8张图片

 

SLAM--相机图像与非线性优化_第9张图片

 2.g20库(基于图优化)

SLAM--相机图像与非线性优化_第10张图片

 

SLAM--相机图像与非线性优化_第11张图片

 

g20步骤:

SLAM--相机图像与非线性优化_第12张图片

 

 总结:

总的来说,最大似然估计转变为求解最小二乘法问题,然后求解时将非线性最小二乘转变为线性最小二乘问题。在求解增量方面最直接的方式就是将目标函数f(x)在x出泰勒展开:

J为雅克比矩阵,H为海塞矩阵。在高斯牛顿法中H=JT*J;

如果保留一阶梯度,那么增量的方向为:

如果保留二阶梯度信息,那么增量方程为:

以下为相关资料:

Jacobian矩阵和Hessian矩阵:https://blog.csdn.net/qq_22424571/article/details/81952037

高斯牛顿(Gauss Newton)、列文伯格-马夸尔特(Levenberg-Marquardt)最优化算法与VSLAM:https://blog.csdn.net/zhubaohua_bupt/article/details/74973347

矩阵求导、几种重要的矩阵及常用的矩阵求导公式:https://blog.csdn.net/daaikuaichuan/article/details/80620518

https://blog.csdn.net/pipisorry/article/details/68961388

 

Ubuntu16.04安装pcl出现cannot find -lvtkproj4解决办法:https://blog.csdn.net/danmeng8068/article/details/77341532

 

你可能感兴趣的:(SLAM--相机图像与非线性优化)