直接法估计相机位姿

引入

在前面的文章：运动跟踪——Lucas-Kanade光流中，我们了解到特征点法存在一些缺陷，并且用光流法追踪像素点的运动来替代特征点法进行特征点匹配的过程来解决这些缺陷。而这篇文章要介绍的直接法则是通过计算特征点在下一时刻图像中的位置来进一步缩短算法计算时间。

直接法估计相机位姿的推导

直接法通过最小化相邻图像之间的光度误差来优化位姿参数。以下是推导过程：

---

1. 问题建模

设参考帧图像为$I_{\text{ref}}$，当前帧图像为$I_{\text{cur}}$，相机的位姿变换为$T\in \text{SE}(3)$，用李代数$\xi \in \mathfrak{se}(3)$参数化。对于参考帧中的每个像素点${\mathbf{p}}_i$，其对应的3D点坐标为${\mathbf{P}}_i$，投影到当前帧中的坐标为：
$${\mathbf{p}}_i^{\prime }=\pi \left(T\cdot {\mathbf{P}}_i\right)$$
其中$\pi$为投影函数，$T=\exp ({\xi }^{\wedge })$。

光度误差定义为：
$$e_i=I_{\text{cur}}({\mathbf{p}}_i^{\prime })-I_{\text{ref}}({\mathbf{p}}_i)$$
则优化目标是最小化总误差：
$$\underset{\xi }{\min }\sum _i\Vert e_i{\Vert }^2$$

---

2. 误差函数的导数

误差关于位姿的雅可比矩阵用链式求导为：
$$\frac{\partial e_i}{\partial \xi }=\frac{\partial I_{\text{cur}}}{\partial {\mathbf{p}}_i^{\prime }}\cdot \frac{\partial {\mathbf{p}}_i^{\prime }}{\partial \xi }$$

(1) 图像梯度项

$$\frac{\partial I_{\text{cur}}}{\partial {\mathbf{p}}_i^{\prime }}=\left[I_x,I_y\right]$$
其中$I_x,I_y$ 为当前帧在${\mathbf{p}}_i^{\prime }$处的图像梯度。

(2) 投影点对位姿的导数

投影函数${\mathbf{p}}_i^{\prime }=\pi (\exp ({\xi }^{\wedge }){\mathbf{P}}_i)$的导数为：
$$\frac{\partial {\mathbf{p}}_i^{\prime }}{\partial \xi }=\frac{\partial \pi }{\partial {\mathbf{P}}^{\prime }}\cdot \frac{\partial {\mathbf{P}}^{\prime }}{\partial \xi }$$
其中：

• ${\mathbf{P}}^{\prime }=T\cdot {\mathbf{P}}_i$为变换后的3D点。
• $\frac{\partial \pi }{\partial {\mathbf{P}}^{\prime }}$为投影函数对3D点的导数：
  $$\frac{\partial \pi }{\partial {\mathbf{P}}^{\prime }}=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$
• $\frac{\partial {\mathbf{P}}^{\prime }}{\partial \xi }$为李代数扰动模型：
  $$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \xi }=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -{\mathbf{P}}^{\prime \wedge }\end{array}\right]$$
  其中${\mathbf{P}}^{\prime \wedge }$为3D点的反对称矩阵。

---

3. 雅可比矩阵推导

结合上述结果，雅可比矩阵为：
$${\mathbf{J}}_i=\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& \frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& -f_x\left(1+\frac{X^{\prime 2}}{Z^{\prime 2}}\right)& \frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y\left(1+\frac{Y^{\prime 2}}{Z^{\prime 2}}\right)& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& -\frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

---

4. 优化求解

使用高斯-牛顿法迭代更新位姿：

1. 计算残差：对每个点计算$e_i=I_{\text{cur}}({\mathbf{p}}_i^{\prime })-I_{\text{ref}}({\mathbf{p}}_i)$。
2. 构建雅可比矩阵：按上述公式计算每个点的${\mathbf{J}}_i$。
3. 求解增量方程：
   $$\mathbf{H}=\sum _i{\mathbf{J}}_i^T{\mathbf{J}}_i,\mathbf{b}=-\sum _i{\mathbf{J}}_i^T{e}_i$$
   $$\Delta \xi ={\mathbf{H}}^{-1}\mathbf{b}$$
4. 更新位姿：
   $$\xi \leftarrow \xi +\Delta \xi$$

---

5. 关键点总结

• 光度误差：基于灰度不变假设，直接比较像素亮度。
• 李代数扰动模型：用于计算位姿变化对投影点的影响。
• 雅可比矩阵：结合图像梯度和几何变换导数，指导优化方向。
• 多尺度优化：还可以采用图像金字塔提高鲁棒性和收敛性。

通过迭代优化，直接法能够有效估计相机的位姿变化，适用于特征缺失的场景，但对光照变化和初始值敏感。
至此，关于视觉里程计即根据图像信息求解相机位姿变换的知识已经讲解完毕。接下来将进入后端优化的学习。