视觉SLAM学习——视觉里程计VO

视觉SLAM学习——视觉里程计VO

本文为个人学习视觉slam之后的一些片面的理解，希望能够对大家有所帮助

文章目录

• 视觉SLAM学习——视觉里程计VO
• 特征点法
• ORB特征检测
• 1、FAST特征点提取（设提取n个角点）：
• 2、对FAST角点构建图像金字塔实现尺度不变性
• 3、对角点采用灰度质心法实现旋转不变性
• 4、获得角点的BRIEF描述子
• 特征匹配
• 相机位姿T估计及3D点P的求取
• 1、2D到2D过程
• 1.1、位姿T的获取：
• 1.2、3D点P的获取
• 1.3、最后可加入深度滤波器对获得的深度进行滤波
• 2、3D到2D过程
• 2.1 直接线性变换DLT：
• 2.2 非线性优化（Bundle Adjustment）
• 3、3D到3D过程
• 3.1 ICP(迭代最近点)
• 3.2 非线性优化方法
• 光流法
• 1、保留特征点，但只计算关键点，不计算描述子
• 2、LK光流法
• 3、ICP或对极几何估计相机运动T
• 4、三角测量获得特征点3D坐标P
• 直接法
• 1、保留特征点，但只计算关键点，不计算描述子
• 2、最小化光度误差求解相机位姿T：
• 3、三角测量获得特征点3D坐标P

特征点法

ORB特征检测

这里仅仅介绍ORB特征点的提取的算法过程：

1、FAST特征点提取（设提取n个角点）：

1.1、选取像素p，获得其亮度为Ip
1.2、设置阈值T（如：Ip的 20%）
1.3、以p为中心选取半径为3的圆上的16个像素点
1.4、预测试像素：检测索引为1、5、9、13的像素点，三个以上满足阈值条件才进行下一步
1.5、若连续N个大于（小于）Ip+T，则将其选为特征点（如FAST-12 : N=12）
1.6、计算Harris响应，选取前n个

2、对FAST角点构建图像金字塔实现尺度不变性

3、对角点采用灰度质心法实现旋转不变性

具体操作为连接区域的几何中心与图像质心，获得角点的向量特征

4、获得角点的BRIEF描述子

4.1、按照某概论分布随机挑选附近两个像素（p，q）
4.2、比较两个像素点大小并获取0，1编码，若p>q则 取1，否则取0
4.3、计算n组像素，获得其向量作为描述子

特征匹配

特征匹配基本原理即是：匹配相邻两帧间的特征点的描述子，获得n组匹配点
这里举两种匹配方法：
1、快速近似最邻近算法ANN：利用局部敏感哈希方法计算
2、汉明距离：两个等长字符串在对应位置上不同字符的数目
至此获得了匹配上的特征点

相机位姿T估计及3D点P的求取

1、2D到2D过程

单目视觉的初始化是一个2D到2D的过程：

1.1、位姿T的获取：

1.1.1、位姿T的获取采用对极几何的方法构建对极约束
1.1.2、经过一系列变换，最终获得本质矩阵E=t^R, 且$x_2^T*E*x_1=0$，t为平移，R为旋转
1.1.3、由于E具有尺度等价性，故可用利用八点发求E
1.1.4、获得本质矩阵E后，可采用奇异值分解(SVD)：E=U∑V(T)，求得t与R

1.2、3D点P的获取

1.2.1、利用三角测量的方法获得方程：$s_1*x_1=s_2*R*x_2+t$，其中s为深度值，x为匹配好的归一化平面点(u,v,1)
1.2.2、对方程左乘x1^ ：s1*x1^ *x1=0=s2*x1^ R*x2+x1^t
1.2.3、利用等式右边可求得x2深度s2的最小二乘解
1.2.4、带入原式可求得s1

1.3、最后可加入深度滤波器对获得的深度进行滤波

2、3D到2D过程

单目初始化完成之后，即可视为一个3D到2D估计的过程，这里介绍两种方法

2.1 直接线性变换DLT：

2.1.1、利用s*x=[R|t]*P获得两个约束，其中s为深度，x为归一化平面坐标(u,v,1)，R|t为3 * 4的矩阵，表姿态，P表世界坐标，tn表R|t的第n行：
$t_1^T*P-t_3^T*P*u=0$
$t_2^T*P-t_3^T*P*v=0$
2.1.2、六组以上的点即可直接求解12个未知数(R:9, t:3),获得位姿T

2.2 非线性优化（Bundle Adjustment）

2.2.1、 构建最小化重投影误差方程

e = 1/2∑( ||u_i-1/s_i*K*exp(ε^ )*Pi||^2 ) 
ε^*=argmin(e) 

相机位姿优化：通过e对ε求偏导可得姿态的雅克比矩阵
特征点空间位置优化：求解e对P的偏导可得空间点的雅克比矩阵

2.2.2、非线性优化方法：
由于上式想要通过解析的方法求得最优解在大矩阵中几乎不可能，故通常采用非线性优化的方式来求取最优解：

一阶梯度法（最速下降法）：
將误差方程式$||f(x)||^2$泰勒展开，保留一阶，并求得增量解⊿x*λ（步长）；
迭代：若⊿x足够小，停止，否则，继续计算⊿x并迭代；

二阶梯度法（牛顿法）：
將误差方程式$||f(x)||^2$泰勒展开，保留二阶项，求解关于⊿x倒数为零的解，获得步长；
得到$H⊿x=-J^T$，H为⊿x的海赛矩阵，J为⊿x的雅克比矩阵。

高斯牛顿法：
构建方程：
$⊿x^*=argmin1/2||f(x)+J(x)*⊿x||^2$
对⊿x求导；
得到：$J(x)^T*J(x)*⊿x=-J(x)^Tf(x)$
將左边$J(x)^T*J(x)$定义为H，右边$-J(x)^T$定义为g，则方程可写为：
$H*⊿x = g$
迭代x即可求得最优解

列文伯格-马夸尔特方法（阻尼牛顿法）：
给定初始值x0，初始化优化半径 μ（信赖区域）；
对于第k次迭代求解：
$⊿x^*=argmin1/2||f(x)+J(x)*⊿x||^2, s.t.||D*⊿xk||^2 ≤μ$
计算ρ=(f(x+⊿x) - f(x))/J(x)*⊿x, 表近似模型与实际函数差异；
根据ρ调整信赖区域大小μ；
若ρ大于某阈值则近似可行，令x(k+1)=x(k)+⊿x；
判断算法收敛。

3、3D到3D过程

3D到3D过程一般在深度相机建图过程中使用。一般可采用ICP或非线性优化求解

3.1 ICP(迭代最近点)

3.1.1 利用重投影误差构建最小二乘问题 $minJ=1/2∑||p_i - (R*p_i'+t)||^2$
3.1.2 引入两组点质心$p=1/n∑p_i, p'=1/n∑p_i'$，带入上式，变换可得：
$minJ=1/2∑||pi -p - R*(pi'-p)||^2 + ||p -R*p'-t||$
由于左边一项之和R相关，根据 $R^*= argmin1/2∑||q_i - R*q_i'||^2$，利用奇异值分解求得R；
將R带入式$t^*=p-Rp'$ ，计算t，获得相机位姿T

3.2 非线性优化方法

3.2.1 构建误差方程e = 1/2∑( ||pi-exp(ε^ )pi’||^2 )，ε=argmin(e)；
3.2.2 利用列文伯格-马夸尔特方法（阻尼牛顿法）求解

光流法

光流法时基于灰度不变假设的一种算法，具体算法步骤如下：

1、保留特征点，但只计算关键点，不计算描述子

2、LK光流法

2.1、根据同一窗口像素具有相同运动建立方程$I(x+dx, y+dy, t+dt) = I(x, y, t)$，其中I表示光强。
2.2、对$I(x+dx, y+dy, t+dt)$的泰勒式减去I(x, y, t)应为0，对余项利用最小二乘求解像素运动速度u=dx/dt，v=dy/dt。

3、ICP或对极几何估计相机运动T

4、三角测量获得特征点3D坐标P

直接法

直接法同光流法一样也是基于灰度不变假设

1、保留特征点，但只计算关键点，不计算描述子

2、最小化光度误差求解相机位姿T：

2.1、构建误差方程：$minJ = ∑e_i^T*e_i，e_i=I_1(p_1,i) - I_2(p_2,i)$，pi为匹配好的特征点
2.2、对上式进行李代数求导获得位姿对应的雅可比矩阵J
2.3、列文伯格-马夸尔特方法（阻尼牛顿法）求解

3、三角测量获得特征点3D坐标P

来源：CSDN

作者：qq_37702859

链接：https://blog.csdn.net/qq_37702859/article/details/103764815