视觉SLAM十四讲笔记-5-1

视觉SLAM十四讲笔记-5-1

主要目标：
１. 理解针孔相机模型，内参与径向畸变参数；
２. 理解一个空间点是如何投影到相机成像平面的；
3. 掌握 opencv 的图像存储与表达方式；
4. 学会基本难点摄像头标定方法;
在前两章中主要介绍了"机器人如何表示自身位姿"，在本章中主要讨论"机器人如何观测外部世界"．

5.1 相机模型

相机将三维世界中的坐标点(单位为米)映射到二维平面(单位为像素)的过程能够用一个几何模型来描述．这个模型有很多种，其中最简单的称为针孔模型．

5.1.1 针孔相机模型

参考链接: link

对上图中简单的针孔模型进行几何建模．设O-x-y-z为相机坐标系，习惯上让z轴指向相机前方，x朝右，y朝下.　O为摄像机的光心，也是针孔模型的针孔．现实世界的空间点P，经过小孔O投影之后，落在物理成像平面O’-x’-y’上，成像点为P’．设Ｐ的坐标为$[X,Y,Z{]}^T$，P’的坐标为$[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$，并且设物理成像平面到小孔的距离为f，根据三角相似关系有：
$$\frac{Z}{f}=-\frac{X}{X^{\prime }}=-\frac{Y}{Y^{\prime }}$$
其中负号表示成立的像是倒立的．不过实际相机得到的像并不是倒立的，为了让模型更符合实际，可以等价地把成像平面对称地放到相机前方，和三维空间点一起放在摄像机坐标系的同一侧．这样可以把公式中的负号去掉．

$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$
把 X’,Y’放到等式左侧，整理得：
$$X^{\prime }=f\frac{X}{Z}$$
$$Y^{\prime }=f\frac{Y}{Z}$$
为了描述传感器将感受到的光线转换成图像像素的过程，设在物理成像平面上固定一个像素平面o-u-v，在像素平面得到了P’的像素坐标：$[u,v{]}^T$．
像素坐标系的定义方式：原点o’位于图像的左上角，u轴向右与x轴平行，v轴向下与y轴平行．像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移．设在u轴方向上缩放了$\alpha$倍，在v轴上缩放了$\beta$倍．同时，原点也平移了$[c_x,c_y{]}^T$．那么，P’的坐标与像素坐标$[u,v{]}^T$的关系为：
$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ \\ v=\beta Y^{\prime }+c_y\end{array}$$
将X’,Y’带入并把$\alpha f$ 合并成 $f_x$，把$\beta f$ 合并成 $f_y$．得：
$$\{\begin{array}{l}u=f_x\frac{X}{Z}+c_x\\ \\ v=f_y\frac{Y}{Z}+c_y\end{array}$$
其中，f的单位为米，$\alpha$，$\beta$的单位为像素/米，所以$f_x$,$f_y$,$c_x$,$c_y$的单位为像素．
把上式写成矩阵形式：
$$\left(\begin{array}{l}u\\ v\\ 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}{l}X\\ Y\\ Z\end{array}\right)\triangleq \frac{1}{Z}\mathbf{K}\mathbf{P}$$
习惯性将Z移到左侧：
$$Z\left(\begin{array}{l}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{l}X\\ Y\\ Z\end{array}\right)\triangleq \mathbf{K}\mathbf{P}$$
在上式中，习惯将中间的量表示为相机的内参数矩阵K．
通常相机的内参在出厂之后是固定的，不会在使用过程中发生变化．有的相机生产商会告诉相机的内参，而有时需要自己确定相机的内参，也就是所谓的标定．(目前，标定算法已比较成熟，如著名的单目棋盘格张正友标定法)．
在上式中，P是在相机坐标系下的坐标，但实际上由于相机在运动，所以P的相机坐标应该是它的世界坐标(记为$P_w$)根据相机的当前位姿变换到相机坐标系的结果．相机的位姿由它的旋转矩阵R和平移向量t来描述．
$$Z{\mathbf{P}}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{K}\left(\mathbf{R}{\mathbf{P}}_w+\mathbf{t}\right)=\mathbf{K}\mathbf{T}{\mathbf{P}}_w$$
其中，相机的位姿 $R,t$ 又称为相机的外参数．相比于不变的内参，外参会随着相机运动发生改变，也就是SLAM中待估计的目标，代表着机器人的轨迹．
上式表明，通常将一个世界坐标点先转换到相机坐标系，再除掉它最后一维的数值(即该点距离相机成像平面的深度)，这相当于把最后一维进行归一化处理，得到点P在相机归一化平面的投影：
$$\left(R{P}_{\mathrm{w}}+t\right)=\underset{\text{相 机 坐 标 }}{\underbrace{[X,Y,Z{]}^{\mathrm{T}}}}\to \underset{\text{归 一 化 坐 标 }}{\underbrace{[X/Z,Y/Z,1{]}^{\mathrm{T}}}}$$
归一化坐标可以看成相机前方Z=1处的平面上的一个点，这个Z=1平面也称为归一化平面．归一化坐标再左乘内参就得到了像素坐标．
从相机内参模型可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了．所以单目视觉中没法得到像素点的深度值．

5.1.2　畸变模型

为了获得更好的成像效果，可以在相机的前方加透镜．透镜的加入会对成像过程中光线的传播产生新的影响．由透镜形状引起的畸变称为径向畸变．
产生畸变的原因:
1）相机的透镜的加入，光线的传播不再是直线。——径向畸变。
2）相机内，成像平面和透镜不一定是完全平行的。 ——切向畸变。
径向畸变主要分为两类：桶形畸变和枕形畸变．

桶形畸变图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反．在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
畸变的矫正：
考虑归一化平面上的任意一点p，坐标为$[x,y{]}^T$，也可以写成极坐标的形式$[r,\theta {]}^T$，其中$r$表示点p与坐标系原点之间的距离，$\theta$表示与水平轴的夹角．径向畸变可以看成坐标点沿着长度方向发生了变化；切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化．通常这些畸变呈多项式关系：
$$\begin{array}{rl}& x_{\text{distorted}}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\\ & y_{\text{distorted}}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\end{array}$$
其中，$[x_{\text{distorted}},y_{\text{distorted}}]$ 是畸变后点的归一化坐标．对于切向畸变，可以使用另外两个参数 $p_1$,$p_2$ 进行纠正:
$$\begin{array}{rl}& x_{\text{distorted}}=x+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ & y_{\text{distorted}}=y+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$
因此，根据上面两个式子，对于相机坐标系中的一点P，能够通过5个畸变参数找到这个点在像素平面上的正确位置；
对于一个空间点，，它的畸变校正过程如下：
1.将三维空间点投影到归一化图像平面。设它的归一化坐标为 $[x,y{]}^T$。
2.对归一化平面上的点进行径向畸变和切向畸变纠正。
$$\{\begin{array}{l}{x}_{\text{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_{\text{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}$$
3.将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
$$\{\begin{array}{l}u=f_x{x}_{\text{distorted}}+c_x\\ v=f_y{y}_{\text{distorted}}+c_y\end{array}$$
实际应用中，有可能没有必要用5个畸变参数，可以灵活选择纠正模型，比如只选择$k_1,p_1,p_2$这３项．

对单目相机的成像过程进行总结：
1.首先，世界坐标系下有一个固定的点 $P$，世界坐标为 $P_w$。
2.由于相机在运动，它的运动由 $R,t$ 或变换矩阵 $T\in SE(3)$ 描述。$P$ 的相机坐标为： $\tilde{P}c=R{P}_w+t$。
3.这时的 $\tilde{P}c$ 仍有 $X,Y,Z$ 三个量，把它们投影到归一化平面 $Z=1$ 上，得到 $P$ 的归 一化相机坐标：$P_c=[X/Z,Y/Z,1{]}^T$ 。
4.有畸变时，根据畸变参数计算 $P_c$ 发生畸变后的坐标。
5.$P$的归一化坐标经过内参后，对应到它的像素坐标：$P_{uv}=K{P}_c$。

5.1.3 双目相机模型

单目针孔相机模型描述了单个相机的成像模型．然而，仅仅根据一个像素，无法确定这个空间点的具体位置(因为点的深度在投影过程中被丢失了)．只有当坐标点P的深度确定时，才能确切地知道它的空间位置．
测量深度的方法有多种，双目相机的原理：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度．
注：
1）左右两个相机都可以看成单目的相机。
2）两个相机的光圈中心都在x轴上，他们的距离称为双目相机的基线（记作 b）。
3）考虑一个空间点 P，它在左眼和右眼各成一像，记作 $P_L,P_R$。由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只有在 x 轴上有位移，因此 P 的像也只在 x 轴（对应图像的 u 轴）上有差异。我们记它在左侧的坐标为 $u_L$，右侧坐标 为 $u_R$。那么，它们的几何关系如上图右侧所示。根据$△P{P}_L{P}_R$ 和 $△P{O}_L{O}_R$ 的相似关系，有：
$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$
稍加整理，得：
$$z=\frac{fb}{d},d=u_L-u_R$$
其中 $d$ 定义为左右图的横坐标之差，称为视差． 可以看出，根据视差可以估计一个像素与相机之间的距离．视差与距离成反比：视差越大，距离越近．
双目相机使用的优缺点：能得到深度信息；d的确定（图像纹理丰富的地方才能确定），d的计算复杂。

5.1.4 RGB-D 相机模型

相比于双目相机通过视差计算深度的方式，RGB-D相机的做法更为主动，它能够主动测量每个像素的深度．
RGB-D相机可按原理分为两类：1）红外结构光；2）脉冲光（TOF，飞行时间法）
无论哪种方法，RGB-D相机都需要向探测目标发送一束光线，根据光线返回的数据来计算距离．
在测量深度之后，相机自己完成深度与彩色图像素的配对，输出一一对应的彩色图和深度图。对RGB-D数据，既可以在图像层面处理，也可以生成点云处理。
RGB-D相机缺点：成本高；功耗大。容易受日光或其他传感器发射的红外光干扰，不能再室外使用；同时使用多个也会相互干扰；对于投射材质的物体，无法使用。

5.2 图像

相机加上镜头，把三维世界中的信息转换成了一张由像素组成的照片，随后存储在计算机中，作为后续处理的数据来源．在数学中，图像可以用一个矩阵来描述．而在计算机中，它们占据一段连续的磁盘或内存空间，可以用二维数组来表示。
本节介绍一些图像处理的基本操作．通过 OpenCV 中图像数据的处理，理解计算机中图像的常见步骤．
从最简单的图像—灰度图说起．在一张灰度图中，每个像素位置 (x,y) 对应一个灰度值 I，所以一张宽度为 w, 高度为 h 的图像，数学上可以记为：
$$I(x,y):{\mathbb{R}}^2\mapsto \mathbb{R}$$
在计算机中并不能表达实数空间，需要对下标和图像读数在某个范围内进行量化．在常见的灰度图中，用 0~255 的整数（一个 unsigned char或1 个字节）来表达图像的灰度读数。那么，一张宽度为 640 像素、高度为 480 像素分辨率的灰度图就可以表示为：

unsigned char image[480][640] //二维数组表达图像

为什么这里的二维数组是480 * 640？因为在程序中，图像以二维数组形式存储，它的第一个下标是指数组的行，而第二个下标指的是列． 在图像中，数组的行数对应图像的高度，而列数对应图像的高度．
如下图所示，显示了传统像素坐标系的定义方式，像素坐标系原点位于图像的左上角，X轴向右， Y轴朝下．

根据这种定义方式，访问一个位于 x,y 处的像素，那么在程序中应该是：

unsigned char pixel = image[y][x];  //访问图像像素，第一个参数为行，第二个参数为列

一个像素的灰度可以用8位整数记录，也就是0-255的值．但是，当记录的信息更多时，一个字节恐怕就不够用了．例如，在RGB-D相机的深度图中，记录了各个像素与相机之间的距离．这个距离通常以毫米为单位，而RGB-D相机的量程通常在十几米左右，超过了255．这时，会采用16位整数(C++中的unsigned char)来记录深度图的信息，也就是位于0-65535的值．换算成米的话，最大可以表示65米，足够RGB-D相机使用．
彩色图像的表示则需要通道（channel）的概念。在计算机中，用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素，就要记录其 R、G、B 三个数值，每一个数值就称为一个通道。最常见的彩色图像有三个通道，每个通道都由 8 位整数表示。在这种规定下，一个像素占据 24 位空间。通道的数量、顺序都是可以自由定义的。在 OpenCV 的彩色图像中，通道的默认顺序是 B、G、R。也就是说，当得到一个 24 位的像素时，前 8 位表示蓝色数值，中间 8 位为绿色，最后 8 位为红色。如果还想表达图像的透明度，就使用 R、G、B、A 四个通道。