【SLAM学习】（二）相机原理

相机模型

相机的作用原理为：将三维空间的点映射到二维成像平面。

下面用最简单的针孔模型来描述这个映射过程：

设 $O-x-y-z$ 为相机坐标系，$O$ 为相机光心，也就是针孔模型里的针孔，空间中一点 $P$ 经小孔投影后在成像平面 $O^{\prime }-x^{\prime }-y^{\prime }$ 形成点 $P^{\prime }$。

设 $P$ 坐标为 $[X,Y,Z{]}^T$， $P^{\prime }$ 坐标为 $[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$ , 成像平面到小孔的距离为焦距 $f$ ，根据相似可得：

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

为了简化模型，我们把成像平面对称到相机前方，这样得出的 $P^{\prime }$ 坐标和 $P$ 方向一致，就可以把负号去掉了。

整理得： $X^{\prime }=f\frac{X}{Z}$ 和 $Y^{\prime }=f\frac{Y}{X}$.

上式描述了 $P^{\prime }$ 和 $P$ 之间的空间关系，但在相机中，我们最终获得的是像素，所以需要在成像平面上进行采样和量化。为了描述传感器将光线转换成图像像素的过程，可以设在成像平面上固定着一个像素平面 $o-u-v$ ，像素平面上 $P^{\prime }$ 的像素坐标为：$[u,v{]}^T$.

像素坐标系的定义方式为：原点位于图像左上角，$u$ 轴与 $x$ 轴平行，$v$ 轴与 $y$ 轴平行。像素坐标系与成像平面在 $u$ 轴上缩放了 $\alpha$ 倍，在 $v$ 轴上缩放了 $\beta$ 倍，原点平移了 $[c_x,c_y{]}^T$，可得像素坐标 $[u,v{]}^T$ 与 $P^{\prime }$ 坐标关系为:

将 $X^{\prime }=f\frac{X}{Z}$ 和 $Y^{\prime }=f\frac{Y}{X}$ 带入，并将 $\alpha f$合并为 $f_X$ , $\beta f$ 合并为 $f_y$ 可得：

$f$ 的单位为米，$\alpha ，\beta$的单位为像素/米，所以 $f_x,f_y$ 的单位为像素。

利用齐次坐标可写为矩阵形式：

再将 $Z$ 移到左边：

$K$ 称为相机的内参数矩阵，包含着焦距 $f$ ，像素平面到成像平面的缩放量 $\alpha \beta$ 和平移量 $c_x,c_y$. 平时所说的标定就是指标定内参数矩阵，可以使用 $ROS,Matlab$ 等软件来标定相机，一般通过照标定板来进行标定：

有内参那么就也有外参，上式中 $P$ 的坐标是在相机坐标系下，由上一节可得世界坐标系到相机坐标系之间的关系可以用欧氏变换来描述，设旋转矩阵为 $R$ ，平移向量为 $t$ ，那么有：

$R,t$ 称为相机的外参数，外参会随着相机运动改变，是SLAM中需要估计的目标，代表着机器人的轨迹。

有人可能会发现 $T$ 是4维方阵，$K$ 为3维方阵，怎么能相乘？

其实 $T{P}_w$ 包含着由齐次坐标到非齐次坐标的转换，把最后一维去掉了。

除此之外，相机坐标系下的坐标 $P_c=(T{P}_w)$ 可以进行归一化处理：

得到的称为归一化坐标，对应着相机前方 $z=1$ 处的平面，称为归一化平面。

$P_c$ 经过内参 $K$ 后即可得到像素坐标，所以可以把像素坐标 $[u,v]$ 看作对归一化平面上的点进行量化测量的结果。

畸变

实际相机中往往加入透镜，透镜的形状会对光线的传播造成影响；组装透镜的过程中往往也不能够保证透镜与成像平面严格平行，这也会使得光线通过透镜到达成像平面时的位置发生变化。

由透镜形状引起的畸变称为径向畸变，径向畸变又分为桶形畸变和枕形畸变。

桶形畸变是因为图像放大率随着光轴的距离增加而减小，枕形畸变则是因为图像放大率随着光轴的距离增加而增大。

切向畸变是因为透镜与成像平面不严格平行造成的。

用数学形式来描述一下两类畸变以及如何纠正它们：

设归一化平面上有一点 $p$ ，其在笛卡尔坐标为：$[x,y{]}^T$，用极坐标可表示为：$[r,\theta {]}^T$. 径向畸变可以看作坐标沿着长度方向上变化了 $\delta r$ ，切向畸变则可以看作坐标沿角度变化了 $\delta \theta$.

由于径向畸变离中心越远越明显，所以可以用一个高次多项式来纠正：

$x_{corrected}=x+x(k_1{r}^2+k_2{r}^4+k_3{r}^6)$

$y_{corrected}=y+y(k_1{r}^2+k_2{r}^4+k_3{r}^6)$

将 $[x,y{]}^T$ 纠正为 $[x_{corrrected},y_{corrected}{]}^T$ ，由于距图像中心较近的区域畸变较小，所以主要是 $k_1$ 起作用；距图像中心较远的边缘区域畸变较大，主要是 $k_2$ 起作用；特殊的摄像头，如鱼眼摄像头，畸变更大，主要是 $k_3$ 起作用。

对于切向畸变，则可以用另外两个参数 $p_1,p_2$ 来纠正：

$x_{corrected}=x+2p_{1}xy+p_2(r^2+2x^2)$

$y_{corrected}=y+2p_{2}xy+p_1(r^2+2y^2)$

综合可得，对于归一化平面上的一点 $p$ ，归一化坐标为 $[x,y{]}^T$ ，距原点距离为 $r$ ，则纠正畸变的公式为：

$x_{corrected}=x+x(k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2)$

$y_{corrected}=y+y(k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{2}xy+p_1(r^2+2y^2)$

然后就可以通过内参数矩阵将纠正后的归一化坐标投影到像素平面了：

$u=f_x{x}_{corrected}+c_x$

$v=f_y{y}_{corrected}+c_y$

实际应用中不一定要用到五个参数，可以灵活选择。

双目相机模型

虽然针孔模型可以描述单个相机的成像过程，但是仅由成像所得的像素是无法确定 $p$ 点空间的具体位置。假设由光心到 $P$ 归一化平面上的坐标所连成一条直线，那么空间中任意在该直线上的一点，经针孔模型所得的像素坐标与 $P$ 一致。所以说只有当 $P$ 的深度确定时，空间的具体位置才确定。

目前确定深度的相机主要是双目相机和RGB-D相机，本节先来介绍一下双目相机。

双目相机的主要原理为视差法测距：

首先，双目相机一般由左右两个水平放置的相机组成，可以看作两个针孔相机。由于是水平放置的，所以两个相机的光心都位于 $x$ 轴上 ，二者距离称作双目相机的基线。

现考虑空间中一点 $P$ ，在左右相机成不同的像，分别记为 $P_L,P_R$，由于左右两个相机只在 $x$ 轴上有偏移，因此成的像也只在 $u$ 轴上有差异，记 $P_L$ 在 $u$ 轴的坐标为 $u_L$ ，$P_R$ 在 $u$ 轴的坐标为 $u_R$ ，由三角形的相似得：

$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$ ，定义 $d=u_L-u_R$ 为视差，则可得 $z=\frac{fb}{d}$.

根据公式可得深度 $z$ 与视差 $d$ 成反比，视差越小，距离越远。因为视差 $d$ 最小为一个像素，所以双目相机的深度存在理论最大值，由 $fb$ 决定，基线越长，则双目相机可测得的最大距离越远。

虽然由视差计算深度的公式很简单，但是由于计算图像上的每一个点的视差计算量很大，所以双目相机测深度仍然需要 $GPU$ 或 $FPGA$ 来计算。

RGB-D相机模型

RGB-D相机主要分为结构光和TOF种原理：

结构光原理为：相机对探测目标发射一束光线，根据返回结构光图案来计算距离。

TOF全称Time-of-flight，通过对目标发射脉冲光，根据返回光的时间来确定距离，这一点和激光雷达很像，但TOF是直接获取整个图像的像素深度。

RGB-D相机通常会生成一一对应的彩色图和深度图，可以通过深度图和彩色图生成点云（PCL）。

图像

相机加上镜头，可以把三维世界中的信息转换成了一个由像素组成的照片，通过二维数组储存在计算机中。

从最简单的灰度图说起，我们通常用0-255之间的整数来表达图像的灰度大小。比如有一张宽度为640，高度为480的灰度图，可以用unsigned char image[480][640] 来表示，注意二维数组的行数表示高度，列数表示宽度。

深度图由于深度相机的量程通常有十几米，一个字节不够用，所以用0-65536之间的数来表示。

彩色图像则需要通道(channel)的概念，通常的彩色图有三个通道，分别对应红绿蓝 $RGB$ 三种颜色，每个通道由八位整数来表示。比如 $OpenCV$ 的彩色图中，通道的默认顺序为 $B,G,R$，也就是说得到一个24位的像素，前8位表示蓝色的数值，中间8位表示绿色的数值，最后8位表示红色的数值。如果还想表示透明度，则还要加上 $A$ 通道。

el)的概念，通常的彩色图有三个通道，分别对应红绿蓝 $RGB$ 三种颜色，每个通道由八位整数来表示。比如 $OpenCV$ 的彩色图中，通道的默认顺序为 $B,G,R$，也就是说得到一个24位的像素，前8位表示蓝色的数值，中间8位表示绿色的数值，最后8位表示红色的数值。如果还想表示透明度，则还要加上 $A$ 通道。