Visual SLAM的基本概念和公式

小书生大侠客

已于 2024-01-06 22:08:05 修改

阅读量168

点赞数

分类专栏： workspace 文章标签：线性代数算法

于 2022-11-11 14:06:19 首次发布

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

版权

workspace 专栏收录该内容

18 篇文章 0 订阅

订阅专栏

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}$