视觉SLAM十四讲：相机模型和OpenCV相关课后程序理解

1. OpenCV库基础应用

//
// Created by g214-j on 18-8-5.
//

#include <iostream>
#include <chrono>       // 在C++11中，<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++){
        for(size_t x=0; x<image.cols; x++){
            // 访问位于 x,y 处的像素
            // 用cv::Mat::ptr获得图像的行指针
            unsigned char* row_ptr = image.ptr<unsigned char>(y);   // row_ptr是第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()<<" s."<<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;
}

 

2. 已知位姿，RGBD图像（彩色图和深度图）和相机内参的点云图（PCL）生成

//
// Created by g214-j on 18-8-6.
//

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

#define DEBUG false

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

    // cv::imread(string,flag);
    // MREAD_UNCHANGED(-1) ：不进行转化，比如保存为了16位的图片，读取出来仍然为16位。
    // IMREAD_GRAYSCALE(0) ：进行转化为灰度图，比如保存为了16位的图片，读取出来为8位，类型为CV_8UC1。
    // IMREAD_COLOR(1) ：进行转化为三通道图像。
    // IMREAD_ANYDEPTH(2) ：如果图像深度为16位则读出为16位，32位则读出为32位，其余的转化为8位。
    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读取原始图像

        // 定义一个7个变量的数组并初始化为0，然后定义一引用，一个for循环，让for循环遍历data中的每一元素d,并给每个元素赋位姿里的值。
        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.rotate(q);  // 这句话是错的
        T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2]));
        poses.push_back(T);

        if(DEBUG){
            cout<<"pose:"<<data[0]<<","<<data[1]<<","<<data[2]<<","<<data[3]<<","<<data[4]<<","<<data[5]<<","<<data[6]<<endl;
            cout<<"变换矩阵："<<T.matrix()<<endl;
        }

    }

    // 计算点云并拼接
    // 相机内参
    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;        // depthScale是RGBD相机的参数吗？？？
                point[0] = (u-cx)*point[2]/fx;          // 相机成像模型（相机坐标下一点转换到像素面），这里反过来就是已知深度和像素坐标，反推出相机坐标系下的x和y
                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];
//                if(DEBUG)
//                    cout<<"pointWorld: x,y,z "<<pointWorld[0]<<" "<<pointWorld[1]<<" "<<pointWorld[2]<<endl;
                // 赋值图像中某个像素点的rgb值（注意： opencv中图像颜色为BGR，取相应值的时候要注意顺序）
                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;
}

 

3.  图像去畸变

//
// Created by g214-j on 18-8-6.
//

//
// Created by 高翔 on 2017/12/15.
//

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "./test.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_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++) {

            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
            // start your code here

            // (v,u)在归一化平面上的空间坐标(x,y,1)
            double xNormal = (u-cx)/fx;
            double yNormal = (v-cy)/fy;

            // 得到矫正数据r方
            double rPow2 = pow(xNormal,2) + pow(yNormal,2);
            // 即便后的归一化坐标(xDistorted,yDistorted,1)
            double xDistorted = xNormal*(1 + k1*rPow2 + k2*pow(rPow2,2)) + 2*p1*xNormal*yNormal + p2*(rPow2 + 2*pow(xNormal,2));
            double yDistorted = yNormal*(1 + k1*rPow2 + k2*pow(rPow2,2)) + p1*(rPow2 + 2*pow(yNormal,2)) + 2*p2*xNormal*yNormal;
            // (xDistorted,yDistorted,1)通过相机模型映射到畸变的像素平面
            u_distorted = fx*xDistorted + cx;
            v_distorted = fy*yDistorted + cy;

            // end your code here

            // 赋值 (最近邻插值)
            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("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

 

4.  双目视差的使用（其实难点应该是深度图的获得，这里我们使用以获得的深度图，自己不做计算）

//
// Created by g214-j on 18-8-6.
//

//
// Created by 高翔 on 2017/12/15.
//

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径，如果不对，请调整
string left_file = "./left.png";
string right_file = "./right.png";
string disparity_file = "./disparity.png";

// 在panglin中画图，已写好，无需调整
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;
    double f = fx;

    // 读取图像(都以单通道灰度图读入)
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 为CV_8U,单位为像素

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {

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

            // start your code here (~6 lines)
            // 根据双目模型计算 point 的位置
            double dis = disparity.at<uchar>(v,u);
            point[2] = f*b/dis;     // 这里视差直接用的像素点间的像素差，这样对吗？？？
            point[0] = (u-cx)*point[2]/fx;
            point[1] = (v-cy)*point[2]/fy;

            pointcloud.push_back(point);

            // end your code here
        }

    // 画出点云
    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;
}