前言

本博客为主要学习《视觉SLAM十四讲》第5讲、《机器人学的状态估计》第6章6.4.1透视相机、《多视图几何》第5章摄像头模型等SLAM内容的总结与整理。
主要包括：
1、针孔相机模型、相机畸变、相机标定
2、双目相机模型、RGBD相机原理
2、双目相机与RGBD相机对比

1、单目相机模型

1.1 针孔相机模型

主要包括四种坐标：世界坐标、相机坐标、归一化相机坐标和像素坐标。
其中O-x-y-z为相机坐标系，O为相机的光心，一般z轴指向相机前方，x轴向右，y轴向下；当z=1时称为归一化平面；像素坐标系定义为：原点O‘位于图像左上角，u轴向右与x轴平行，v轴向下与y轴平行。

内参的推导：
根据针孔相机的前投影模型得到三维点和成像平面上点的关系：

$\frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'}\Rightarrow X'=f\frac{X}{Z}，Y'=f\frac{Y}{Z}$

再从成像平面到像素坐标：

$u=\alpha X'+c_x，v=\beta Y'+c_y$

整理得到：

$\begin{bmatrix}u\\v\\1 \end{bmatrix}=\frac{1}{Z}\begin{bmatrix}fx&0&c_x\\0&fy&c_y\\0&0&1 \end{bmatrix}\begin{bmatrix}X\\Y\\Z \end{bmatrix} \triangleq \frac{1}{Z}KP$

针孔模型总体流程：

1、世界坐标系下有一个点P，世界坐标为：$P_w$

2、相机的运动由R、t描述，将其转移到相机坐标系下为：$\bar P_c=RP_w+t$

3、将相机坐标投影到归一化平面上，得到归一化相机坐标：$P_c = [X/Z,Y/Z,1]$

4、归一化坐标经过内参后对应到其像素坐标：$P_{uv}=KP_c$

1.2 相机畸变

为了获得更好的成像效果，会在相机前方安装滤镜。滤镜自身的形状对光纤传播产生影响，引起的畸变称为径向畸变，主要分为桶形畸变和枕型畸变；在机械组装过程中不能保证透镜和成像平面严格平行产生的时切向畸变。

对于归一化平面上一点$p=[x,y]^T$，也可以写成极坐标的形式$[r,\theta]^T$，假设畸变后点的坐标为$[x_{distorted},y_{distorted}]$。对于径向畸变有：
$x_{distorted}=x(1+k_1r^2+k_2r^4+k_3r^6)$

$y_{distorted}=y(1+k_1r^2+k_2r^4+k_3r^6)$

对于切向畸变有：

$x_{distorted}=x+2p_1xy+p_2(r^2+2x^2)$

$y_{distorted}=y+p_1(r^2+2y^2)+2p_2xy$

去畸变流程：对于每个归一化坐标$(x,y)$，找到其在原始（畸变）图像上的坐标$(x_{distorted},y_{distorted})$，然后将纠正后的点通过内参数投影到像素平面，得到该点在图像上的正确位置。

1.3 相机标定（张正友标定）

相机标定的主要目的是确定相机的内参K与畸变系数，在标定过程中也会求得相机的运动即外参。

这里主要介绍最常用的张氏标定，即棋盘格标定，其是介于传统标定方法和自标定方法之间的一种方法。注意张氏标定仅考虑了径向畸变而没有切向畸变。

1）求单应性矩阵H
设棋盘格中一点的三维世界坐标$P=[X,Y,Z,1]^T$，像素坐标为$m=[u,v,1]^T$，得棋盘格平面到图像平面的单应性关系为：
$sm=K[R,T]X$

令相机内参K为：$\begin{bmatrix}\alpha & \gamma & u_0\\0 & \beta & v_0\\0&0&1 \end{bmatrix}$

令棋盘格所在平面为Z=0平面，可得：
$s\begin{bmatrix}u\\v\\1 \end{bmatrix}=K\begin{bmatrix}r_1 & r_2 & r_3 &t \end{bmatrix}\begin{bmatrix}X\\Y\\0\\1 \end{bmatrix}=K\begin{bmatrix}r_1 & r_2 &t \end{bmatrix}\begin{bmatrix}X\\Y\\1 \end{bmatrix}$

$\begin{bmatrix}u\\v\\1 \end{bmatrix}=\frac{1}{s} K\begin{bmatrix}r_1 & r_2 &t \end{bmatrix}\begin{bmatrix}X\\Y\\1 \end{bmatrix} \triangleq H\begin{bmatrix}X\\Y\\1 \end{bmatrix}$

得到两个平面的单应性矩阵$H=[h_1,h_2,h_3]$，其是一个齐次方程，有8个未知数，每对对应点提供两个方程，所以需要4对对应点可以算出世界平面到图像平面的单应性矩阵H。

2）求内参矩阵
由上述得
$r_1 = sK^{-1}h_1，r_2 = sK^{-1}h_2$
由于旋转矩阵正交性，可得：
$r_1^Tr_2=0，||r_1||=||r_2||=1$

代入有：
$h_1^TK^{-T}K^{-1}h_2=0，h_1^TK^{-T}K^{-1}h_1=h_2^TK^{-T}K^{-1}h_2$

每个单应性矩阵提供两个方程，而内参矩阵包含5个参数，至少需要3个单应性矩阵

3）计算外参矩阵
根据$\frac{1}{s} K\begin{bmatrix}r_1 & r_2 &t \end{bmatrix}= H = \begin{bmatrix}h_1 &h_2 &h_3\end{bmatrix}$，我们已经得到了H和K，可得：

$s=\frac{1}{||K^{-1}h_1||}=\frac{1}{||K^{-1}h_2||}$

$r_1 = sK^{-1}h_1，r_2 = sK^{-1}h_2$

$r_3 = r_1\times r_2$

$t = sK^{-1}h_3$

4）极大似然估计
上述推导了理论值，然而实际测量存在高斯噪声，因此需要使用最大似然估计进行优化，假设采集了n副棋盘格图像，每个棋盘格上检测到角点m个，其中第i副棋盘格上的角点Mj的像素投影点为：
$\hat m(K,R_i,t_i,M_{ij})=K[R|t]M_{ij}$

构造似然函数并使用LM算法迭代求解非线性最小二乘问题，并采用之前计算的理论值作为初值：
$\sum^n_{i=1}\sum^m_{j=1} ||m_{ij}-\hat m(K,R_i,t_i,M_{ij})||^2$

5）径向畸变估计
张氏标定只关注影响最大的径向畸变。根据之前的径向畸变方程：
$u_{distorted}=u+(u-u_0)(k_1r^2+k_2r^4)$

$v_{distorted}=v+(v-v_0)(k_1r^2+k_2r^4)$

化成矩阵形式：
$\begin{bmatrix}(u-u_0)r^2 &(u-u_0)r^4\\(v-v_0)r^2&(v-v_0)r^4 \end{bmatrix}\begin{bmatrix}k_1\\k_2 \end{bmatrix}=\begin{bmatrix}u_{distorted}-u\\v_{distorted}-v \end{bmatrix}$

记为：$Dk=d\Rightarrow k=(D^TD)^{-1}D^Td$，然后再对最大似然估计进行优化。

1.4 全局快门（global shutter）和卷帘快门（rolling shutter）

全局快门是在同一时间进行曝光，其曝光时间短，会增加噪点；卷帘快门时通过传感器逐行扫描进行曝光，不同行像元的曝光时间不同，因此当曝光不当或物体移动较快时，会出现部分曝光(partial exposure)、斜坡图形(skew)、晃动(wobble) 等现象（称为果冻效应），但是可以达到更高的帧数和更低的噪声。

---

2、立体相机模型

2.1 双目相机模型

双目相机一般由两个水平放置的相机组成，可以将其看作针孔相机模型。两者的距离称为刷双目相机的基线。

假设空间点P在左右相机各成的像为$P_L、P_R$，理想情况下两者只在图像x轴上有差异，记左侧坐标为$u_L$，右侧坐标为$u_R$，根据三角形相似关系有：
$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$

可得：
$z=\frac{fb}{d}，d=u_L-u_R$

这里d为左右两图的横坐标之差，称为视差。由于视差最小为一个像素，双目深度存在一个理论上的最大值，由$fb$决定。

2.2 RGBD相机原理

这部分想详细了解推荐看博客 https://blog.csdn.net/electech6/article/details/78707839 和
https://blog.csdn.net/electech6/article/details/78349107

RGBD相机原理主要分为：
1、通过红外结构光(Structured Light)来测量像素距离，如Kinect1、Project Tango1、Intel Realsense等；
通过近红外激光器，将具有一定结构特征的光（比如离散光斑、条纹光、编码结构光等）投射到物体上，再由专门的红外摄像头进行采集。然后通过返回的结构光图案，计算物体与自身的距离。

2、通过飞行时间法（TOF）原理来测量像素距离，如Kinect2、TOF传感器等。
通过发射脉冲光到被观测物体上，然后接收从物体反射回去的脉冲光，通过发送到返回的光束飞行（往返）时间来计算被测物体离相机的距离。分为光脉冲调制和连续波调制。

2.3 双目相机与RGBD相机对比

（部分数据可能是前几年的技术水平，仅供参考，但局限性与基本趋势不会变）

相机	TOF	结构光	双目
测距方式	主动式	主动式	被动式
工作原理	根据红外(IR)光飞行时间直接测量	主动投影已知编码图案	图像特征点匹配，三角测量间接计算
帧率	较高，可达上百FPS	30fps	从高到低都有（一般较低）
分辨率	低于640X480	可达1080X720	可达2K
测量精度	最高里厘米	近距离可达0.01-1mm	近距离可达毫米级
测量范围	可测量较远距离	一般10m以内	由于基线限制，只能测量较近距离
强光环境	中	弱	良好
低光环境	良好	良好	弱
影响因素	不受光照变化和物体纹理影响，受多重反射影响	容易受光照影响，难以用于室外	受光照变化和物理纹理影响很大，夜晚无法使用
硬件成本	高	中	低
软件复杂度	低	中	高
功耗	很高，需全面照射	中，只需照射局部区域	低