视觉SLAM学习打卡【4】-相机和图像

• 学习之前内容碰到的一些小demo，感觉很简单，所以没在自己环境下运行。等学完李群李代数（抽象数学，理论推导较多）后，已经出现了eigen库(矩阵计算)、pangolin库(一种可视化工具，类似于ros当中的rviz)、sophus库(eigen升级版，加入李群李代数的计算)等有意思的库，便想跑一跑相关的demo。
• 痛苦因此产生。笔者的linux系统是unbantu20.04.6（双系统），IDE用的是VScode。eigen库还好，虽然遇到了问题，但最终找到解决之法。可怕的是pangolin库，报错层出不穷，从改cmakelist.txt到安装不同版本的pangolin(eg: pangolin-0.5、pangolin-0.6、最新版)，浪费了宝贵的一天(没进展且没relax)，共阅30篇左右解决问题的博客，问题依旧存于人间。
• 但笔者never give up ，若找到解决之法（但愿），后期会专门出一篇 报错-解决方案大全。

一、相机在机器人里充当的角色

1.slam问题的数学表述

(1)运动方程
$${\mathrm{x}}_{\mathrm{k}}=\mathrm{f}({\mathrm{x}}_{k-1},{\mathrm{u}}_{\mathrm{k}},{\mathrm{w}}_{\mathrm{k}})$$

• ${\mathrm{x}}_{\mathrm{k}}$、${\mathrm{x}}_{\mathrm{k}-1}$表示小萝卜在k和k − 1时刻的位置.
• ${\mathrm{u}}_{\mathrm{k}}$表示运动传感器的读数(有时也叫输入).
• ${\mathrm{w}}_{\mathrm{k}}$表示噪声.

(2)观测方程
$${\mathrm{z}}_{\mathrm{k},\mathrm{j}}=\mathrm{h}({\mathrm{y}}_{\mathrm{j}},{\mathrm{x}}_{\mathrm{k}},{\mathrm{v}}_{\mathrm{k},\mathrm{j}})$$

• 表示小萝卜在${\mathrm{x}}_{\mathrm{k}}$位置上看到路标点${\mathrm{y}}_{\mathrm{j}}$产生的观测数据${\mathrm{z}}_{\mathrm{k},\mathrm{j}}$
• ${\mathrm{y}}_{\mathrm{j}}$表示第j个路标点
• $v_{\mathrm{k},\mathrm{j}}$表示噪声

2.观测数据即为相机的成像过程

在以相机为主的视觉SLAM中，观测方程的观测数据${\mathrm{z}}_{\mathrm{k},\mathrm{j}}$指相机的成像过程.

二、针孔相机模型

介绍此模型，共用到五种坐标：世界坐标、相机坐标、归一化坐标、成像坐标和像素坐标。为厘清它们之间的关系，做图如下：

• 空间点P在相机坐标系下的坐标为$[\mathrm{X},\mathrm{Y},\mathrm{Z}{]}^{\mathrm{T}}$，投影到成像坐标系下的坐标为$[{\mathrm{X}}^{\prime },{\mathrm{Y}}^{\prime },{\mathrm{Z}}^{\prime }{]}^{\mathrm{T}}$，由相似关系可得：$$\frac{Z}{f}=-\frac{X}{X^{\prime }}=-\frac{Y}{Y^{\prime }}$$
• 针孔模型投影为倒像，故有负号。实际相机中会通过软件自动翻转，此处可以把负号去掉。
  $$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$整理得：
  $$\{\begin{array}{rl}& \text{X’=f}\frac{\text{X}}{\text{Z}}\\ & \text{Y’=f}\frac{\text{Y}}{\text{Z}}\end{array}$$
• 像素坐标系和成像坐标系之间，相差一个缩放和原点的平移。转换得到的投影点P’在像素坐标系上的像素坐标为${\mathrm{P}}_{\mathrm{u},\mathrm{v}}$
  $${\mathrm{P}}_{\mathrm{u},\mathrm{v}}=[\mathrm{u},\mathrm{v}{]}^{\mathrm{T}}$$$$\{\begin{array}{r}\mathrm{u}=\alpha {\mathrm{X}}^{\prime }+{\mathrm{c}}_{\mathrm{x}}={\mathrm{f}}_{\mathrm{x}}\frac{\mathrm{X}}{\mathrm{Z}}+{\mathrm{c}}_{\mathrm{x}}\\ \mathrm{v}=\beta {\mathrm{Y}}^{\prime }+{\mathrm{c}}_{\mathrm{y}}={\mathrm{f}}_{\mathrm{x}}\frac{\mathrm{X}}{\mathrm{Z}}+{\mathrm{c}}_{\mathrm{x}}\end{array}$$其中$f_x=\alpha f$，$f_y=\beta f$。$\mathrm{u},\mathrm{v},{\mathrm{c}}_{\mathrm{x}},{\mathrm{c}}_{\mathrm{y}},{\mathrm{f}}_{\mathrm{x}},{\mathrm{f}}_{\mathrm{y}}$单位为像素，$\alpha$、 $\beta$的单位为像素/米。
• 把上式写成矩阵表达式$${\mathrm{ZP}}_{\mathrm{uv}}=\mathrm{Z}[\begin{array}{c}\mathrm{u}\\ \mathrm{v}\\ 1\end{array}]=\left[\begin{array}{ccc}{\mathrm{f}}_{\mathrm{x}}& 0& {\mathrm{c}}_{\mathrm{x}}\\ 0& {\mathrm{f}}_{\mathrm{y}}& {\mathrm{c}}_{\mathrm{y}}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{X}\\ \mathrm{Y}\\ \mathrm{Z}\end{array}\right]\triangleq \mathrm{KP}$$
  其中矩阵K称为相机的内参数矩阵.
• 由于相机的位姿随着机器人运动不断变化，需要进行世界坐标到相机坐标的坐标变换。$${\mathrm{ZP}}_{\mathrm{uv}}=\mathrm{K}({\mathrm{RP}}_{\mathrm{W}}+\mathrm{t})={\mathrm{KTP}}_{\mathrm{W}}$$
• 将最后一维Z进行归一化处理,得到点P在归一化平面的归一化坐标$${\mathrm{P}}_{\mathrm{c}}=[X/Z,Y/Z,1{]}^{\mathrm{T}}$$
  注：由$p_{uv}=kT\frac{p_w}{z}$可知，不管空间上有多少点$p_w$（实际上为光心到归一化平面上一点射线包含的所有点），都用一个$p_{uv}$坐标表示，即点的深度在投影过程中被丢失了。

三、畸变模型

由于透镜的自身形状影响光线及机械组装不平行，导致成像出现几遍。

• 径向畸变（坐标点沿长度方向变化）：分为桶形畸变和枕形畸变
  $$\begin{array}{rl}{\mathrm{x}}_{\mathrm{distorted}}& =\mathrm{x}(1+{\mathrm{k}}_1{\mathrm{r}}^2+{\mathrm{k}}_2{\mathrm{r}}^4+{\mathrm{k}}_{3}r6)\\ {\mathrm{y}}_{\mathrm{distorted}}& =\mathrm{y}(1+{\mathrm{k}}_1{\mathrm{r}}^2+{\mathrm{k}}_2{\mathrm{r}}^4+{\mathrm{k}}_{3}r6)\end{array}$$
• 切向畸变（坐标点沿水平夹角变化）
  $$\begin{array}{l}{\mathrm{x}}_{\mathrm{distorted}}=\mathrm{x}+2{\mathrm{p}}_1\mathrm{xy}+{\mathrm{p}}_2({\mathrm{r}}^2+2{\mathrm{x}}^2)\\ {\mathrm{y}}_{\mathrm{distorted}}=\mathrm{y}+{\mathrm{p}}_1({\mathrm{r}}^2+2{\mathrm{y}}^2)+2{\mathrm{p}}_2\mathrm{xy}\end{array}$$
• 对归一化平面上的点计算径向畸变和切向畸变
  $$\{\begin{array}{rl}{x}_{\mathrm{corrected}}& =x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ y_{\mathrm{corrected}}& =y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}.$$
  

四、双目相机模型

由$\Delta P{P}_L{P}_R\text{与}\Delta PP{O}_L{O}_R$相似得：
$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$整理得$$z=\frac{fb}{d},d\stackrel{def}{=}{u}_L-u_R$$

• d为视差。视察越大，距离越近.（由于视差最小为1像素，所以双目深度最大在理论上为fb）
• 基线b越大，双目测得得最大距离越远（依此分析面向，是不是眼间距越宽，看的越“长远”呢）

五、RGB-D相机模型

通过物理方式测距.

1.通过红外结构光原理测量像素距离

相机根据返回得结构光图案，计算物体与自身之间的距离。

2.通过飞行时间法 (Time-of- flight, ToF) 原理测量像素距离

相机向目标发射脉冲光，根据发送到返回之间的光束飞行时间，确定物体与自身的距离。

六、图像

计算机把图像（由多个像素组成）以矩阵的形式储存在内存中。

#表示一张宽度为640像素、高度为480像素分辨率的灰度图
unsigned char image[480][640];
#访问位于（x,y）处的像素
unsigned char pixel = image[y][x];