Visual SLAM的基本概念和公式-CSDN博客

本文链接：https://blog.csdn.net/wubobupt2/article/details/127804911

本文详细介绍了在三维空间中，如何通过SE(3)和se(3)模型计算相机位姿变化对特征点重投影误差的影响，包括坐标转换和求导公式，对于视觉定位和计算机视觉理解关键。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

pose： $\mathbf{x} \in se(3)$ ，未知量（ $se (3)$ 的解释见 附录A）
landmark/reference point： $\mathbf{y} \in R^{3 \times 1}$ ，未知量
reprojection function: $\mathbf{p} = \mathbf{p}(\mathbf{x}, \mathbf{y}) \in R^{2 \times 1}$
observation： $\mathbf{z} \in R^{2 \times 1}$ , 已知量
reprojection error function： $\mathbf{f} = \mathbf{z} - \mathbf{p}(\mathbf{x}, \mathbf{y}) \in R^{2 \times 1}$

有如下的求导公式（推导过程见 附录B）：
$\frac{\partial \mathbf{f}}{\partial \mathbf{y}} = \begin{bmatrix} -\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ 0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \cdot \mathbf{R}$

$\frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{f c_1 c_2}{c_3^2} &-f(1 + \frac{c_1^2}{c_3^2}) &\frac{f c_2}{c_3} &-\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ f(1 + \frac{c_2^2}{c_3^2}) &-\frac{f c_1 c_2}{c_3^2} &-\frac{f c_1}{c_3} &0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix}$

其中 $\mathbf{c} = [c_1, c_2, c_3]^\top$ ， $f = f_u = f_v$ 就是等效焦距：
$\begin{bmatrix} \mathbf{c} \\ 1 \\ \end{bmatrix} = \mathbf{T} \begin{bmatrix} \mathbf{y} \\ 1 \\ \end{bmatrix}$

$\mathbf{T} = \begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} &1 \\ \end{bmatrix} = \exp(\mathbf{x}^{\wedge})$

附录

A

$se (3)$ 以及左扰动模型见 “SE(3)和se(3)，左扰动模型” 和 “SO(3)与so(3)、左扰动模型”

B

上述求导公式的推导过程：

这里用了计算图的方式：

$\mathbf{f}=[f_1, f_2]^\top$ , $\mathbf{z}=[z_1, z_2]^\top$ , $\mathbf{p}=[p_1, p_2]^\top$ :
$\mathbf{f} = \mathbf{z} - \mathbf{p}$

$\frac{\partial \mathbf{f}}{\partial \mathbf{p}} = \begin{bmatrix} -1 &0 \\ 0 &-1 \\ \end{bmatrix}$

$\mathbf{c} = [c_1, c_2, c_3]^\top$ :
$\begin{cases} p_1 = f \cdot \frac{c_1}{c_3} + c_x \\ p_2 = f \cdot \frac{c_2}{c_3} + c_y \\ \end{cases}$

$\frac{\partial \mathbf{p}}{\partial \mathbf{c}} = \begin{bmatrix} \frac{f}{c_3} &0 &-\frac{f c_1}{c_3^2} \\ 0 &\frac{f}{c_3} &-\frac{f c_2}{c_3^2} \\ \end{bmatrix}$

$\mathbf{y} = [y_1, y_2, y_3]^\top$ :
$\begin{bmatrix} \mathbf{c} \\ 1 \\ \end{bmatrix} = \mathbf{T} \begin{bmatrix} \mathbf{y} \\ 1 \\ \end{bmatrix}$

$\mathbf{T} = \begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} &1 \\ \end{bmatrix} = \exp(\mathbf{x}^{\wedge})$

$\frac{\partial \mathbf{c}}{\partial \mathbf{y}} = \mathbf{R}$

$\frac{\partial \mathbf{c}}{\partial \mathbf{x}} = [-\mathbf{c}^{\wedge}, \mathbf{I}]$

于是
$\frac{\partial \mathbf{f}}{\partial \mathbf{y}} = \begin{bmatrix} -\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ 0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \cdot \mathbf{R}$

$\begin{aligned} \frac{\partial \mathbf{f}}{\partial \mathbf{x}} &= \begin{bmatrix} -\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ 0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \cdot [-\mathbf{c}^{\wedge}, \mathbf{I}] \\ &= \begin{bmatrix} \frac{f c_1 c_2}{c_3^2} &-f(1 + \frac{c_1^2}{c_3^2}) &\frac{f c_2}{c_3} &-\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ f(1 + \frac{c_2^2}{c_3^2}) &-\frac{f c_1 c_2}{c_3^2} &-\frac{f c_1}{c_3} &0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \end{aligned}$