slam入门——十四讲笔记（四）

第五讲 相机与图像

【本节目标】

1. 理解针孔相机模型、内参与径向畸变参数
2. 理解一个空间点是如何投影到相机成像平面的
3. 掌握OpenCV的图像存储与表达方式
4. 学会基本的摄像头标定方法

前面两讲中，我们介绍了“机器人如何表示自身位姿”的问题，部分地解释了SLAM经典模型中变量的含义和运动方程部分。本讲，我们要讨论“机器人如何观测外部世界”，也就是观测方程部分。而在以相机为主的视觉SLAM中，观测主要是指相机成像的过程。

本讲，我们首先讨论相机模型，说明投影关系具体如何描述，相机的内参是什么。同时，简单介绍双目成像与RGB-D相机的原理。然后，介绍二维照片像素的基本操作。最后，我们根据内外参数的含义，演示一个点云拼接的实验。

5.1 相机模型

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

5.1.1 针孔相机模型

现在来对这个简单的针孔模型进行几何建模。设 $O-x-y-z$ 为相机坐标系，习惯上我们让 $z$ 轴指向相机前方，$x$ 向右，$y$ 向下。$O$ 为摄像机的光心，也是针孔模型中的针 孔。现实世界的空间点$P$，经过小孔 $O$ 投影之后，落在物理成像平面 $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 }}(5.1)$$
其中负号表示成的像是倒立的。为了简化模型，我们把可以成像平面对称到相机前方，和 三维空间点一起放在摄像机坐标系的同一侧，如下图中间的样子所示。这样做可以把公 式中的负号去掉，使式子更加简洁：
$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}(5.2)$$

整理得：
$$X^{\prime }=f\frac{X}{Z}{Y}^{\prime }=f\frac{Y}{Z}(5.3)$$
式（5.3）描述了点 $P$ 和它的像之间的空间关系。不过，在相机中，我们最终获得的是 一个个的像素，这需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光 线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面 $o-u-v$。我们 在像素平面得到了 $P^{\prime }$ 的像素坐标：$[u,v{]}^T$.

该式中，我们把中间的量组成的矩阵称为相机的内参数矩阵（Camera Intrinsics）$K$。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。鉴于标定算法业已成熟，且网络上能找到大量的标定教学，我们在此就不介绍了。

除了内参之外，自然还有相对的外参。考虑到在式（5.6）中，我们使用的是 $P$ 在相机坐标系下的坐标。由于相机在运动，所以 $P$ 的相机坐标应该是它的世界坐标（记为 $P_w$）， 根据相机的当前位姿，变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵 $R$ 和平移向量 $t$ 来描述。那么有:

5.1.2 畸变

为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响: 一是透镜自身的形状对光线传播的影响，二是在机械组装过程中， 透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像面时的位置发生变化。
由透镜形状引起的畸变称之为径向畸变。在针孔模型中，一条直线投影到像素平面上 还是一条直线。可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直 线在图片中变成了曲线x。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜 往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类，桶形畸变 和枕形畸变，如图5-3所示。

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。 在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。 除了透镜的形状会引入径向畸变外，在相机的组装过程中由于不能使得透镜和成像面 严格平行也会引入切向畸变。如图 5-4 所示。

为更好地理解径向畸变和切向畸变，我们用更严格的数学形式对两者进行描述。我们知道平面上的任意一点 $p$ 可以用笛卡尔坐标表示为 $[x,y{]}^T$, 也可以把它写成极坐标的形式$[r,\theta {]}^T$，其中 $r$ 表示点 $p$ 离坐标系原点的距离，$\theta$ 表示和水平轴的夹角。径向畸变可看成 坐标点沿着长度方向发生了变化 $\delta r$, 也就是其距离原点的长度发生了变化。切向畸变可以 看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化 $\delta \theta$。 对于径向畸变，无论是桶形畸变还是枕形畸变，由于它们都是随着离中心的距离增加 而增加。我们可以用一个多项式函数来描述畸变前后的坐标变化：这类畸变可以用和距中心距离有关的二次及高次多项式函数进行纠正：

其中 $[x,y{]}^T$ 是未纠正的点的坐标，$[x_{corrected},y_{corrected}{]}^T$ 是纠正后的点的坐标，注意它们 都是归一化平面上的点，而不是像素平面上的点。 在式（5.11）描述的纠正模型中，对于畸变较小的图像中心区域，畸变纠正主要是 $k_1$起作用。而对于畸变较大的边缘区域主要是 $k_2$ 起作用。普通摄像头用这两个系数就能很 好的纠正径向畸变。对畸变很大的摄像头，比如鱼眼镜头，可以加入 $k_3$ 畸变项对畸变进行纠正。
另一方面，对于切向畸变，可以使用另外的两个参数 $p_1,p_2$ 来进行纠正:

因此，联合式（5.11）和式（5.12），对于相机坐标系中的一点 $P(X,Y,Z)$，我们能够 通过五个畸变系数找到这个点在像素平面上的正确位置:

1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为 $[x,y{]}^T$。

2. 对归一化平面上的点进行径向畸变和切向畸变纠正。
   

3. 将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
   
   在上面的纠正畸变的过程中，我们使用了五个畸变项。实际应用中，可以灵活选择纠正模型，比如只选择 $k_1,p_1,p_2$ 这三项等。

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

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

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

5.1.3 双目相机模型

针孔相机模型描述了单个相机的成像模型。然而，仅根据一个像素，我们是无法确定 这个空间点的具体位置的。这是因为，从相机光心到归一化平面连线上的所有点，都可以 投影至该像素上。只有当 $P$ 的深度确定时（比如通过双目或 RGB-D 相机），我们才能确切地知道它的空间位置。
测量像素距离（或深度）的方式有很多种，像人眼就可以根据左右眼看到的景物差异（或称视差）来判断物体与我们的距离。双目相机的原理亦是如此。通过同步采集左右相机的图像，计算图像间视差，来估计每一个像素的深度。下面我们简单讲讲双目相机的成像原理（图 5-6 ）。

双目相机一般由左眼和右眼两个水平放置的相机组成。当然也可以做成上下两个目， 但我们见到的主流双目都是做成左右的。在左右双目的相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的，意味两个相机的光圈中心都位于 $x$ 轴上。它们的距离称为双目相机的基线（Baseline, 记作 b），是双目的重要参数。

现在，考虑一个空间点 $P$，它在左眼和右眼各成一像，记作 $P_L,P_R$。由于相机基线的
存在，这两个成像位置是不同的。理想情况下，由于左右相机只有在 $x$ 轴上有位移，因此 $P$ 的像也只在 $x$ 轴（对应图像的 $u$ 轴）上有差异。我们记它在左侧的坐标为 $u_L$，右侧坐标 为 $u_R$。那么，它们的几何关系如图5-6右侧所示。根据三角形 $P-P_L-P_R$ 和 $P-O_L-O_R$ 的相似关系，有：

这里 $d$ 为左右图的横坐标之差，称为视差（Disparity）。根据视差，我们可以估计一 个像素离相机的距离。视差与距离成反比：视差越大，距离越近$y$。同时，由于视差最小为 一个像素，于是双目的深度存在一个理论上的最大值，由 $fb$ 确定。我们看到，当基线越长 时，双目最大能测到的距离就会变远；反之，小型双目器件则只能测量很近的距离。

虽然由视差计算深度的公式很简洁，但视差 $d$ 本身的计算却比较困难。我们需要确切 地知道左眼图像某个像素出现在右眼图像的哪一个位置（即对应关系），这件事亦属于“人 类觉得容易而计算机觉得困难”的事务。当我们想计算每个像素的深度时，其计算量与精 度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因， 双目深度估计仍需要使用 GPU 或 FPGA 来计算。这将在十三章中提到。

5.1.4 RGB-D相机模型

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

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

5.2 图像

相机加上镜头，把三维世界中的信息转换成了一个由像素组成的照片，随后存储在计算机中，作为后续处理的数据来源。在数学中，图像可以用一个矩阵来描述；而在计算机中，它们占据一段连续的磁盘或内存空间，可以用二维数组来表示。这样一来，程序就不必区别它们处理的是一个数值矩阵，还是有实际意义的图像了。
本节，我们将介绍计算机图像处理的一些基本操作。特别地，通过 OpenCV 中图像数据的处理，理解计算机中处理图像的常见步骤，为后续章节打下基础。

5.2.1 计算机中图像的表示

灰度图一般是0-255 8位 unsigned char类型
深度图一般是0-65535 16位 unsigned short类型。单位是毫米，也可以表示65米的距离了，够用了。
彩色图一般是多通道 比如RGB 3通道 每个通道0-255 所以需要24位

5.3 实践：图像的存取与访问

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

5.3.1 安装OpenCV

OpenCV提供了大量的开源图像算法，是计算机视觉中使用极广的图像处理算法库。
本书也使用 OpenCV 做基本的图像处理。在使用之前，我建议你从源代码安装它。在 ubuntu 下，你可以选择从源代码安装和只安装库文件两种方式：

1. 从源代码安装，是指从 OpenCV 网站下载所有的 OpenCV 源代码。并在你的机器上编译安装，以便使用。好处是可以选择的版本比较丰富，而且能看到源代码，不过需要花费一些编译时间；
2. 只安装库文件，是指通过 Ubuntu 来安装由 Ubuntu 社区人员已经编译好的库文件，这样你就无需重新编译一遍。
   由于我们使用较新版本的 OpenCV，所以你必须从源代码来安装它。一来，你可以调整一些编译选项，来匹配你的编程环境（例如需不需要 GPU 加速等）；再者，源代码安装可以使用一些额外的功能。OpenCV 目前维护了两个主要版本，分为 OpenCV 2.4 系列和 OpenCV 3 系列。本书使用 OpenCV 3 系列。 由于 OpenCV 工程比较大，我们就不放在本书的3rdparty下了。请读者从 http: //opencv.org/downloads.html 中下载，选择 OpenCV for Linux 版本即可。你会获得一 个像 opencv-3.1.0.zip 这样的压缩包。将它解压到任意目录下，我们发现 OpenCV 亦是一个 cmake 工程。

在编译之前，先来安装OpenCV的依赖项：

sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev

事实上 OpenCV 的依赖项很多，缺少某些编译项会影响它的部分功能（不过我们也不会用到所有功能）。OpenCV 会在 cmake 阶段检查依赖项是否会安装，并调整自己的功能。如果你的电脑上有 GPU 并且安装了相关依赖项，OpenCV 也会把 GPU 加速打开。不过对于本书，上边那些依赖项就足够了.

随后的编译安装和普通的 cmake 工程一样，请在 make 之后，调用 sudo make install将 OpenCV 安装到你的机器上（而不是仅仅编译它）。视你的机器配置，这个编译过程大概需要二十分钟到一个小时不等。如果你的 CPU 比较强力，可以使用“make -j4”这样的命令，调用多个线程进行编译（-j 后边的参数就是使用的线程数量）。在安装之后，OpenCV默认存储到你的/usr/local 目录下。你可以去寻找 opencv 头文件与库文件的安装位置，看看它们都在哪里。另外，如果你之前已经安装了 OpenCV 2 系列，我建议你把 OpenCV 3安装到不同的地方——想想这应该如何操作。

5.3.2 操作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;
}

5.4 实践：拼接点云

最后，我们来练习一下相机内外参的使用方法。本节程序提供了五张 RGB-D 图像，并且知道了每个图像的内参和外参。根据 RGB-D 图像和相机内参，我们可以计算任何一个像素在相机坐标系下的位置。同时，根据相机位姿，又能计算这些像素在世界坐标系下的位置。如果把所有像素的空间坐标都求出来，相当于构建一张类似于地图的东西。现在我们就来练习一下。

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry> 
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
#include <pcl/visualization/pcl_visualizer.h>

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> 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;
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }
    
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud ); 
    for ( int i=0; i<5; i++ )
    {
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d 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;
                
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                pointCloud->points.push_back( p );
            }
    }
    
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( joinMap )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# opencv 
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# eigen 
include_directories( "/usr/include/eigen3/" )

# pcl 
find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )

add_executable( joinMap joinMap.cpp )
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )