VSLAM学习(一) 三维运动、相机模型、SLAM模型

目录
VSLAM学习(一) 三维运动、相机模型、SLAM模型
VSLAM学习(二) 非线性优化
VSLAM学习(三) 单目相机位姿估计
VSLAM学习(四) Bundle Adjustment

一、三维空间刚体运动

1.1 旋转矩阵

同一向量在不同坐标系下的坐标不同
假如有两组坐标系，他们的基分别为$({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)$和$({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })$，向量$\mathbf{a}$在这两组坐标系下的坐标分别为$(a_1,a_2,a_3{)}^{\mathrm{T}}$和$(a_1^{\prime },a_2^{\prime },a_3^{\prime }{)}^{\mathrm{T}}$，那么他们满足

$$({\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3)\left(\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right)=({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })\left(\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right)$$

即

$$\begin{array}{rl}\mathbf{a}=\left(\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right)& =\left(\begin{array}{c}{\mathbf{e}}_1^{\mathrm{T}}\\ {\mathbf{e}}_2^{\mathrm{T}}\\ {\mathbf{e}}_3^{\mathrm{T}}\end{array}\right)({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime })\left(\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right)\\ & =\left(\begin{array}{ccc}{\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^{\mathrm{T}}{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^{\mathrm{T}}{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^{\mathrm{T}}{\mathbf{e}}_3^{\prime }\end{array}\right){\mathbf{a}}^{\prime }\stackrel{\mathrm{def}}{=}{\mathbf{Ra}}^{\prime }\end{array}$$

此处的$\mathbf{R}$即为${\mathbf{a}}^{\prime }$坐标系到$\mathbf{a}$坐标系的旋转矩阵
可以看出$\mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I}$且$\det (\mathbf{R})=1$

1.2 空间坐标变换

三维空间的刚体运动是由旋转与平移组成的，可以表示为

$$\mathbf{b}={\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1$$

其中${\mathbf{R}}_1\in {\mathbb{R}}^{3\times 3}$是旋转矩阵，${\mathbf{t}}_1\in {\mathbb{R}}^3$为平移量
但是这种表示方法在多坐标系转换时会很复杂：

$$\begin{gathered} \mathbf{b}={\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1,\mathbf{c}={\mathbf{R}}_2\mathbf{b}+{\mathbf{t}}_2 \\ \Rightarrow \mathbf{c}={\mathbf{R}}_2({\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1)+{\mathbf{t}}_2 \end{gathered}$$

因此为了避免麻烦，可引入齐次坐标和变换矩阵

$$\begin{gathered} \mathbf{b}={\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1 \\ \Rightarrow \left(\begin{array}{c}\mathbf{b}\\ 1\end{array}\right)=\left(\begin{array}{cc}{\mathbf{R}}_1& {\mathbf{t}}_1\\ 0^{\mathrm{T}}& 1\end{array}\right)\left(\begin{array}{c}\mathbf{a}\\ 1\end{array}\right)\stackrel{\mathrm{def}}{=}{\mathbf{T}}_1\left(\begin{array}{c}\mathbf{a}\\ 1\end{array}\right) \\ \Rightarrow \tilde{\mathbf{b}}={\mathbf{T}}_1\tilde{\mathbf{a}} \end{gathered}$$

那么连续转换就可以表示成

$$\tilde{\mathbf{c}}={\mathbf{T}}_2\tilde{\mathbf{b}}={\mathbf{T}}_2{\mathbf{T}}_1\tilde{\mathbf{a}}$$

1.3 旋转向量 与 罗德里格斯公式

除了可以用旋转矩阵$\mathbf{R}$来表示一个旋转以外
还可以用一个旋转轴和一个旋转角来表示一个旋转

旋转轴、角与旋转向量的关系可以由罗德里格斯公式(Rodrigues’s Formula)来表达：

$$\begin{gathered} \mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge } \\ or \\ \mathbf{R}=\mathbf{I}+(1-\cos \theta )({\mathbf{n}}^{\wedge }{)}^2+\sin \theta {\mathbf{n}}^{\wedge } \end{gathered}$$

其中$\theta$表示旋转角，$\mathbf{n}$表示旋转轴单位向量，${\mathbf{n}}^{\wedge }$表示向量$\mathbf{n}$的反对称矩阵(skew-symmetric matrix)或叉积矩阵
也有很多其他地方将反对称矩阵记为${\mathbf{n}}_{\times }$

• 补充知识点：叉积矩阵
  $$\begin{array}{rl}\mathbf{a}\times \mathbf{b}& =\left(\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right)=\left(\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right)\\ & =\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right)\mathbf{b}\\ & \stackrel{\mathrm{def}}{=}{\mathbf{a}}^{\wedge }\cdot \mathbf{b}\end{array}$$
    这里$\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right)$称为向量 $\mathbf{a}$ 的叉积矩阵，或者反对称矩阵，记为 ${\mathbf{a}}^{\wedge }$ 或者 ${\mathbf{a}}_{\times }$
    
    同时我们可以看出叉积矩阵的性质：$({\mathbf{a}}^{\wedge }{)}^2=\mathbf{a}{\mathbf{a}}^T-\mathbf{I},({\mathbf{a}}^{\wedge }{)}^3=-{\mathbf{a}}^{\wedge }$

由此可见，使用向量$\mathbf{\varphi }=\theta \mathbf{n}$同样可以描述一个三维旋转$\mathbf{R}$
$\mathbf{\varphi }$ 与 $\mathbf{R}$ 的具体关系可以学习李代数与李群

特殊地，
当 $\theta$ 很小的时候，$\sin \theta \sim \theta ,1-\cos \theta \sim o(\theta )$
那么罗德里格斯公式可以表示为：
$$\mathbf{R}\approx \mathbf{I}+\theta {\mathbf{n}}^{\wedge }=\left(\begin{array}{ccc}1& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 1& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 1\end{array}\right)$$

二、相机模型

2.1 针孔相机模型

针孔相机的成像原理就是小孔成像

(图片制作中…)

为了计算方便，一般把成像平面翻转到物体同一侧

$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$

我们将成像的像素坐标记为$(x,y{)}^T$
图片中心的像素坐标记为$(c_x,c_y{)}^T$
成像图片的像素与实际世界长度的比例记为横轴比例：$\alpha$，纵轴比例：$\beta$
那么有

$$\{\begin{array}{l}x=\alpha X^{\prime }+c_x\\ y=\beta Y^{\prime }+c_y\end{array}$$

整合之前关系式，记$f_x=\alpha f,f_y=\beta f$

$$\{\begin{array}{l}x=f_x\frac{X}{Z}+c_x\\ y=f_y\frac{Y}{Z}+c_y\end{array}$$

整理为齐次坐标

$$\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)$$

矩阵$\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)$叫做相机内参矩阵，暂记为$\mathbf{K}$，即

$$Z{\mathbf{p}}_{pixel}=\mathbf{K}{\mathbf{p}}_{camera}=\mathbf{K}(\mathbf{R}{\mathbf{p}}_{world}+\mathbf{t})=\mathbf{K}{\mathbf{T}}_{cw}{\mathbf{p}}_{world}$$

(第一步和最后一步都隐含了齐次转换，等号并不严谨，理解意思即可)

2.2 图像畸变

图像畸变一般分为径向畸变和切向畸变

2.2.1 径向畸变

我们使用${\mathbf{p}}^{\prime }=(x^{\prime },y^{\prime }{)}^{\mathrm{T}}$表示发生畸变以后图片上的坐标
用$\mathbf{p}=(x,y{)}^{\mathrm{T}}$表示真实应该在的图片上的位置坐标
则

$$\{\begin{array}{rl}x& =x^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6+\dots )\\ y& =y^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6+\dots )\end{array}$$

其中$r=\Vert {\mathbf{p}}^{\prime }{\Vert }_2=\sqrt{x^{\prime 2}+y^{\prime 2}}$
$k$为参数，畸变越大的相机需要的参数越多。

2.2.2 切向畸变

沿用上面的符号
$$\{\begin{array}{rl}x& =x^{\prime }+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})\\ y& =y^{\prime }+2p_2{x}^{\prime }{y}^{\prime }+p_1(r^2+2y^{\prime 2})\end{array}$$
$p_1,p_2$为参数

2.2.3 混合畸变

其实就是把两种畸变加起来即可
$$\{\begin{array}{rl}x& =x^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6+\dots )+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})\\ y& =y^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6+\dots )+2p_2{x}^{\prime }{y}^{\prime }+p_1(r^2+2y^{\prime 2})\end{array}$$

三、SLAM模型

3.1 运动观测方程

${\mathbf{x}}_k$是指$k$时刻的机器人位姿（VSLAM中一般是相机的位姿）
${\mathbf{z}}_{k,j}$是指$k$时刻对第$j$个路标点的观测值（VSLAM中一般是特征点在照片上的像素点坐标）

$$\{\begin{array}{rlr}{\mathbf{x}}_k& =f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k& \cdot \cdot \cdot 运动方程\\ {\mathbf{z}}_{k,j}& =h({\mathbf{y}}_j,{\mathbf{x}}_k)+{\mathbf{v}}_{k,j}& \cdot \cdot \cdot 观测方程\end{array}$$
其中，${\mathbf{u}}_k$是指$k$时刻的运动传感器的读数或者输入
　　　${\mathbf{w}}_k$是指$k$时刻的运动噪声
　　　${\mathbf{y}}_j$表示第$j$个路标点
　　　${\mathbf{v}}_{k,j}$表示$k$时刻第$j$个路标点的观测噪声
　　　$f(\cdot )$是运动函数
　　　$h(\cdot )$是观测函数

3.2 投影模型

观测函数$h(\cdot )$是一个物体从世界到相机图片的投影过程，该过程分为以下几步：

我们先记某一物体在世界坐标系下的坐标为${\mathbf{P}}_w$，现在我们要将其投影到相机图片中的像素位置。
再记该物体在相机坐标系下的坐标和归一化坐标分别为${\mathbf{P}}_c^{\prime }$和${\mathbf{P}}_c$

根据三维空间旋转，我们知道

$${\mathbf{P}}_c^{\prime }={\mathbf{R}}_{wc}{\mathbf{P}}_w+\mathbf{t}=\left(\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right)$$

对相机坐标归一化

$${\mathbf{P}}_c=\left(\begin{array}{c}{u}_c\\ v_c\\ 1\end{array}\right)=\left(\begin{array}{c}{X}_c/Z_c\\ Y_c/Z_c\\ 1\end{array}\right)$$

在对相机的畸变进行处理（这里以镜像畸变为例）

$$\{\begin{array}{r}{u}_c^{\prime }=u_c(1+k_1{r}_c^2+k_2{r}_c^4)\\ v_c^{\prime }=v_c(1+k_1{r}_c^2+k_2{r}_c^4)\end{array}$$

最后在根据相机内参算出函数$h(\cdot )$最终结果（齐次变换最后一行的1我省去了）：
$$\left(\begin{array}{c}u\\ v\end{array}\right)=\mathbf{K}\left(\begin{array}{c}{u}_c^{\prime }\\ v_c^{\prime }\end{array}\right)=\left(\begin{array}{c}{f}_x{u}_c^{\prime }+c_x\\ f_y{v}_c^{\prime }+c_y\end{array}\right)$$

3.3 优化

我们所使用的设备和所在的环境都是存在噪声的，所以为导致仪器的测量数据不准确，需要使用算法进行优化。

一般的优化方法有两种：

• 滤波器
• 非线性优化

若${\mathbf{w}}_k$与${\mathbf{v}}_{k,j}$都是高斯白噪声，且观测方程$\mathbf{h}(\cdot )$是线性的，那么可以直接使用滤波器来进行优化。
现在平面SLAM很多使用的还是滤波器的原理，例如Gmapping

而三维的视觉SLAM，一般则需要使用非线性优化。