网站制作公司咨询,网站首页index.php全屏展示代码怎么弄,品牌运营,东营建设企业网站引入
在前面的文章#xff1a;运动跟踪——Lucas-Kanade光流中#xff0c;我们了解到特征点法存在一些缺陷#xff0c;并且用光流法追踪像素点的运动来替代特征点法进行特征点匹配的过程来解决这些缺陷。而这篇文章要介绍的直接法则是通过计算特征点在下一时刻图像中的位置…引入
在前面的文章运动跟踪——Lucas-Kanade光流中我们了解到特征点法存在一些缺陷并且用光流法追踪像素点的运动来替代特征点法进行特征点匹配的过程来解决这些缺陷。而这篇文章要介绍的直接法则是通过计算特征点在下一时刻图像中的位置来进一步缩短算法计算时间。
直接法估计相机位姿的推导 直接法通过最小化相邻图像之间的光度误差来优化位姿参数。以下是推导过程 1. 问题建模
设参考帧图像为 I ref I_{\text{ref}} Iref当前帧图像为 I cur I_{\text{cur}} Icur相机的位姿变换为 T ∈ SE ( 3 ) T \in \text{SE}(3) T∈SE(3)用李代数 ξ ∈ s e ( 3 ) \xi \in \mathfrak{se}(3) ξ∈se(3)参数化。对于参考帧中的每个像素点 p i \mathbf{p}_i pi其对应的3D点坐标为 P i \mathbf{P}_i Pi投影到当前帧中的坐标为 p i ′ π ( T ⋅ P i ) \mathbf{p}_i \pi\left(T \cdot \mathbf{P}_i\right) pi′π(T⋅Pi) 其中 π \pi π为投影函数 T exp ( ξ ∧ ) T \exp(\xi^\wedge) Texp(ξ∧)。
光度误差定义为 e i I cur ( p i ′ ) − I ref ( p i ) e_i I_{\text{cur}}(\mathbf{p}_i) - I_{\text{ref}}(\mathbf{p}_i) eiIcur(pi′)−Iref(pi) 则优化目标是最小化总误差 min ξ ∑ i ∥ e i ∥ 2 \min_\xi \sum_i \| e_i \|^2 ξmini∑∥ei∥2 2. 误差函数的导数
误差关于位姿的雅可比矩阵用链式求导为 ∂ e i ∂ ξ ∂ I cur ∂ p i ′ ⋅ ∂ p i ′ ∂ ξ \frac{\partial e_i}{\partial \xi} \frac{\partial I_{\text{cur}}}{\partial \mathbf{p}_i} \cdot \frac{\partial \mathbf{p}_i}{\partial \xi} ∂ξ∂ei∂pi′∂Icur⋅∂ξ∂pi′
(1) 图像梯度项 ∂ I cur ∂ p i ′ [ I x , I y ] \frac{\partial I_{\text{cur}}}{\partial \mathbf{p}_i} \left[ I_x, I_y \right] ∂pi′∂Icur[Ix,Iy] 其中 I x , I y I_x, I_y Ix,Iy 为当前帧在 p i ′ \mathbf{p}_i pi′处的图像梯度。
(2) 投影点对位姿的导数
投影函数 p i ′ π ( exp ( ξ ∧ ) P i ) \mathbf{p}_i \pi(\exp(\xi^\wedge) \mathbf{P}_i) pi′π(exp(ξ∧)Pi)的导数为 ∂ p i ′ ∂ ξ ∂ π ∂ P ′ ⋅ ∂ P ′ ∂ ξ \frac{\partial \mathbf{p}_i}{\partial \xi} \frac{\partial \pi}{\partial \mathbf{P}} \cdot \frac{\partial \mathbf{P}}{\partial \xi} ∂ξ∂pi′∂P′∂π⋅∂ξ∂P′ 其中 P ′ T ⋅ P i \mathbf{P} T \cdot \mathbf{P}_i P′T⋅Pi为变换后的3D点。 ∂ π ∂ P ′ \frac{\partial \pi}{\partial \mathbf{P}} ∂P′∂π为投影函数对3D点的导数 ∂ π ∂ P ′ [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] \frac{\partial \pi}{\partial \mathbf{P}} \begin{bmatrix} \frac{f_x}{Z} 0 -\frac{f_x X}{Z^2} \\ 0 \frac{f_y}{Z} -\frac{f_y Y}{Z^2} \end{bmatrix} ∂P′∂π[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′] ∂ P ′ ∂ ξ \frac{\partial \mathbf{P}}{\partial \xi} ∂ξ∂P′为李代数扰动模型 ∂ P ′ ∂ ξ [ I 3 × 3 − P ′ ∧ ] \frac{\partial \mathbf{P}}{\partial \xi} \begin{bmatrix} \mathbf{I}_{3\times3} -\mathbf{P}^{\wedge} \end{bmatrix} ∂ξ∂P′[I3×3−P′∧] 其中 P ′ ∧ \mathbf{P}^{\wedge} P′∧为3D点的反对称矩阵。 3. 雅可比矩阵推导
结合上述结果雅可比矩阵为 J i [ f x Z ′ 0 − f x X ′ Z ′ 2 f x X ′ Y ′ Z ′ 2 − f x ( 1 X ′ 2 Z ′ 2 ) f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y ( 1 Y ′ 2 Z ′ 2 ) f y X ′ Y ′ Z ′ 2 − f y X ′ Z ′ ] \mathbf{J}_i \begin{bmatrix} \frac{f_x}{Z} 0 -\frac{f_x X}{Z^2} \frac{f_x X Y}{Z^2} -f_x\left(1 \frac{X^2}{Z^2}\right) \frac{f_x Y}{Z} \\ 0 \frac{f_y}{Z} -\frac{f_y Y}{Z^2} -f_y\left(1 \frac{Y^2}{Z^2}\right) \frac{f_y X Y}{Z^2} -\frac{f_y X}{Z} \end{bmatrix} Ji Z′fx00Z′fy−Z′2fxX′−Z′2fyY′Z′2fxX′Y′−fy(1Z′2Y′2)−fx(1Z′2X′2)Z′2fyX′Y′Z′fxY′−Z′fyX′ 4. 优化求解
使用高斯-牛顿法迭代更新位姿
计算残差对每个点计算 e i I cur ( p i ′ ) − I ref ( p i ) e_i I_{\text{cur}}(\mathbf{p}_i) - I_{\text{ref}}(\mathbf{p}_i) eiIcur(pi′)−Iref(pi)。构建雅可比矩阵按上述公式计算每个点的 J i \mathbf{J}_i Ji。求解增量方程 H ∑ i J i T J i , b − ∑ i J i T e i \mathbf{H} \sum_i \mathbf{J}_i^T \mathbf{J}_i, \quad \mathbf{b} -\sum_i \mathbf{J}_i^T e_i Hi∑JiTJi,b−i∑JiTei Δ ξ H − 1 b \Delta \xi \mathbf{H}^{-1} \mathbf{b} ΔξH−1b更新位姿 ξ ← ξ Δ ξ \xi \leftarrow \xi \Delta \xi ξ←ξΔξ 5. 关键点总结
光度误差基于灰度不变假设直接比较像素亮度。李代数扰动模型用于计算位姿变化对投影点的影响。雅可比矩阵结合图像梯度和几何变换导数指导优化方向。多尺度优化还可以采用图像金字塔提高鲁棒性和收敛性。
通过迭代优化直接法能够有效估计相机的位姿变化适用于特征缺失的场景但对光照变化和初始值敏感。 至此关于视觉里程计即根据图像信息求解相机位姿变换的知识已经讲解完毕。接下来将进入后端优化的学习。
