视觉SLAM十四讲_视觉里程计2

---

前言

如有侵权,请告知删除

B站一位学姐的SLAM十四讲讲解的笔记.后续在此基础上继续完善.

问题引出:
$$\begin{gathered} \boxed{特征点法}\{\begin{array}{l}理解图像特征点的意义,掌握在单幅图像中提取特征点及多幅图像中匹配特征点的方法\\ 理解对极几何的原理,利用对极几何的约束,恢复出图像之间的摄像机的三维运动\\ 理解如何通过三角化,获得二维图像上对应的三维结构\\ 理解PNP问题,及利用已知三维结构与图像的对应关系,求解摄像机的三维运动.\\ 理解ICP问题.及利用点云的匹配关系,求解摄像机的三维运动.\end{array} \\ \Downarrow \\ \boxed{特征点法问题}\{\begin{array}{ll}在图像中\mathbf{提取}特征点并计算特征描述& 非常耗时\\ 通过计算描述子距离在不同图像中寻找\mathbf{匹配}& 非常耗时\\ 利用通过匹配点信息计算相机位姿& 正常\end{array} \end{gathered}$$

$$\boxed{改进:}\{\begin{array}{ll}通过其他方式寻找匹配点(\cancel{描述子})& 光流\\ 1.提取可能是随机点的特征点;2.不去进行匹配,直接进行位姿估计& 直接法\end{array}$$

---

提示：以下是本篇文章正文内容，下面内容仅供参考

一、光流法

一种描述像素随时间在图像之间运动的方法, 本质上是估计像素在不同时刻图像中的运动。(希望解出像素到了下一刻的那个位置)

$$\{\begin{array}{ll}稀疏光流& LK光流为代表\\ 稠密光流& HS光流为代表\end{array}$$
在视觉SLAM中,可以用光流去追一些特征点,角点(一张图像上的部分像素),所以一般用的都是稀疏光流。

1.1、LK光流

• 设在t时刻位于(x,y)处的像素的灰度为I(x,y,t) ; 其中I(x,y,t)为 在t时刻,x,y坐标的灰度值
• 在t+Δt时刻运动了(x+dx,y+dy)处,其灰度值为:I(x+dx,y+dy,t+dt)
  我们想计算dx,dy
• 假设灰度不变I(x+dx,y+dy,t+dt)=I(x,y,t)
  (是一个理想的假设,实际上可能会不成立)

$$\begin{gathered} 多元函数(x_k^1,x_k^2,....,x_k^n)处的泰勒展开公式: \\ f(x^1,x^2,....,x^n)=f(x_k^1,x_k^2,....,x_k^n)+\sum _{i=1}^n(x^i-x_k^i)f_{x^i}^{\prime }(x_k^1,x_k^2,....,x_k^n) \\ +\frac{1}{2!}\sum _{i,j=1}^n(x^i-x_k^i)(x^j-x_k^j)f_{ij}^{{}^{\prime \prime }}(x_k^1,x_k^2,....,x_k^n)+O^n \end{gathered}$$

$$\begin{gathered} I(x+dx,y+dy,t+dt)=I(x,y,t) \\ 对t+dt时刻的灰度进行泰勒一阶展开并保留一阶项 \\ I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt \\ 由灰度不变\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0⟹\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}. \\ \Downarrow \\ {I}_{x}u+I_{y}v=-I_t \\ \begin{array}{c}\frac{\partial I}{\partial x}:图像在x方向上的梯度I_x.\\ \\ \frac{\partial I}{\partial y}:图像在y方向上的梯度I_y\\ \\ \frac{\partial x}{\partial t}:x方向的速度u\\ \\ \frac{\partial x}{\partial t}:y方向的速度v\\ \\ \frac{\partial I}{\partial t}:图像灰度关于时间的变化I_t\end{array} \end{gathered}$$

---

$$I_{x}u+I_{y}v=-I_t$$
这里未知量是u和v, 上式为一个二元一次方程,使用一个像素点无法求出
–需要引用额外的约束
(不仅假设像素灰度值不变,而且要假定一个窗口内的像素具有相同的运动)
考虑一个大小为w×w的窗口,则含有w²数量个像素
$$[I_x{I}_y{]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-I_{tk},k=1,2,...,w^2.$$
超定方程,通过最小二乘解出来求得运动u,v
$$A=\left[\begin{array}{c}[I_x,I_y{]}_1\\ ⋮[I_x,I_y{]}_k\end{array}\right],b=\left[\begin{array}{c}{I}_{t}1\\ ⋮\\ I_{tk}\end{array}\right].⟹A\left[\begin{array}{c}u\\ v\end{array}\right]=-b.⟹{\left[\begin{array}{c}u\\ v\end{array}\right]}^{\ast }=-(A^{T}A{)}^{-1}{A}^{T}b.$$

$$\begin{gathered} 最小二乘解:Ax=-b则e=Ax+b \\ \min ||e|{|}_2=\min (Ax+b{)}^T(Ax+b)=\min (x^T{A}^{T}Ax+2x^T{A}^{T}b+b^{T}b) \\ \frac{d||e|{|}_2}{dx}=2A^{T}Ax+2A^{T}b=0 \\ x=-(A^{T}A{)}^{-1}{A}^{T}b \end{gathered}$$

---

问题:我们把光流写成了非线性优化问题,就必须假设优化的初始值靠近最优解.才能在一定程度上保证算法的熟练。如果相机运动较快,两章图像差异较为明显,那么单层图像光流法容易达到一个局部最小值。这种情况可以通过图像金字塔来改善。
解决:图像金字塔是指对不同一个图像进行缩放,得到不同分辨率的图像, 如图8-2所示.
流程图:
  由粗至精的好处在于,当原始图像的像素运动较大时,在金字塔顶层的图像看来,运动仍然在一个很小的范围内。例如,原始图像的特征点运动了20个像素,很容易由于图像非凸性导致优化困在极小值里。但现在假设没有缩放倍率0.5倍的金字塔,那么往上两层图像里,像素运动就只有5个像素了,这时结果就明显好于直接在原始图像上优化。

二、直接法

• 光流仅估计了像素间的平移,但是没有用到相机本身的集合结构(投影模型)以及考虑相机的旋转和图像的缩放
• 直接法考虑了这些信息
  

直接法的推导:
已知:三维点在第一个相机坐标系下的三维坐标$$P=\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)$$两张图像的灰度值; 三维点投影在第一章图像上的像素坐标p₁;
未知:图像1到图像2的位姿
假设:灰度不变假设 (光度不变)
目标:$$\underset{T}{\min }J(T)=\sum _{i=1}^N{e}_i^T{e}_i,e_i=I_1(p_{1.i})-I_2(p_{2,i}).$$
思想:先初始化一个不准确的位姿,计算p₂,根据p₂和p₁的灰度值差异,不断迭代调整位姿

2.1 公式推导

像素坐标与相机坐标:
$$\begin{gathered} p_1={\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}_1=\frac{1}{Z_1}KP. \\ {p}_2={\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}_2=\frac{1}{Z_2}K(\underset{\mathrm{q}}{\underbrace{\boxed{RP+t}}})=\frac{1}{Z_2}K(exp({\xi }^{\wedge })P{)}_{1:3} \\ 其中q是以p_2为相机坐标 \end{gathered}$$

$$\underset{T}{\min }J(T)=\sum _{i=1}^N{e}_i^T{e}_i,e_i=I_1(p_{1.i})-I_2(p_{2,i}).$$
q为了p在第二个相机坐标系下的坐标,u为它的像素坐标
$$e(T)=I_1(p_1)-I_2(u),$$
我们关心误差e是如何随着相机位姿T变化的,需要分析他们的导数关系
$$\frac{\partial e}{\partial T}=-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta \xi }$$

• (1)$$\frac{\partial I_2}{\partial u}为u处的像素梯度$$
• (2)$$\frac{\partial u}{\partial q}为投影方程关于相机坐标系下的三维点的导数,记q=\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)$$
  $$u=\frac{f_{x}X+c_x}{Z},v=\frac{f_{y}Y+c_y}{Z}⟹\frac{\partial u}{\partial q}=\left[\begin{array}{ccc}\frac{\partial u}{\partial X}& \frac{\partial u}{\partial Y}& \frac{\partial u}{\partial Z}\\ & & \\ \frac{\partial v}{\partial X}& \frac{\partial v}{\partial Y}& \frac{\partial v}{\partial Z}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ & & \\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]$$
  (3) $$\begin{gathered} \frac{\partial q}{\partial \delta \xi }为变换后的三维点对变换的导数: \\ \frac{\partial q}{\partial \delta \xi }=[I,-q^{\wedge }]. \end{gathered}$$
  由(2)和(3)上式可知:
  $$\frac{\partial u}{\partial \delta \xi }=\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& -\frac{f_{x}XY}{Z^2}& f_x+\frac{f_x{X}^2}{Z^2}& -\frac{f_{x}Y}{Z}\\ & & & & & \\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z^{\prime }}\end{array}\right]$$
  误差相对于李代数的雅可比矩阵:
  $$J=-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \delta \xi }.$$

— 根据使用的图像信息不同,可以分为:
稀疏直接法:     只处理稀疏角点或关键点
半稠密直接法: 使用部分梯度明显的像素
稠密直接法:     使用所有像素

---

总结

引用:
视觉SLAM十四讲_视觉里程计