视觉SLAM十四讲学习记录第五讲

第五讲相机与图像

前面两讲中，我们介绍了“机器人如何表示自身位姿”的问题，部分地解释了SLAM经典模型中变量的含义和运动方程部分。本讲将讨论“机器人如何观测外部世界”，也就是观测方程部分。而在以相机为主的视觉SLAM中，观测主要是指相机成像的过程。
我们在现实生活中能看到大量的照片。在计算机中，一张照片由很多像素组成，每个像素记录了色彩或亮度的信息。三维世界中的一个物体反射或发出的光线，穿过相机光心后，投影在相机的成像平面上。相机的感光器件接受到光线后，产生测量值，就得到了像素，形成了我们见到的照片。这个过程能否用数学原理来描述呢？本讲将首先讨论相机模型，说明投影关系具体如何描述，相机的内参是什么。同时，简单介绍双目成像与RGB-D相机的原理。然后，介绍二维照片像素的基本操作。最后，根据内外参数的含义，演示一个点云拼接的实验。

5.1 相机模型

相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程能够用一个几何模型进行描述。这个模型有很多种，其中最简单的称为针孔模型。针孔模型是很常用而且有效的模型，它描述了一束光线通过针孔之后，在针孔背面投影成像的关系。在本书中我们我们用一个简单的模型对该关系进行建模。同时，由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变。因此，我们使用针孔和畸变两个模型来描述整个投影过程。
本节先给出相机的针孔模型，再对透镜的畸变模型进行讲解。这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数（Intrinsics）。

5.1.1 针孔相机模型

在初中物理课堂上，为我们应该都见过一个蜡烛投影实验：在一个暗箱的前方放着一支点燃的蜡烛，蜡烛的光透过暗箱上的一个小孔投影在暗箱的后方平面上，并在这个平面上形成一个倒立的蜡烛图像。在这个过程中，小孔模型能够把三维世界中的蜡烛投影到一个二维成像平面。同理，我们也可以用这个简单的模型来解释相机的成像过程，如下图：
在这里插入图片描述
现在对这个简单的针孔模型进行几何模型。设 $O - x - y - z$ 为相机坐标系，习惯上我们让 $z$ 轴指向相机前方， $x$ 轴向右， $y$ 轴向下（上图我们应该站在左侧看右侧，即假想我们是相机）。 $O$ 为摄像机的光心，也是针孔模型中的针孔。现实世界的空间点 $P$ ，经过小孔 $O$ 投影之后，落在物理成像平面 $O^{'}-x^{'}-y^{'}$ 上，成像点为 $P^{'}$ 。设 $P$ 的坐标为 $X,Y,Z]^T，$ $P^{'}$ 的坐标为 $X^{'},Y^{'},Z^{'}]^T，$ 并且设物理成像平面到小孔的距离为 $f$ （焦距）。那么，根据三角形相似关系，有

$\frac{Z}{f} = -\frac{X}{X^{'}}=-\frac{Y}{Y^{'}}$

其中负号表示成的像是倒立的。不过，实际相机得到的图像并不是倒像（否则相机的使用会变的十分不方便）。为了让模型更符合实际，我们可以等价地把成像平面对称地放到相机前方，和三维空间点一起放在摄像机坐标系的同一侧，如下图所示。这样做可以把公式中的负号去掉，使式子更加简洁：

$\frac{Z}{f} = \frac{X}{X^{'}}=\frac{Y}{Y^{'}}$
在这里插入图片描述
把 $X^{'},Y^{'}$ 放到等式左侧，整理得

$X^{'} = f\frac{X}{Z}$

$Y^{'} = f\frac{Y}{Z}$

读者可能要问，为什么我们可以看似随意地把成像平面挪到前方呢？这只是我们处理真实世界与相机投影的数学手段，并且，大多数相机输出的图像并不是倒像——相机自身的软件会帮你翻转这张图像，所以我们实际得到的是正向，也就是对称的成像平面上的像。所以，尽管从物理原理的角度看，小孔成像应该是倒向，但由于我们对图像做了预处理，所以理解成在对称平面上的像并不会带来什么坏处。于是，在不引起歧义的情况下，我们也不加限制地称后一种情况为针孔模型。
上式描述了点 $P$ 和它的像之间的空间关系，这里所有点的单位都可理解成米，比如比如焦距是0.2米， $X^{'}$ 是0.14米。不过，在相机中，我们最终获得的是一个个的像素，这还需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面 $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{cases}u = \alpha X^{'}+c_x\\v=\beta Y^{'}+c_y\end{cases}$

代入上式并把 $\alpha f$ 合并成 $f_x$ ，把 $\beta f$ 合并成 $f_y$ ，得

$\begin{cases}u = f_x\frac{X}{Z}+c_x\\v=f_y\frac{Y}{Z}+c_y\end{cases}$

其中， $f$ 的单位为米， $\alpha,\beta$ 的单位为像素/米，所以 $f_x,f_y$ 和 $c_x,c_y$ 的单位为像素。把该式写成矩阵形式会更加简洁，不过左侧需要用到齐次坐标，右侧则是非齐次坐标：

$\begin{pmatrix}u\\\\v\\\\1\end{pmatrix}=\frac{1}{Z}\begin{pmatrix}f_x&0&c_x\\\\0&f_y&c_y\\\\0&0&1\end{pmatrix}\begin{pmatrix}X\\\\Y\\\\Z\end{pmatrix}\overset{def}{=}\frac{1}{Z}KP.$

我们习惯性地把 $Z$ 挪到左侧：

$Z\begin{pmatrix}u\\\\v\\\\1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\\\0&f_y&c_y\\\\0&0&1\end{pmatrix}\begin{pmatrix}X\\\\Y\\\\Z\end{pmatrix}\overset{def}{=}KP.$

该式中，我们把中间的量组成的矩阵称为相机的内参数（Camera Intrinsics)矩阵K。通常认为，该相机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。鉴于标定算法业已成熟（如著名的单目棋盘格张正友标定法），这里不赘述了。
有内参，自然也有相对的外参。在上式中，我们使用的是 $P$ 在相机坐标系下的坐标，但实际上由于相机在运动，所以 $P$ 的相机坐标系应该是它的世界坐标（记为 $P_w$ ）根据相机的当前位姿变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵 $R$ 和平移向量 $t$ 来描述。那么有

$ZP_{uv} = Z\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}=K(RP_w+t)=KTP_w$

注意后一个式子隐含了一次齐次坐标到非齐次坐标的转换。它描述了 $P$ 的世界坐标到像素坐标的投影关系。其中，相机的位姿 $R, t$ 又称为相机的外参数（Camera Extrinsics）。相比于不变的内参，外参会随着相机运动发生改变，也是SLAM中待估计的目标，代表着机器人的轨迹。
投影过程还可以从另一个角度来看。我们可以把一个世界坐标点先转换到相机坐标系，再除掉它最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点 $P$ 在相机归一化平面上的投影：

$(RP_w + t) = \underbrace{[X,Y,Z]^T}_{相机坐标} \rightarrow \underbrace{[X/Z,Y/Z,1]^T}_{归一化坐标}.\\\;$

归一化坐标可看成相机前方 $z = 1$ 处的平面上的一个点，这个 $z = 1$ 平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标 $u,v]^T$ 看成对归一化平面上的点进行量化测试的结果。从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的。这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

5.1.2 畸变模型

为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入会对成像过程中光线的传播产生新的影响：一是透镜自身的形状对光线传播的影响；二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像面时的位置发生变化。
由透镜形状引起的畸变（Distortion，也叫失真）称为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。可是，在实际拍摄的图片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类：桶形畸变和枕形畸变，如下图所示。
在这里插入图片描述
桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行，所以也会引入切向畸变，如下图所示：
在这里插入图片描述
为了更好地理解径向畸变和切向畸变，我们用更严格的数学形式对两者进行描述。考虑归一化平面上的任意一点 $p$ ，它的坐标为 $x,y]^T$ ，也可写成极坐标的形式 $[r,\theta]^T，$ 其中 $r$ 表示点 $p$ 与坐标系原点之间的距离， $\theta$ 表示与水平轴的夹角。径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。通常假设这些畸变呈现多项式关系，即：

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

其中， $x_{distorted},y_{distorted}]^T$ 是畸变后点的归一化坐标。另外，对于切向畸变，可以使用另外两个参数 $p_1,p_2$ 进行纠正：

$x_{distorted} = x + 2p_1xy + p_2(r^2 + 2x^2)$
$y_{distorted} = y + p_1(r^2 + 2y^2) + 2p_2xy$

因此，联合上面两式，对于相机坐标系中的一点 $P$ ，我们能够通过5个畸变系数找到这个点在像素平面上的正确位置：

将三维空间点投影到归一化图像平面。设它的归一化坐标为 $x,y]^T$ 。
对归一化平面上的点计算径向畸变和切向畸变。
$\begin{cases}x_{distorted} = x(1 + k_1r^2 + k_2r^4 +k_3r^6) + 2p_1xy + p_2(r^2 + 2x^2)\\ y_{distorted} = x(1 + k_1r^2 + k_2r^4 +k_3r^6) + p_1(r^2 + 2y^2) + 2p_2xy\end{cases}$
将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
$\begin{cases}u = f_x x_{distorted + c_x}\\v = f_y y_{distorted + c_y}\end{cases}$

在上面的纠正畸变的过程中，我们使用了5个畸变项。实际应用中，可以灵活选择纠正模型，比如只选择 $k_1,p_1,p_2$ 这3项。
在本节中，我们对相机的成像过程使用针孔模型进行了建模，也对透镜引起的径向畸变和切向畸变进行了描述。实际的图像系统中，学者提出了很多其他的模型，比如相机的仿射模型和透镜模型等，同时也存在很多其他类型的畸变。考虑到视觉SLAM中一般都使用普通的摄像头，针孔模型及径向畸变和切向畸变模型已经足够，因此，我们不再对其他模型进行描述。
值得一提的是，存在两种去畸变处理（Undistort，或称畸变矫正）做法。我们可以选择先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。或者，也可以从畸变图像上的某个点出发，按照畸变方程，讨论其畸变前的空间位置。二者都是可行的，不过前者在视觉SLAM中似乎更常见。所以，当一个图像去畸变之后，我们就可以直接用针孔模型建立投影关系，而不用考虑畸变。因此，后文的讨论中，我们可以直接假设图像已经进行了去畸变处理。
最后，我们总结单目相机的成像过程：

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

综上所述，我们一共谈到了四种坐标：世界坐标、相机坐标、归一化坐标和像素坐标。

5.1.3 双目相机模型

针孔相机模型描述了单个相机的成像模型。然而，仅根据一个像素，我们无法确定这个空间点的具体位置。这是因为，从相机光心到归一化平面连线上的所有点，都可以投影至该像素上。只有当P的深度确定时（比如通过双目或RGB-D相机），我们才能确切地知道它的空间位置，如下图：
在这里插入图片描述
测量像素距离（或深度）地方式有很多种，比如人眼就可以根据左右眼看到的景物差异（或称视差）判断物体与我们的距离。双目相机的原理亦是如此：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。下面简单介绍双目相机的成像原理：
在这里插入图片描述
双目相机一般由左眼相机和右眼相机两个水平放置的相机组成。当然也可以做成上下两个目，不过我们见到的主流双目都是做成左右形式的。在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的，意味着两个相机的光圈中心都位于 $x$ 轴上。两者之间的距离称为双目相机的基线（记作 $b$ ），是双目相机的重要参数。
现在，考虑一个空间点 $P$ ,它在左眼相机和右眼相机各成一像，记作 $P_L,P_R$ 。由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只在 $x$ 轴上有位移，所以 $P$ 的像也只在 $x$ 轴上有差异。记它的左侧坐标为 $u_L$ ，右侧坐标为 $u_R$ ，几何关系如下所示。根据 $\Delta PP_LP_R$ 和 $\Delta PO_LO_R$ 的相似关系，有

$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}.$

稍加整理，得

$\frac{fb}{d},d\overset{def}{=}u_L-u_R.$

其中 $d$ 定义为左右图得横坐标之差，称为视差。根据视差，我们可以估计一个像素与相机之间得距离。视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由 $f b$ 确定。我们看到，基线越长，双目能测到的最大距离就越远；反之，小型双目器件则只能测量很近的距离。相似地，我们人眼在看非常远的物体时（如很远的飞机），通常不能准确判断它的距离。
虽然由视差计算深度的公式很简洁，但视差 $d$ 本身的计算却比较困难。我们需要确切地知道左眼图像的某个像素出现在右眼图像的哪一个位置（即对应关系），这件事也属于“人类觉得容易而计算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGA来实时计算。

5.1.4 RGB-D相机模型

相比于双目相机通过视差计算深度的方式，RGB-D相机的做法更“主动”，它能够主动测量每个像素的深度。目前的RGB-D相机按原理可分为两大类：

通过红外结构光（Structured Light）原理测量像素距离。例子有Kinect 1代、Project Tango 1代、Intel RealSense等。
通过飞行时间（Time-of-Flight，ToF）原理测量像素距离。例子有Kinect 2代和一些现有的ToF传感器等。

无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线（通常是红外光）。在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。而在ToF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。ToF的原理和激光传感器十分相似，只不过激光是通过逐点扫描获取距离的，而ToF相机则可以获得整个图像的像素深度，这也正是RGB-D相机的特点。所以，如果你把一个RGB-D相机拆开，通常会发现除了普通的摄像头，至少会有一个发射器和一个接收器。
在测量深度之后，RGB-D相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云（Point Cloud）。既可以在图像层面对RGB-D数据进行处理，也可在点云层面处理。
RGB-D相机能够实时地测量每个像素点地距离。但是，由于使用这种发射-接收地测量方式，其适用范围比较受限。用红外光进行深度值测量地RGB-D相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。在没有调制地情况下，同时使用多个RGB-D相机时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D相机在成本、功耗方面，都有一些劣势。

5.2 图像

相机加上镜头，把三维世界中的信息转换成了一张由像素组成的照片，随后存储在计算机中，作为后续处理的数据来源。在数学中，图像可以用一个矩阵来描述；而在计算机中，它们占据一段连续的磁盘或内存空间，可以用二维数组来表示。这样一来，程序就不必区别它们处理的是一个数值矩阵，还是有实际意义的图像了。
本节，我们将介绍计算机图像处理的一些基本操作。特别地，通过OpenCV中图像数据的处理，理解计算机中处理图像的常见步骤，为后续章节打下基础。从最简单的图像——灰度图说起。在一张灰度图中，每个像素位置 $(x, y)$ 对应一个灰度值 $I$ ,所以，一张宽度为 $w$ 、高度为 $h$ 的图像，数学上可以记为一个函数：

$I(x,y):\mathbb{R}^2\rightarrow\mathbb{R}.$
其中，（x,y）是像素的坐标。然而，计算机并不能表达实数空间，所以我们需要对下标和图像读数在某个范围内进行量化。例如，(x,y)通常是从0开始的整数。在常见的灰度图中，用0~255的整数（即一个unsigned char，1个字节）来表达图像的灰度读数。那么，一张宽度为640像素、高度为480分辨率的灰度图就可以表示为：

unsigned char image[480][640];

为什么这里的二维数组是480×640呢？因为在程序中，图像以二维数组形式存储。它的第一个下标是指数组的行，而第二个下标则是列。在图像中，数组的行数对应图像的高度，而列数对应图像的宽度。
下面考察这幅图像的内容。图像自然是由像素组成的。当访问某一个像素时，需要指明他所处的坐标，如下图所示。该图左边显示了传统像素坐标系的定义方式。像素坐标系原点位于图像的左上角， $X$ 轴向右， $Y$ 轴向下（即前面所说的 $u, v$ 坐标）。如果它还有第三个轴—— $Z$ ,那么根据右手法则， $Z$ 轴应该是向前的。这种定义方式是与相机坐标系一致的。我们平时说的图像的宽度或列数，对应着 $X$ 轴；而图像的行数或高度，则对应着它的 $Y$ 轴。
根据这种定义方式，如果我们讨论一个位于 $x, y$ 处的像素，那么它在程序中的访问方式应该是：

unsigned char pixel = image[y][x];

它对应着灰度值 $I (x, y)$ 的读数。请注意这里的 $x$ 和 $y$ 的顺序。如果你在写程序时不慎调换了 $x, y$ 的坐标，编译器无法提供任何信息，而你所能看到的只是程序运行中的一个越界错误而已。
在这里插入图片描述
一个像素的灰度可以用8位整数记录，也就是一个0~255的值。当我们要记录的信息更多时，一个字节恐怕就不够了。例如，在RGB-D相机的深度图中，记录了各个像素与相机之间的距离。这个距离通常以毫米为单位，而RGB-D相机的量程通常在十几米左右，超过了255。这时，人们会采用16位整数（C++中的unsigned short）来记录深度图的信息，也就是位于0 ~ 65535的值。换算成米的话，最大可以表示65米，足够RGB-D相机使用了。
彩色图像的表示则需要通道（channel）的概念。在计算机中，我们用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素，就要记录其R、G、B三个数值，每一个数值就称为一个通道。例如，最常见的彩色图像有三个通道，每个通道都由8位整数表示。在这种规定下，一个像素占据24位空间。
通道的数量、顺序都是可以自由定义的。在OpenCV的彩色图像中，通道的默认顺序是B、G、R。也就是说，当我们得到一个24位的像素时，前8位表示蓝色数值，中间8位为绿色数值，最后8位为红色数值。同理，也可使用R、G、B的顺序表示一个彩色图。如果还想表达图像的透明度，就使用R、G、B、A四个通道。

5.3 实践：计算机中的图像

5.3.1 OpenCV的基本使用方法

下面通过一个演示程序来理解：在OpenCV中图像是如何存取，我们又是如何访问其中的像素的。

#include <iostream>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
int main(int argc, char **argv) {
    
    
	// 读取argv[1]指定的图像
	cv::Mat image;
	image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

	//判断图像文件是否正确读取
	if(image.data == nullptr){
    
     //数据不存在，可能是文件不存在
		cerr << "文件" << argv[1] << "不存在." << endl；
		return 0;
	}

	//文件顺利读取，首先输出一些基本信息
	cout << "图像宽为" << image.cols << "，高为" <<image.rows << "，通道数为" << image.channels() << endl;
	cv::imshow("image",image); //用cv::imshow显示图像
	cv::waitKey(0); 		   //暂停程序，等待一个按键输入

	// 判断image的类型
	if (image.type() != CV_8UC1 && image.type() != CV_8UC3 ) {
    
    
	// 图像类型不符合要求
	cout << "请输入一张彩色图或灰度图." << endl;
	return 0;
	}

	// 遍历图像，请注意以下遍历方式也可使用于随即像素访问
	// 使用std::chrono给算法计时
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	for (size_t y = 0;y < image.rows; y++){
    
    
		// cv::Mat::ptr获得图像的行指针
		unsigned char *row_ptr = image.ptr<unsigned char>(y);  //row_ptr是第y行的头指针
		for (size_t x = 0; x < image.cols; x++) {
    
    
			// 访问位于x,y处的像素
			unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr指向待访问的像素数据
			// 输出该像素的每个通道，如果是灰度图就只有一个通道
			for (int c = 0;c != image.channels(); c++) {
    
    
				unsigned char data = data_ptr[c]; // data为I[x,y]第c个通道的值
			}
		}
	}
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast < chrono::duration< double >>(t2 - t1);
	cout << "遍历图像用时：" << time_used.count() << " 秒。 " endl;

	// 关于cv:Mat的拷贝
	// 直接赋值并不会拷贝数据
	cv::Mat image_another = image;
	// 修改image_another会导致image发生变化
	image_another(cv::Rect(0,0,100,100)).setTo(0); // 将左上角100*100的块置零
	cv::imshow("image", image);
	cv::waitKey(0);

	//使用clone函数拷贝数据
	cv::Mat image_clone = image.clone();
	image_clone(cv::Rect(0,0,100,100)).setTo(255);
	cv::imshow("image", image);
	cv::imshow("image_clone", image_clone);
	cv::waitKey(0);
	
	//对于图像还有很多基本的操作，如剪切、旋转、缩放等，详情请参考OpenCV官方文档介绍的函数使用方法
	cv::destroyAllWindows();
	return 0;
}

在该例程中，我们演示了如下几个操作：图像读取、显示、像素遍历、复制、赋值等。大部分的注解已写在代码里。编译该程序时，你需要在CMakeList.txt添加OpenCV的头文件，然后把程序链接到库文件上。同时，由于使用了C++11标准(如nullptr和chrono)，还需要设置编译器：
slambook/ch5/imageBasics/CMakeLists.txt

// 添加C++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )

// 寻找OpenCV库
find_package( OpenCV REQUIRED)
// 添加头文件
include_directories( ${
    
    OpenCV_INCLUDE_DIRS})

add_executable( imageBasics imageBasics.cpp )
// 链接OpenCV库 
target_link_libraries( imageBasics ${
    
    OpenCV_LIBS} )

关于代码，我们给出几点说明：

程序从argv[1]，也就是命令行的第一个参数中读取图像位置。程序中自备了一张Ubuntu.png供测试使用。因此，编译之后，使用如下命令调用此程序：
build/imageBasics ubuntu.png
如果在IDE中调用此程序，则请务必确保把参数同时给它。者可以在启动项中配置。
在程序的10~18行，使用cv::imread函数读取图像，并把图像和基本信息显示出来
在程序的35~47行，遍历了图像中的所有像素，并计算了整个循环所用的时间。请注意像素的遍历方式并不是唯一的，而且例程给出的方式也不是最高校的。OpenCV提供了迭代器，你可以通过迭代器遍历图像的像素。或者，cv::Mat::data提供了指向图像数据开头的指针，你可以直接通过该指针自行计算偏移量，然后得到像素的实际内存位置。例程所用的方式是为了便于读者理解图像的结构。
作者的机器上（虚拟机），遍历这张图像用时大约12.74毫秒。读者可以在自己机器上做实验。我们使用的是cmake默认的debug模式，如果使用release模式会快很多。
OpenCV提供了许多对图像进行操作的函数，我们在此不一一列举，否则本书就会变成OpenCV操作手册。例程给出了较为常见的读数、显示操作，以及复制图像中可能陷入深拷贝误区。在编程过程中，读者还会碰到图像的旋转、插值等操作，这时你应该自行查阅函数对应的文档，以了解它们的原理与使用方式。

应该指出，OpenCV并不是唯一的图像库，它只是许多图像库里使用范围较广泛的一个。不过，多数图像库对图像的表达是大同小异的。我们希望读者了解了OpenCV对图像的表示后，能够理解其他库中图像的表达，从而在需要数据格式时能够自己处理。另外，由于cv::Mat也是矩阵类，除了表示图像，我们也可以用它来存储位姿等矩阵数据。一般认为，Eigen对于固定大小的矩阵使用起来效率更高。

5.3.2 图像去畸变

在理论部分我们介绍了径向和切向畸变，下面来演示一个去畸变过程。OpenCV提供了去畸变函数cv::Undistort()，但本例我们从公式出发计算畸变前后的图像坐标。

#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "./distorted.png" //请确保路径正确

int main(int argc, char **argv) {
    
    
	// 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
	// 畸变参数
	double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

	cv::Mat image = cv::imread(image_file, 0); // 图像是灰度图，CV_8UC1
	int rows = image.rows, cols = image.cols;
	cv:Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸变以后的图

	// 计算去畸变后图像的内容
	for (int v = 0; v < rows; v++) {
    
    
		for (int u = 0; u < cols; u++) {
    
    
			// 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
			double x = (u - cx) / fx, y = (v - cy) / fy;
			double r = sqrt(x * x + y * y);
			double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
			double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
			double u_distorted = fx * x_distorted + cx;
			double v_distorted = fx * y_distorted + cy;
	
			// 赋值（最近邻插值）
			if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows){
    
    
				image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int)u_distorted);
			}
			else {
    
    
				image_undistort.at<uchar>(v, u) = 0;
			}
		}	
	}

	// 画出去畸变后图像
	cv::imshow("distorted", image);
	cv::imshow("undistorted", image_undistort);
	cv::waitKey();
	return 0;
}

5.4 实践：3D视觉

5.4.1 双目视觉

我们已经介绍了双目视觉的成像原理。现在我们从双目视觉的左右图像出发，计算图像对应的视差图，然后计算各像素在相机坐标系下的坐标，它们将构成点云。在第5讲代码目录的stereo文件夹中。部分代码如下：

int main(int argc, char ** argv) {
    
    
	// 内参
	double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
	// 基线
	double b = 0.573;
	// 读取图像
	cv::Mat left = cv::imread(left_file, 0);
	cv::Mat right = cv::imread(right_file, 0);
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0,96,9,8 * 9 * 9, 32 * 9 * 9,1,63,10,100,32); // 神奇的参数
	cv::Mat disparity_sgbm, disparity;
	sgbm->compute(left, right, disparity_sgbm);
	disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

	//生成点云
	vector<Vector4d, Eigen::aligned_allocator<Vector4d>>pointcloud;
	
	// 如果你的机器慢， 请把后面的v++和u++改成v+=2，u+=2
	for (int u = 0;u < left.cols; u ++){
    
    
		if (disparity.at<float>(v,u) <= 10.0 || disparity.at<float>(v,u) >= 96.0)
		continue;

		Vector4d point(0, 0, 0, left.at<uchar>(v, u)/255.0); //前三维为xyz,第四维为颜色

		//根据双目模型计算point的位置
		double x = （u - cx）/ fx;
		double y = （v - cy）/ fy;
		double depth = fx * b / (disparity.at<float>(v,u));
		point[0] = x * depth;
		point[1] = y * depth;
		point[2] = depth;

		pointcloud.push_back(point);
	}

	cv::imshow("disparity", disparity / 96.0);
	cv::waitKey(0);
	// 画出点云
	showPointCloud(pointcloud);
	return 0;
}

这个例子中我们调用了OpenCV实现的SGBM（Semi-Global Batch Matching）算法计算左右图像的视差，然后通过双目相机的几何模型把它变换到相机的3D空间中。SGBM使用了来自网络的经典参数配置，我们主要调整了最大和最小视差。视差数据结合相机的内参、基线，即能确定各点在三维空间中的位置。
本文不打算展开介绍双目相机的视差计算算法，感兴趣的读者可以阅读相关的参考文献。除了OpenCV实现的双目算法，还有许多其他的库专注于实现高效的视差计算。这是一个复杂又实用的课题。

5.4.2 RGB-D视觉

最后，我们演示了RGB-D视觉的例子。RGB-D相机的方便之处在于能通过物理方法获得像素深度信息。如果已知相机的内外参，我们就可以计算任何一个像素在世界坐标系下的位置，从而建立一张点云地图。现在我们就来演示。
我们准备了5对图像，位于slambook/ch5/rgbd文件夹中。在color/下有1.png到5.png共5张RGB图，而在depth/下有5张对应的深度图。同时，pose.txt文件给出了5张图像的相机外参位姿（以 $T_{wc}$ 形式）。位姿记录的形式和之前一样，为平移向量加旋转四元数：

$x,y,z,q_x,q_y,q_z,q_w],$

其中， $q_w$ 是四元数的实部。例如，第一对图的外参为：

$[- 0.228993, 0.0065704, 0.0287837, - 0.0004327, - 0.113131, - 0.0326832, 0.993042] .$

下面我们写一段程序，完成两件事：
(1)根据内参计算一对RGB-D图像对应的点云
(2)根据各张图的相机位姿（也就是外参），把点云加起来，组成地图。

int main(int argc, char **argv) {
    
    
	vector<cv::Mat> colorImgs, depthImgs; //彩色图和深度图
	TrajectoryType poses; // 相机位姿

	ifstream fin("./pose.txt");
	if (!fin) {
    
    
		cerr << "请在有pose.txt的目录下运行此程序" << endl;
		return 1;
	}
	for (int i = 0; i < 5; i++) {
    
    
		boost::format fmt("./%s/%d.%s"); //图像文件格式
		colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
		depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(),-1)); // 使用-1读取原始图像

		double data[7] = {
    
    0};
		for (auto &d:data) fin >> d;
		Sophus::SE3d pose(Eigen::Quaterniond(data[6]), data[3], data[4], data[5]),Eigen::Vector3d(data[0], data[1], data[2]));
		poses.push_back(pose);
	}

	// 计算点云并拼接
	// 相机内参
	double cx = 325.5;
	double cy = 253.5;
	double fx = 518.0;
	double fy = 519.0;
	double depthScale = 1000.0;
	vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
	pointcloud.reserve(1000000);

	for (int i = 0; i < 5; i ++) {
    
    
		cout << "转换图像中：" << i + 1 << endl;
		cv::Mat color = colorImgs[i];
		cv::Mat depth = depthImgs[i];
		Sophus::SE3d T = poses[i];
		for (int v = 0; v < color.rows; v++)
			for (int u = 0;u < color.cols;u++) {
    
    
				unsigned int d = depth.ptr<unsigned short>(v)[u]; //深度值
				if (d == 0) continue; // 为0表示没有测量到
				Eigen::Vector3d point;
				point[2] = double(d) / depthScale;
				point[0] = (u - cx) * point[2] / fx;
				point[1] = (v - cy) * point[2] / fy;
				Eigen::Vector3d pointWorld = T * point;

				Vector6d p;
				p.head<3>() = pointWorld;
				p[5] = color.data[v * color.step + u * color.channels()]; //blue
				p[4] = color.data[v * color.step + u * color.channels() + 1]; //green
				p[3] = color.data[v * color.step + u * color.channels() + 2]; //red
				pointcloud.push_back(p);
			}
	}
	
	cout << "点云共有" << pointcloud.size() << "个点." << endl;
	showPointCloud(pointcloud);
	return 0;
}

在这里插入图片描述

运行程序后即可在Pangolin窗口中看到拼合的点云地图。可以拖动鼠标查看。
通过这些例子，我们演示了计算机视觉中一些常见的、单目、双目和深度相机算法。希望能体会到内外参、畸变参数的含义。

课后习题

1. *寻找一个相机（你手机或笔记本的摄像头即可），标定它的内参。你可能会用到标定板，或者自己打印一张标定用的棋盘格。
参考我的另一篇: 相机标定的博文

2. 叙述相机内参的物理意义。如果一个相机的分辨率变成两倍而其他地方不变，它的内参如何变化？
参考 @清欢_ 的一篇CSDN博文相机内参的物理意义
以及参考 nullwh 的一篇code world博文《视觉SLAM十四讲第二版》笔记及课后习题（第五讲)
相机内参的物理意义：把相机坐标系下3D空间点投影到像素坐标系中，相机内参K:
$\begin{pmatrix}\alpha f&0&c_x\\\\0&\beta f&c_y\\\\0&0&1\end{pmatrix} = \begin{pmatrix}f_x&0&c_x\\\\0&f_y&c_y\\\\0&0&1\end{pmatrix}$
其中f的单位为米， $\alpha,\beta$ 的单位为像素/米，所以 $f_x,f_y$ 的单位为像素。

相机的投影公式为：
$\begin{cases}u = f_x\frac{X}{Z}+c_x\\v=f_y\frac{Y}{Z}+c_y\end{cases}$
u的单位为像素，其物理意义为世界坐标系下的点在像素平面的投影。所以当分辨率增加一倍，u,v也就变为原来的两倍。为了保证恒等，则等式右边也得乘以二，然而三维世界坐标并未变化，所以焦距的值和主点的坐标值也会增加一倍。亦即 $f_x、f_y$ 变为原来的两倍。

3. 搜索特殊的相机（鱼眼或全景）相机的标定方法。它们与普通的针孔模型有何不同？
鱼眼相机参考 Chogori-Ma 的一篇CSDN博文鱼眼相机标定以及OpenCV实现

鱼眼相机标定:有时候也可以用普通相机的标定方法对其进行标定，但是却不能保证去畸变后的效果是最好的。因此对于Gopro等鱼眼镜头拍摄出来的图像去畸变，最好的方法就是采用鱼眼相机标定方法进行标定。
与它普通镜头的成像模型没有区别。两者之间的区别主要体现在畸变系数，鱼眼相机的畸变系数为 ${ k_1,k_2,k_3,k_4\}$ ，畸变系数不同，就导致鱼眼相机的投影关系也发生了变化，主要变化发生在考虑畸变情况下的投影关系转化。

全景相机参考 苏源流 的一篇CSDN博文计算机视觉全景相机标定（MATLAB/opencv）
全景相机标定:图像变形非常大，全景相机的是一个像素球面。其标定参数因相机而定，但是大部分的相机标定还是分为去畸变和像素面投影两步。

4. 调研全局快门相机（global shutter）和卷帘快门相机（rolling shutter）的异同。它们在SLAM 中有何优缺点？
参考 abcwoabcwo 的一篇CSDN博文详细图解，一眼就能看懂！卷帘快门（Rolling Shutter）与全局快门（Global Shutter）的区别

该篇博文讲的十分详细，对于优缺点总结如下：
Global shutter 曝光时间更短，但会增加读出噪声；
对于相机厂家，Rolling shutter可以达到更高的帧速，但当曝光不当或物体移动较快时，会出现部分曝光(partial exposure)、斜坡图形(skew)、晃动(wobble) 等现象。这种Rolling shutter方式拍摄出现的现象，就定义为果冻效应。
曝光时间短的应用（如<500μs）适合Global shutter，曝光时间长（如大于500μs）时，选择rolling shutter可以有更低的噪声和更高的帧速。
在slam中，低速情况下两者区分不大，但是全局相机目前是一种趋势适用于各种场合。

5. RGB-D 相机是如何标定的？以Kinect 为例，需要标定哪些参数？（参照https://github.com/code-iai/iai_kinect2.）

参考 凌晨四点天使之城 的一篇CSDN博文RGB-D相机的标定与图像配准
RGB-D相机一般配备彩色相机和红外相机，分别获取外观以及深度信息。所以需要标定以下参数：

彩色相机内参： $f_{x_{rgb}},f_{y_{rgb}},c_{x_{rgb}},c_{y_{rgb}}$
彩色相机径向畸变参数： $k_{1_{rgb}},k_{2_{rgb}},k_{3_{rgb}}$
彩色相机切向畸变参数： $p_{1_{rgb}},p_{2_{rgb}}$
红外/深度相机内参： $f_{x_{ir}},f_{y_{ir}},c_{x_{ir}},c_{y_{ir}}$
红外/深度相机径向畸变参数： $k_{1_{ir}},k_{2_{ir}},k_{3_{ir}}$
红外/深度相机切向畸变参数： $p_{1_{ir}},p_{2_{ir}}$
彩色相机和红外相机之间的位姿变换(外参)： $R_{rgb2ir},t_{rgb2ir}$
深度图像中深度值校准： $d_{shifted} = ad + \beta$

RGB-D相机的标定是采用ros环境下的lai_kinect2的配准工具包，具体的步骤博客中介绍的十分详细，在此不予赘述。（大致与普通的相机标定类似，只不过要考虑两个相机的内参以及两者之间的外参或称位姿变换）

6.除了示例程序演示的遍历图像的方式，你还能举出哪些遍历图像的方法？

参考 CoolBare 的一篇CSDN博文遍历图像像素的14种方法
主要有这几种：

一个一个点读取的方法：image.at<>(i,j)（类似数组元素的遍历）;
指针遍历：Mat图像在内存中连续排布可以用 image.Ptr<>()指针遍历；(用的最多)
迭代器遍历：利用cv里的iterator进行遍历，取出头cimage.begin()和尾cimage.end()。

7. *阅读OpenCV官方教程，学习它的基本用法。
OpenCV官网建议科学上网，不然可能会有点慢。