Robotics Lab2——相机模型，点云图拼接与深度测量

Robotics_Lab2：1.通过对RGB-D相机参数读取、画面帧的拼接验证，形成点云图，了解机器视觉中深度图像处理的一般工作流程；
2.掌握OpenCV、Eigen、PCL等第三方图像图像库的使用方法；3.尝试自行搭建双目视觉系统，实现对视图中各物体的深度测量。

相机模型

计算机视觉中一般用到四种坐标系：像素坐标系(pixel)，成像平面坐标系(picture)，相机坐标系(camera)，世界坐标系(world)；
其中{world}，{camera}，{picture}的坐标系单位为长度，一般为mm，{pixel}坐标系单位为像素，一般为pix。
相机将三维现实世界中的坐标点(单位为米) 映射到二维图像平面(单位为像素)的过程，可以描述为一个几何模型，易于编程处理。
图像采集的过程涉及世界坐标系到像素坐标系的转换(其间经过其它坐标系)，而机器人构建现实三维地图的过程涉及已知的像素坐标到世界坐标系相应位置的转换。

单目相机模型

[原理] 投影平面到小孔的距离为焦距f，物体到小孔的距离为Z，物体和投影是倒立相似的关系。下面是SLAM十四讲中立体的针孔相机模型：

针孔相机模型.png

如果按照实际的投影关系建立坐标系，则成像总是倒立的，为了简化模型，将成像平面对称到相机前方，和三维空间点一起放在摄像机坐标系的同一侧。

单目相机的内参和外参

单目相机的内参主要用于像素坐标系和相机坐标系之间的变换；外参主要用于相机坐标系和世界坐标系之间的变换。
相机的内参一般是出厂固定的，若厂商没有提供参数，则需要相机标定，从而确定相机的内参[cx,cy,fx,fy]，此外还会有透镜畸变参数(5个)`。
相机的外参包括旋转部分(R)和平移部分(t)，其中R若为旋转矩阵，则一共有3+3=6个参数；R还可以用四元数表示，其具有参数少，无奇异性的优点。
旋转部分和平移部分组成欧式变换矩阵(T)，其是4x4维的矩阵。
单目相机的坐标变换推导如下图：

单目相机内参.jpg

单目相机外参.jpg

基于OpenCV的单目相机标定

[单目摄像机标定目的] 使摄像机实际状态无限接近理论推导的理想状态。单目摄像机标定最终将确定9个参数，摄像机内参数有4个，透镜畸变参数5个。
[单目摄像机标定流程]
1. 制作标定板
2. 使用摄像机拍摄不同角度的标定板
3. 将照片放置于预设的文件夹中
4. 编写程序计算摄像机内参数和透镜畸变参数
5. 保存9个参数
相机标定得到的cx,cy一般是相机分辨率的一半。
[关键源代码和库函数]
1. bool findChessboardCorners((InputArray image, Size patternSize, OutputArray corners)
寻找棋盘格标定板的焦点。三个参数依次代表输入图像、角点数目和存储角点的变量。检测到角点以后，常常需要用void drawChessboardCorners()函数将其画出来.
如果找到的角点数目和输入的角点数目相同，就会用彩色圆圈画出角点，否则只用红色圆圈画出角点。
1. CameraCalibrator类

双目相机模型

双目相机的内参和外参

双目相机的内参与单目相机的最主要区别为双目相机需要确定两个相机之间的相对位置关系(含基线b)。
进行双目相机的标定前，一般先进行单目相机的标定，确定单目相机的内参矩阵和畸变矩阵等。
两相机的坐标关系可以相互转换，一般以左相机为主相机，同样包括旋转矩阵R和平移矩阵T，其主要用于立体校正和对极几何。
单目相机需要标定的参数双目相机都要标定。
双目相机的坐标变换推导如下图：

双目相机.jpg

双目相机可以通过视差计算深度，从而使机器人可以获取现实世界的距离等三维信息，进行地图构建和路径规划等。

基于OpenCV的双目相机标定

本征矩阵E和基础矩阵F

`对级几何在双目问题中非常的重要，可以简化立体匹配等问题；而要应用对级几何去解决问题，比如[求级线]，需要知道本征矩阵或者基础矩阵，因此双目标定过程中也会把本征矩阵和基础矩阵算出来。
本征矩阵E描述的是同一点投影在{picture}坐标系下的关系，单位为mm；其为3x3的矩阵。
基础矩阵F描述的是同一点投影在{pixel}坐标系下的关系，单位为pix。

RGB-D相机模型

RGB-D相机能够主动测量每个像素的深度。其按原理可分为两大类：
1. 通过红外结构光来测量像素距离。如Kinect 1代、Project Tango 1代、Intel RealSense等。
2. 通过飞行时间法原理测量像素距离。如Kinect 2代，一些现有的ToF传感器等。
RGB-D相机需要向探测目标发射一束光线(通常为红外光)：结构光原理中，相机根据返回的结构光图案，计算物体离自身的距离；
ToF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体离自身的距离，其可以获得整个图像的像素深度。
测量深度后，RGB-D相机自动完成深度与彩色图像素间的配对，输出一一对应的彩色图和深度图，使我们可以计算像素的3D相机坐标，生成点云。
[缺点] 使用范围受限：用红外进行深度值测量，易受日光或其他传感器发射的红外光干扰，不能在室外使用；投射材质的物体不能接受反射光，使得相机无法测量点位置。
成本高、功耗高。

点云图拼接代码分析

代码分析的注释写到了源代码里

int main( int argc, char** argv )
{
    /*
     * 计算机内存中，数字图像以矩阵的形式存储。OpenCV中，数据结构Mat是保存图像像素信息的矩阵；
     * 其主要包含两部分：矩阵头和一个指向像素数据的矩阵指针。
     * 矩阵头主要包含矩阵尺寸、存储方法、存储地址和引用次数(用于复制)；矩阵头大小为常数，不会随着图像的大小而改变，但保存图像像素数据的矩阵会随图像大小的改变而改变。
     * 当进行图像复制时，不再复制整个Mat数据，而只复制矩阵头和指向像素矩阵的指针。
     */
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图；共5张，所有图像分别存储到动态数组中。


    /*
     * Eigen::Isometry3d表示欧氏变换矩阵(4x4); Eigen::aligned_allocator为STL兼容分配器，用于需要非标准对齐的类型。
     * vector有两个参数，后面的参数一般是默认的，这里用适合Eigen库的对齐方式来初始化容器，总共有5张图片 所以对应着5个位姿矩阵
     */
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿


    /*
     * pose.txt以Twc的形式分别记录五张图像的相机位姿(相机的外参数)。[x,y,z,qx,qy,qz,qw]——平移向量加旋转四元数，其中qw是四元数的实部
    */

    ifstream fin("./pose.txt");   // 读取相机位姿文件
    if (!fin)
    {
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        return 1;
    }

    for ( int i=0; i<5; i++ )
    {
        /*
         * 用boost中的format格式类，来循环读取图片，否则单张读取图片就会有问题
         * 当在命令行中执行的时候这里必须要为../    在当前ide中执行的时候要修改为./
         */
        boost::format fmt( "./%s/%d.%s" );   // 图像文件格式; 可执行文件在build中,图像在上一个目录,所以用../

        /*
         * imread读取绝对路径下的图片，[其读取的是参数值，所以要取str()];返回Mat对象，添加到彩色图片数组的末尾；循环读取并添加。
         * 这里的%对应./ color对应%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 )   // auto表示自动根据后面的元素 获得符合要求的类型
            fin>>d;      // 文件流类型的变量fin依次读取五张图像的相机位姿数据(7个)，赋值给d数组
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );   // 后四个数据保存为Eigen四元数对象(实部在前，虚部在后)
        Eigen::Isometry3d T(q);   // 欧式变换矩阵初始化旋转部分(四元数表示)
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));   // 欧式变换矩阵初始化平移向量
        poses.push_back( T );   // 存储欧式变换矩阵到位姿数组中
    }


    /*
     * 点云数据：指在一个三维坐标系统中的一组向量的集合。通常以x,y,z三维坐标的形式表示，既表示几何位置信息，还表示一个点的RGB颜色、灰度值、深度和分割结果等。
     */

    // 计算点云并拼接
    // 相机内参
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout<<"正在将图像转换为点云..."<<endl;

    // 定义点云使用的格式：这里用的是XYZRGB
    /*
     * 程序中点云中的点携带的信息为坐标x,y,z和颜色b,g,r; PointT代表一个点，PointCloud为全部点的集合(点云)
     */
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    // 新建一个点云
    /*
     * PointCoud::Ptr是一个智能指针类; 这里通过构造函数初始化指针指向的申请的空间
     */
    PointCloud::Ptr pointCloud( new PointCloud );


    /*
     * 接下来的整个for循环对每个图片依次做核心处理：
     *         通过图像得知每个像素的坐标，使用内参将像素坐标转换到相机坐标；
     *         再使用外参(相机位姿)将像素在相机坐标的位置转换到世界坐标；
     *         世界坐标中的值和点云中点数据的值是两个不相容的标准，需要将世界坐标点存储到点云格式的变量中(包含颜色信息)。
     */
    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];   // 相机外参定义欧式变换矩阵T

        /*
         * 变换部分：对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标
         */

        /*
         * 单通道遍历图像的方式总结:
         * 注意深度图像的像素占16位  与普通图片每个通道的像素为8位不同
         * 1、同样是用上面的双层for循环，遍历图像 用at方式
         * for ( int v=0; v<color.rows; v++ )
         *      for ( int u=0; u<color.cols; u++ )
         *          unsigned int d = depth.at<unsigned short >(v,u);
         *
         *
         * 2、使用迭代器进行图像的遍历
         * 不是基于for循环了;
         * 迭代器的参数是通道数，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short
         * cv::MatIterator_<unsigned short > begin,end;
         * for( begin =depth.begin<unsigned short >(), end = depth.end<unsigned short >(); begin != end; ){}
         *
         * 3、使用指针的方式 如本实验的代码
 */

        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {

                /*
                 * 使用Mat中的ptr模板函数，返回unsigned short类型的指针；
                 * 因为图像为单通道的(深度图)   用depth.ptr<unsigned short> ( v ) 来获取行指针；
                 * 一般彩色图像像素中每一个通道是8位，而深度值按16位存储。
                 */
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值; 相当于按行来遍历(理解为内部的for循环)

                if ( d==0 ) continue; // 为0表示没有测量到

                Eigen::Vector3d point;   // point为三维向量类型，表示相机坐标系下的点
                point[2] = double(d)/depthScale;   // 对实际尺度的一个缩放

                /*
                 * 根据针孔成像模型，计算像素点在相机坐标系中的位置坐标; SLAM十四讲有公式推导
                 */
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;  // 通过变换矩阵T，将坐标点从相机坐标系变换到世界坐标系

                PointT p ;   // 定义一个点云的点对象
                // 将世界坐标系下的坐标用PCL专门的点云格式存储起来
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                /* ?
                 * color.step 虽然是一个类，但是它内部有一个转换操作符 operator size_t() const;
                 * 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中
                 */
                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 );
            }
    }

    //这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为假，但是如果设置成true的话 也没有看出来有什么不一样的地方  ?
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    //获取pointCloud指向的对象 这个就当做获取普通指针指向的对象来理解，这个对象是在定义的时候new出来的一段内存空间。
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );    // 函数参数是要存储的点云的名称，和要拼接的点云名称；点云用指针形式。

    /*
     * 查看点云使用pcl_viewer
     */
    return 0;
}