视觉SLAM⑤--相机与图像

5.0 本章简介

5.1 相机模型

5.1.1针孔相机模型

5.1.2 畸变模型

5.1.3 双目相机模型

5.1.4 RGB-D相机模型

5.2 图像

5.3 实践：计算机中的图像

5.3.1 OpenCV的基本使用方法

5.3.2 图像去畸变

5.4 3D视觉

5.4.1 双目视觉

5.4.2 RGB-D视觉

5.0 本章简介

主要目标：
①理解针孔相机的模型、内参与径向畸变参数

②理解一个空间点是如何投影到相机成像平面的

③掌握 OpenCV的图像存储与表达方式

④学会基本的摄像头标定方法。

⑤修改该章代码的错误并让其顺利运行

前面介绍了“机器人如何表示自身位姿”的问题，解释了SLAM经典模型中变量的含义和运动方程部分。

本讲将讨论“机器人如何观测外部世界”，也就是观测方程部分。而在以相机为主的视觉SLAM 中，观测主要是指相机成像的过程。
将首先讨论相机模型，说明投影关系具体如何描述，相机的内参是什么。同时，简单介绍双目成像与RGB-D相机的原理。然后，介绍二维照片像素的基本操作。最后，根据内外参数的含义，演示一个点云拼接的实验。

5.1 相机模型

相机将三维世界中的坐标点(单位为米)映射到二维图像平面(单位为像素)的过程能够用一个几何模型进行描述。

这个模型有很多种，其中最简单的称为针孔模型。针孔模型是很常用而且有效的模型，它描述了一束光线通过针孔之后，在针孔背面投影成像的关系，由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变。因此，我们使用针孔和畸变两个模型来描述整个投影过程。
先介绍相机的针孔模型，再对透镜的畸变模型进行讲解。这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数(Intrinsics)。

5.1.1针孔相机模型

图5-1 初中时候讲的小孔成像

设相机坐标系，让轴指向相机前方，轴向右，轴向下(此图我们应该站在左侧看右侧)。为摄像机的光心，也是针孔模型中的针孔。

现实世界的空间点，经过小孔投影之后，落在物理成像平面上，成像点为。设的坐标为，为，并且设物理成像平面到小孔的距离为(焦距）。那么，根据三角形相似关系，有

式5-1 相似三角形的相似关系

负号表示成的像是倒立的。不过，实际相机得到的图像并不是倒像。

把成像平面对称地放到相机前方，和三维空间点一起放在摄像机坐标系的同一侧。这样做可以把公式中的负号去掉：

图5-2 等价地把成像平面对称地放到相机前方

在相机中，我们最终获得的是一个个的像素，这还需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面。我们在像素平面得到了的像素坐标：

像素坐标系：原点位于图像的左上角，轴向右与轴平行，轴向下与轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。

我们设像素坐标在轴上缩放了倍，在轴上缩放了倍。同时，原点平移了。那么，的坐标与像素坐标的关系为：

式5-2 像素坐标系与成像坐标系关系

图5-3 像素坐标系与成像坐标系关系

代入式5-1并把合并成，把合并成，得

式5-3 焦距、放大倍数、成像平面坐标、偏移量与像素矩阵关系

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

式5-4 将式5-4写成矩阵形式

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

式5-5 改进式5-4

该式中，我们把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics)矩阵K。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。
有内参，自然也有相对的外参。在式5-5中，我们使用的是在相机坐标系下的坐标。

但实际上由于相机在运动，所以P的相机坐标应该是它的世界坐标(记为)根据相机的当前位姿变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵和平移向量来描述。那么有：

式5-6 先把P从世界坐标变到相机坐标系下

(式子解释：是像素坐标系下的坐标，是世界坐标系下的坐标，这里的实际上是，将世界坐标系转换为相机坐标系再左乘以相机内参数矩阵获得相机的像素坐标。）

注意后一个式子隐含了一次齐次坐标到非齐次坐标的转换。(三维是而是四维)

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

(式子解释：中，两边统统除以Z，左边就是，右边除以Z变成，关键是除以Z有什么用呢？打开你的短视频平台，观看视频，你一定不知道拍视频的这个人和他家地面的（不能估算）距离吧！，那估计不了就不估计了，都当成距离相机1m吧！)

图5-4 归一化处理-同一直线上的投影点仍是同一个

式5-7 归一化处理

归一化坐标可看成相机前方处的平面上的一个点，这个平面也称为归一化平面。

归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标看成对归一化平面上的点进行量化测量的结果。

从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

式5-8 总结这一节说了什么

5.1.2 畸变模型

1.畸变模型

为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入会对成像过程中光线的传播产生新的影响：

        一是透镜自身的形状对光线传播的影响。

        二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像面时的位置发生变化。
        由透镜形状引起的畸变（Distortion，也叫失真）称为径向畸变。

在针孔模型中，一条直线投影到像素平面上还是一条直线。

可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称它们主要分为两大类：桶形畸变和枕形畸变
        桶形畸变图像放大率随着与光轴之间的距离增加而减小而枕形畸变则恰好相反。

        在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
        除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行,所以也会引人切向畸变。

图5-5-1 桶形失真和枕形失真

图5-5-2 镜头畸变

图5-6 切向畸变

为了更好地理解径向畸变和切向畸变,我们用更严格的数学形式对两者进行描述。考虑归一化平面上的任意一点，它的坐标为，也可写成极坐标的形式，其中表示点与坐标系原点之间的距离，表示与水平轴的夹角。

径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。

切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

通常假设这些畸变呈多项式关系，即：

式5-9 径向畸变：多项式描述

其中，是畸变后点的归一化坐标。另外，对于切向畸变，可以便用另外两个参数进行纠正：

式5-10 切向畸变：多项式描述

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

（注意，式中的是失真即修正前像素坐标，为修正后的像素坐标，像素坐标是先归一化再进行去畸变再计算成像素坐标的，切记！）

式5-11 放在一起：实际当中可灵活保留各项系数

2.总结：单目相机的成像过程

在上面的纠正畸变的过程中，我们使用了5个畸变项。实际应用中，可以灵活选择纠正模型，比如只选择这3项。
        在本节中，我们对相机的成像过程使用针孔模型进行了建模，也对透镜引起的径向畸变和切向畸变进行了描述。
        存在两种去畸变处理(Undistort，或称畸变校正)做法。

我们可以选择先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。或者，也可以从畸变图像上的某个点出发，按照畸变方程，讨论其畸变前的空间位置。二者都是可行的，不过前者在视觉SLAM中似乎更常见。所以，当一个图像去畸变之后，我们就可以直接用针孔模型建立投影关系，而不用考虑畸变。因此，在后文的讨论中，我们可以直接假设图像已经进行了去畸变处理。

①将三维空间点投影到归一化图像平面。设它的归一化坐标为。

②对归一化平面上的点计算径向畸变和切向畸变。
③将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。




图5-7 总结：单目相机的成像过程

综上所述，我们一共谈到了四种坐标：世界坐标、相机坐标、归一化坐标和像素坐标。请厘清它们的关系，它反映了整个成像的过程。

5.1.3 双目相机模型

针孔相机模型描述了单个相机的成像模型。然而，仅根据一个像素，我们无法确定这个空间点的具体位置（仅凭照过的照片，无法还原当时的3D情况）。

这是因为，从相机光心到归一化平面连线上的所有点，都可以投影至该像素上。只有当Р的深度确定时（比如通过双目或RGB-D相机)，我们才能确切地知道它的空间位置，如图5-4所示。

测量像素距离(或深度)的方式有很多种，比如人眼就可以根据左右眼看到的景物差异(或称视差)判断物体与我们的距离。双目相机的原理亦是如此：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。下面简单介绍双目相机的成像原理(如图5-8所示)。

图5-8 双目相机的成像原理

双目相机的成像模型如上。

为左右光圈中心，方框为成像平面，为焦距。和为成像平面的坐标(以为原点建立坐标系按照图中坐标定义，应该是负数，所以图中标出的距离为)。

双目相机一般由左眼相机和右眼相机两个水平放置的相机组成。当然也可以做成上下两个目，不过我们见到的主流双目都是做成左右形式的。

在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的,意味着两个相机的光圈中心都位于轴上。两者之间的距离称为双目相机的基线(记作)，是双目相机的重要参数。
        现在，考虑一个空间点，它在左眼相机和右眼相机各成一像，记作。由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只在轴上有位移，所以P的像也只在轴(对应图像的轴)上有差异。记它的左侧坐标为，右侧坐标为，几何关系如图5-8右侧所示。根据和的相似关系，有



式5-12-1 双目相机成像几何关系

其中定义为左右图的横坐标之差，称为视差。根据视差，我们可以估计一个像素与相机之间的距离。

        视差与距离成反比：视差越大，距离越近。(单闭上一个眼睛看自己手指头和远方物体)

同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由确定。我们看到，基线越长，双目能测到的最大距离就越远；反之，小型双目器件则只能测量很近的距离。

相似地，我们人眼在看非常远的物体时（如很远的飞机)，通常不能准确判断它的距离。
        虽然由视差计算深度的公式很简洁，但视差本身的计算却比较困难。我们需要确切地知道左眼图像的某个像素出现在右眼图像的哪一个位置(即对应关系)，这件事也属于“人类觉得容易而计算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGA来实时计算。

5.1.4 RGB-D相机模型

相比于双目相机通过视差计算深度的方式，RGB-D相机的做法更“主动”，它能够主动测量每个像素的深度。目前的RGB-D相机按原理可分为两大类：
①通过红外结构光(Structured Light)原理测量像素距离。例子有Kinect 1代、Project Tango 1代、Intel ReaSense等。
②通过飞行时间(Time-of-Flight，ToF)原理测量像素距离。例子有Kinect 2代和一些现有的ToF传感器等。

图5-9 RGB-D 相机原理示意图

无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线(通常是红外光)。

        在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。

而在ToF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。(类似蝙蝠的超声波定位吧)

        ToF的原理和激光传感器十分相似，只不过激光是通过逐点扫描获取距离的，而ToF相机则可以获得整个图像的像素深度，这也正是RGB-D相机的特点。所以，如果把一个RGB-D相机拆开，通常会发现除了普通的摄像头，至少会有一个发射器和一个接收器。
        在测量深度之后，RGB-D相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云(Point Cloud)。既可以在图像层面对RGB-D数据进行处理，也可在点云层面处理。本讲的第二个实验将演示RGB-D相机的点云构建过程。
        RGB-D相机能够实时地测量每个像素点的距离。但是，由于使用这种发射–接收的测量方式，其使用范围比较受限。用红外光进行深度值测量的RGB-D相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。

在没有调制的情况下，同时使用多个RGB-D相机此时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D相机在成本、功耗方面都有一些劣势。

5.2 图像

1.引出问题

相机加上镜头，把三维世界中的信息转换成了一张由像素组成的照片，随后存储在计算机中，作为后续处理的数据来源。

在数学中，图像可以用一个矩阵来描述；而在计算机中，他们占据一段连续的磁盘或内存空间，可以用二维数组来表示。这样一来，程序就不必区别它们处理的是一个数值矩阵，还是有实际意义的图像了。
本节，我们将介绍计算机图像处理的一些基本操作。特别地，通过OpenCV中图像数据的处理，理解计算机中处理图像的常见步骤，为后续章节打下基础。我们从最简单的图像——灰度图说起。

2.灰度图

在一张灰度图中，每个像素位置对应一个灰度值，所以，一张宽度为、高度为的图像，数学上可以记为一个函数：

其中，是像素的坐标。然而，计算机并不能表达实数空间，所以我们需要对下标和图像读数在某个范围内进行量化。例如，通常是从0开始的整数。在常见的灰度图中，用0~255的整数(即一个unsigned char,1个字节)来表达图像的灰度读数。那么，一张宽度为640像素、高度为480像素分辨率的灰度图就可以表示为：
unsigned char image[480][640];
为什么这里的二维数组是480×640呢？

因为在程序中，图像以二维数组形式存储。它的第一个下标是指数组的行，而第二个下标则是列。在图像中，数组的行数对应图像的高度，而列数对应图像的宽度。
当访问某一个像素时，需要指明它所处的坐标，如图5-10所示。

该图左边显示了传统像素坐标系的定义方式。像素坐标系原点位于图像的左上角，X轴向右，Y轴向下(也就是前面所说的u, v坐标)。如果它还有第三个轴——Z轴，那么根据右手法则，Z轴应该是向前的。这种定义方式是与相机坐标系一致的。我们平时说的图像的宽度或列数，对应着X轴；而图像的行数或高度则对应着它的Y轴。
根据这种定义方式，如果我们讨论一个位于处的像素，那么它在程序中的访问方式应该是：
unsigned char pixel = image[y][x];
它对应着灰度值的读数。请注意这里的和的顺序。如果在写程序时不慎调换了的坐标，编译器无法提供任何信息，所能看到的只是程序运行中的一个越界错误而已。

图5-10 图像坐标示意图

图5-11 像素坐标系与灰度值矩阵的坐标关系

3.灰度图、深度图、彩色图像的表示方法

一个像素的灰度可以用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的基本使用方法

1.代码

#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;
}

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

另外注意！按照书上的CMakeLists.txt是跑不起来的！会出现一大堆找不到opencv的引用问题，主要原因是缺少相应的链接，找不到opencv函数库。

CMakeLists.txt文件修改如下：

cmake_minimum_required( VERSION 2.8 )
project(imageBasics)set( CMAKE_CXX_FLAGS "-std=c++11 -O3")
find_package(OpenCV)
include_directories(${OpenCV})add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})

这时执行：可以正常运行

liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch5/imageBasics/build$ cmake ..
-- Found OpenCV: /usr/local (found version "4.4.0")
-- Configuring done
-- Generating done
-- Build files have been written to: /home/liuhongwei/桌面/slambook2/ch5/imageBasics/build
liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch5/imageBasics/build$ make
Scanning dependencies of target imageBasics
[ 25%] Building CXX object CMakeFiles/imageBasics.dir/imageBasics.cpp.o
[ 50%] Linking CXX executable imageBasics
[ 50%] Built target imageBasics
Scanning dependencies of target undistortImage
[ 75%] Building CXX object CMakeFiles/undistortImage.dir/undistortImage.cpp.o
[100%] Linking CXX executable undistortImage
[100%] Built target undistortImage

运行结果：

2.代码补充说明

关于代码，我们给出几点说明:
①程序从 argv[1]，也就是命令行的第一个参数中读取图像位置。我们为读者准备了一张图
像(ubuntu.png，一张Ubuntu的壁纸)供测试使用。因此，编译之后，使用如下命令调用此程序:
终端输入:
build/imageBasics ubuntu.png
如果在IDE中调用此程序，则请务必确保把参数同时给它。

②在程序的10~18行，使用cv::imread 函数读取图像，并把图像和基本信息显示出来。

③在程序的35~47行，遍历了图像中的所有像素，并计算了整个循环所用的时间。请注意
像素的遍历方式并不是唯一的，而且例程给出的方式也不是最高效的。

OpenCV提供了迭代器，你可以通过迭代器遍历图像的像素。或者，cv::Mat::data提供了指向图像数据开头的指针，你可以直接通过该指针自行计算偏移量，然后得到像素的实际内存位置。例程所用的方式是为了便于读者理解图像的结构。
④OpenCV提供了许多对图像进行操作的函数。例程给出了较为常见的读取、显示操作，以及复制图像中可能陷入的深拷贝误区。在编程过程中，读者还会碰到图像的旋转、插值等操作，这时你应该自行查阅函数对应的文档，以了解它们的原理与使用方式。应该指出，OpenCV并不是唯一的图像库，它只是许多图像库里使用范围较广泛的一个。不过，多数图像库对图像的表达是大同小异的。我们希望读者了解了OpenCV对图像的表示后，能够理解其他库中图像的表达，从而在需要数据格式时能够自己处理。另外，由于cv::Mat 也是矩阵类，除了表示图像，我们也可以用它来存储位姿等矩阵数据。一般认为，Eigen对于固定大小的矩阵使用起来效率更高。

5.3.2 图像去畸变

1.代码及问题及解决

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

#include <opencv2/opencv.hpp>
#include <string>using namespace std;string image_file = "/home/liuhongwei/桌面/slambook2/ch5/imageBasics/distorted.png";   // 请确保路径正确int main(int argc, char **argv) {// 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。// 畸变参数double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;// 内参double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1int 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 = fy * 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;
}

另外注意！按照书上的代码跑不起来的！会出现莫名其妙的错误。

这时因为路径问题，请屏幕前的你想一想，打开你的虚拟机/双系统，你的文件结构是这样的：

build文件里放着undistortImage文件，即编译好要执行的程序，那么如果是按照他书上写得文件路径 ./distorted.png，但是build里面并没有这个文件......，因此报错，于是，为了保险起见，请把文件路径改成绝对路径。

string image_file = "/home/liuhongwei/桌面/slambook2/ch5/imageBasics/distorted.png";   // 请确保路径正确

2.代码执行结果

改完之后程序就能跑起来了，程序运行结果如下：

图5-16 运行结果

5.4 3D视觉

5.4.1 双目视觉

1.代码及问题及解决

我们已经介绍了双目视觉的成像原理。现在我们从双目视觉的左右图像出发，计算图像对应的视差图，然后计算各像素在相机坐标系下的坐标，它们将构成点云。

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>using namespace std;
using namespace Eigen;// 文件路径
string left_file = "/home/liuhongwei/桌面/slambook2/ch5/stereo/left.png";
string right_file = "/home/liuhongwei/桌面/slambook2/ch5/stereo/right.png";// 在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);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+=2for (int v = 0; v < left.rows; v++)for (int u = 0; u < left.cols; u++) {if (disparity.at<float>(v, u) <= 0.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;
}void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {if (pointcloud.empty()) {cerr << "Point cloud is empty!" << endl;return;}pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glPointSize(2);glBegin(GL_POINTS);for (auto &p: pointcloud) {glColor3f(p[3], p[3], p[3]);glVertex3d(p[0], p[1], p[2]);}glEnd();pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}return;
}

同样，他给我们的代码还是跑不起来，我的机器报了很多错，为什么呢？我们一一解释

报这个错的原因是他让我们安装的opencv包是opencv3，但是他自己的机器安装的opencv4，电脑上当然找不到包（难道凭空变出来）？解决方法，头文件改成opencv2。

#include <opencv2/opencv.hpp>

错误2：代码报错

这个是因为路径设置错误，上一节已经阐述明白，故不做多解释。

2.代码执行结果

图5-19 双目视觉实践结果

这个例子中我们调用了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张图像的相机外参位姿(以形式)。位姿记录的形式和之前一样，为平移向量加旋转四元数:

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

下面我们写一段程序，完成两件事：
(1）根据内参计算一对RGB-D图像对应的点云。
(2）根据各张图的相机位姿（也就是外参)，把点云加起来，组成地图。
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;// 在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);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()];   // bluep[4] = color.data[v * color.step + u * color.channels() + 1]; // greenp[3] = color.data[v * color.step + u * color.channels() + 2]; // redpointcloud.push_back(p);}}cout << "点云共有" << pointcloud.size() << "个点." << endl;showPointCloud(pointcloud);return 0;
}void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {if (pointcloud.empty()) {cerr << "Point cloud is empty!" << endl;return;}pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glPointSize(2);glBegin(GL_POINTS);for (auto &p: pointcloud) {glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);glVertex3d(p[0], p[1], p[2]);}glEnd();pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}return;
}
运行程序后即可在 Pangolin窗口中看到拼合的点云地图(如图5-20所示)。

图5-20 拼合的点云地图

通过这些例子，我们演示了计算机视觉中一些常见的单目、双目和深度相机算法。通过这些简单的例子，体会相机内外参、畸变参数的含义。