推广优化公司网站,关键词搜索名词解释,重庆网站制作长沙,网站开发交接协议书本文总结视觉SLAM中常用的光流法与直接法
1、Lucas-Kanade光流法
相机所拍摄到的图像随相机视角的变化而变化#xff0c;这种变化也可以理解为图像中像素的反向移动。“光流”#xff08;Optical Flow#xff09;是指通过分析连续图像帧来估计场景中像素或特征点的运动的技…本文总结视觉SLAM中常用的光流法与直接法
1、Lucas-Kanade光流法
相机所拍摄到的图像随相机视角的变化而变化这种变化也可以理解为图像中像素的反向移动。“光流”Optical Flow是指通过分析连续图像帧来估计场景中像素或特征点的运动的技术即根据连续的两张图片和已知某个固定的空间点在 t t t时刻对应的的像素坐标 q \mathbf{q} q估计其他时刻该空间点对应的像素坐标 p \mathbf{p} p光流法常用算法为LK光流法 LK光流法常用算法为常用的光流法在LK光流法中认为图像中每个像素坐标 [ u , v ] T [u,v]^{T} [u,v]T处的灰度都是随时间 t t t变化的函数且做如下两条假设
灰度不变假设同一空间点对应的像素坐标的灰度值在各个图像中是不变的局部运动一致假设相邻区域内的像素具有相同的运动
1.1、解析解法
设对应于同一空间点的像素随时间变化的函数为 ( u ( t ) , v ( t ) ) (u(t),v(t)) (u(t),v(t))根据灰度不变假设存在固定灰度值 C C C有 I ( u ( t ) , v ( t ) , t ) C (1) I(u(t),v(t),t)C\tag{1} I(u(t),v(t),t)C(1) 在上式中对 t t t求导得到 ∂ I ∂ u ∂ u ∂ t ∂ I ∂ v ∂ v ∂ t ∂ I ∂ t 0 (2) \frac{\partial{I}}{\partial{u}}\frac{\partial{u}}{\partial{t}}\frac{\partial{I}}{\partial{v}}\frac{\partial{v}}{\partial{t}}\frac{\partial{I}}{\partial{t}}0\tag{2} ∂u∂I∂t∂u∂v∂I∂t∂v∂t∂I0(2) ∇ t u ∂ u ∂ t , ∇ t v ∂ v ∂ t \nabla_{t}u\frac{\partial{u}}{\partial{t}},\nabla_{t}v\frac{\partial{v}}{\partial{t}} ∇tu∂t∂u,∇tv∂t∂v为 x x x轴 y y y轴方向上的像素移动速度这两个量也是LK光流法的求解目标 ∇ u I ∂ I ∂ u , ∇ v I ∂ I ∂ v \nabla_{u}I\frac{\partial{I}}{\partial{u}},\nabla_{v}I\frac{\partial{I}}{\partial{v}} ∇uI∂u∂I,∇vI∂v∂I为灰度在 x , y x,y x,y方向上的梯度也可称为像素梯度 ∇ t I ∂ I ∂ t \nabla_{t}I\frac{\partial{I}}{\partial{t}} ∇tI∂t∂I为固定点处灰度对时间的导数 ( 2 ) (2) (2)可以化简为 [ ∇ u I , ∇ v I ] [ ∇ t u ∇ t v ] − ∇ t I (3) [\nabla_{u}I,\nabla_{v}I]\begin{bmatrix}\nabla_{t}u\\\nabla_{t}v\end{bmatrix}-\nabla_{t}I\tag{3} [∇uI,∇vI][∇tu∇tv]−∇tI(3) 令 w [ ∇ t u ∇ t v ] \mathbf{w}\begin{bmatrix}\nabla_{t}u\\\nabla_{t}v\end{bmatrix} w[∇tu∇tv]上式是一个二元一次方程仅靠该方程无法计算 w \mathbf{w} w还需引入其他约束。
根据局部运动一致假设可以认为像素 q i \mathbf{q}_{i} qi附近的某邻域内全部像素 q j , j 1 , ⋯ , w \mathbf{q}_{j},j1,\cdots,w qj,j1,⋯,w再 Δ t \Delta{t} Δt时间段内具有相同的运动因此 ( 3 ) (3) (3)可以写成 [ ∇ u I 1 ( q 1 ) , ∇ v I 1 ( q 1 ) ⋮ ∇ u I 1 ( q w ) , ∇ v I 1 ( q w ) ] w [ − ∇ t I ( q 1 ) ⋮ − ∇ t I ( q w ) ] (4) \begin{bmatrix}\nabla_{u} I_{1}(\mathbf{q}_{1}),\nabla_{v} I_{1}(\mathbf{q}_{1})\\\vdots\\ \nabla_{u} I_{1}(\mathbf{q}_{w}),\nabla_{v} I_{1}(\mathbf{q}_{w})\end{bmatrix}\mathbf{w}\begin{bmatrix}-\nabla_{t}I(\mathbf{q}_{1})\\\vdots\\-\nabla_{t}I(\mathbf{q}_{w})\end{bmatrix}\tag{4} ⎣⎢⎡∇uI1(q1),∇vI1(q1)⋮∇uI1(qw),∇vI1(qw)⎦⎥⎤w⎣⎢⎡−∇tI(q1)⋮−∇tI(qw)⎦⎥⎤(4) 其中 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \nabla_{u} I_{… 记 ( 4 ) (4) (4)中系数矩阵为 A \mathbf{A} A等号右侧矩阵为 b \mathbf{b} b则方程变为 A w b \mathbf{A}\mathbf{w}\mathbf{b} Awb 上式是关于 w \mathbf{w} w的超定方程组可以通过最小二乘的方式求解即令 w ∗ arg min w ∥ A w − b ∥ 2 (6) \mathbf{w}^{\ast}\underset{\mathbf{w}}{\arg\min}\,\|\mathbf{A}\mathbf{w}-\mathbf{b}\|^{2}\tag{6} w∗wargmin∥Aw−b∥2(6) 根据§1容易求出 w ∗ \mathbf{w}^{\ast} w∗根据 q i w ∗ Δ t \mathbf{q}_{i}\mathbf{w}^{\ast}\Delta{t} qiw∗Δt即可计算新像素位置
1.2、优化解法
通过最小化两张图像对应像素邻域内的灰度差也可以求出给定点 q \mathbf{q} q在第二张图像中的对应像素 p \mathbf{p} p即 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{p}^{\a… e j \mathbf{e}_{j} ej对 p \mathbf{p} p的雅可比矩阵为 J j ∂ e j ∂ p [ − ∇ u I 2 ( p j ) − ∇ v I 2 ( p j ) ] (8) \mathbf{J}_{j}\frac{\partial\mathbf{e}_{j}}{\partial\mathbf{p}}\begin{bmatrix}-\nabla_{u}I_{2}(\mathbf{p}_{j})\\ -\nabla_{v}I_{2}(\mathbf{p}_{j})\end{bmatrix}\tag{8} Jj∂p∂ej[−∇uI2(pj)−∇vI2(pj)](8) 再求出 H k ∑ j 1 w J j J j T b k ∑ j 1 w J j T e j \mathbf{H}_{k}\sum_{j1}^{w}\mathbf{J}_{j}\mathbf{J}_{j}^{T}\quad\quad \mathbf{b}_{k}\sum_{j1}^{w}\mathbf{J}^{T}_{j}\mathbf{e}_{j} Hkj1∑wJjJjTbkj1∑wJjTej 增量方程为如下式可以通过增量方程计算更新量 H k Δ p k − b k \mathbf{H}_{k}\Delta\mathbf{p}_{k}-\mathbf{b}_{k} HkΔpk−bk 得到更新量后第二张图片中像素坐标可以更新为 p k 1 p k Δ p k \mathbf{p}_{k1}\mathbf{p}_{k}\Delta\mathbf{p}_{k} pk1pkΔpk
2、直接法 直接法并不单独估计第二张图片中的像素点位置而是对第一张图片中的像素点根据相机位姿估计值寻找其在第二张图片中对应的像素位置并通过图片中对应像素的灰度差不断优化相机位姿变换得到最优位姿变换同时使两张图片的灰度差最小。下面进行详细说明。
已知像素 q i , i 1 , ⋯ , n \mathbf{q}_{i},i1,\cdots,n qi,i1,⋯,n和其对应的深度及摄像机内参矩阵 K [ f x 0 c x 0 f y c y 0 0 1 ] \mathbf{K}\left[\begin{array}{ccc} f_{x}0c_{x}\\ 0f_{y}c_{y}\\ 001 \end{array}\right] K⎣⎡fx000fy0cxcy1⎦⎤ 可以还原出三维空间位置 x i \mathbf{x}_{i} xi令 X i [ x i 1 ] ∈ R 4 \mathbf{X}_{i}\begin{bmatrix}\mathbf{x}_{i}\\1\end{bmatrix}\in\mathbb{R}^{4} Xi[xi1]∈R4并记从第一张图片到第二张图片对应的相机位姿变换为 T ∈ S E ( 3 ) \mathbf{T}\in SE(3) T∈SE(3)则 x i \mathbf{x}_{i} xi在第二个相机坐标系下的空间坐标为 y i ( T X i ) 1 : 3 [ X , Y , Z ] T \mathbf{y}_{i}(\mathbf{T}\mathbf{X}_{i})_{1:3}[X,Y,Z]^{T} yi(TXi)1:3[X,Y,Z]T 对应的像素坐标为 p i 1 Z ( K y i ) 1 : 2 \mathbf{p}_{i}\frac{1}{Z}(\mathbf{K}\mathbf{y}_{i})_{1:2} piZ1(Kyi)1:2 直接法求解优化问题 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{T}^{\a… 暂时省略下标根据链式求导法则得到 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \frac{\partial… 容易得到 ∂ p ∂ y [ f x Z 0 − f x X Z 2 0 f y Z − f x Y Z 2 ] ∂ y ∂ T [ I , − y ∧ ] \frac{\partial{\mathbf{p}}}{\partial\mathbf{y}}\begin{bmatrix} \frac{f_{x}}{Z}0-\frac{f_{x}X}{Z^{2}}\\ 0\frac{f_{y}}{Z}-\frac{f_{x}Y}{Z^{2}} \end{bmatrix}\quad\quad\frac{\partial\mathbf{y}}{\partial\mathbf{T}}[\mathbf{I},-\mathbf{y}^{\wedge}] ∂y∂p[Zfx00Zfy−Z2fxX−Z2fxY]∂T∂y[I,−y∧] 因此 ( 10 ) (10) (10)后两项可以写成 ∂ p ∂ T ∂ p ∂ y ∂ y ∂ T [ f x Z 0 − f x X Z 2 − f x X Y Z 2 f x f x X 2 Z 2 − f x Y Z 0 − f y Z − f x Y Z 2 − f y − f y Y 2 Z 2 f x X Y Z 2 f x X Z ] (11) \frac{\partial\mathbf{p}}{\partial\mathbf{T}}\frac{\partial\mathbf{p}}{\partial\mathbf{y}}\frac{\partial\mathbf{y}}{\partial\mathbf{T}}\begin{bmatrix} \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_{x}Y}{Z^{2}}-f_{y}-\frac{f_{y}Y^{2}}{Z^{2}}\frac{f_{x}XY}{Z^{2}}\frac{f_{x}X}{Z} \end{bmatrix}\tag{11} ∂T∂p∂y∂p∂T∂y[Zfx00−Zfy−Z2fxX−Z2fxY−Z2fxXY−fy−Z2fyY2fxZ2fxX2Z2fxXY−ZfxYZfxX](11) 故 ( 10 ) (10) (10)又可以写成 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \frac{\partial… 问题 ( 9 ) (9) (9)的雅可比矩阵为 J i ∂ e i ∂ T \mathbf{J}_{i}\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{T}} Ji∂T∂ei 由此得到 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲\mathbf{H}_{k}… 则更新量可以通过下式计算 H k Δ T k − b k \mathbf{H}_{k}\Delta\mathbf{T}_{k}-\mathbf{b}_{k} HkΔTk−bk
并通过下式更新 T k 1 E x p ( Δ T k ) T k \mathbf{T}_{k1}\mathrm{Exp}(\Delta\mathbf{T}_{k})\mathbf{T}_{k} Tk1Exp(ΔTk)Tk 最终得到最优的位姿变换
实验
直接法在kitti数据集上的效果如下图可以看到追踪效果良好
附录
§1、标准最小二乘问题
标准最小二乘问题对给定 A ∈ R M × N \mathbf{A}\in\mathbb{R}^{M\times{N}} A∈RM×N计算 x ∗ ∈ R N \mathbf{x}^{\ast}\in\mathbb{R}^{N} x∗∈RN使得 KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{x}^{\a… 首先对 A \mathbf{A} A进行SVD分解 A U [ Σ r × r O O O ] V T \mathbf{A}\mathbf{U} \begin{bmatrix} \boldsymbol\Sigma_{r\times{r}}\mathbf{O}\\ \mathbf{O}\mathbf{O} \end{bmatrix}\mathbf{V}^{T} AU[Σr×rOOO]VT 则 A \mathbf{A} A的伪逆为 A † V [ Σ r × r − 1 O O O ] U T (A2) \mathbf{A}^{\dagger}\mathbf{V} \begin{bmatrix} \boldsymbol\Sigma_{r\times{r}}^{-1}\mathbf{O}\\ \mathbf{O}\mathbf{O} \end{bmatrix}\mathbf{U}^{T}\tag{A2} A†V[Σr×r−1OOO]UT(A2) 可以证明满足 ( A 1 ) \mathrm{(A1)} (A1)的模长最小的解为 x ∗ A † b (A3) \mathbf{x}^{\ast}\mathbf{A}^{\dagger}\mathbf{b}\tag{A3} x∗A†b(A3) 特别地当 r a n k ( A ) N \mathrm{rank}(\mathbf{A})N rank(A)N时 A † ( A T A ) − 1 A \mathbf{A}^{\dagger}(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A} A†(ATA)−1A ( A 1 ) \mathrm{(A1)} (A1)仅有如下一个解 x ∗ ( A T A ) − 1 A b (A4) \mathbf{x}^{\ast}(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A}\mathbf{b}\tag{A4} x∗(ATA)−1Ab(A4)
