第5讲 相机与图像

1 操作OpenCV图像

  cpp文件内容如下，

#include <iostream>
#include <chrono>
#include <ctime>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;

string path = "../ubuntu.png";
clock_t start, last;

int main()
{
    start = clock();

    Mat image;
    image = imread(path);  //imread()函数读取指定路径下的图像

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

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

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

    //使用指针遍历图像image中的像素
    //steady_clock是单调的时钟，相当于教练中的秒表，只会增长，适合用于记录程序耗时。
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //size_t全称是size_type，它表示sizeof()函数的返回值，是无符号整型unsigned int变量。
    //使用size_t的目的是提供一种可移植的方法来声明与系统中可寻址的内存区域一致的长度。
    for(size_t y = 0; y < image.rows; y++)
    {
        //用ptr获得图像的行指针
        unsigned char* row_ptr = image.ptr<unsigned char>(y);  //row_ptr是第y行的头指针
        for(size_t x = 0; x < image.cols; x++)
        {
            //data_ptr指向待访问的像素数据
            unsigned char* data_ptr = &row_ptr[x * image.channels()];
            //输出该像素的每个通道，如果是灰度图就只有一个通道
            for(int c = 0; c != image.channels(); c++)
                unsigned char data = data_ptr[c];  //data为image(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;

    //直接赋值并不会拷贝数据，图像名image有点类似于指针
    Mat image_another = image;
    //修改image_another也会导致image发生变化
    image_another(Rect(0, 0, 100, 100)).setTo(0);  //将左上角100*100的块置为黑色
    imshow("image", image);
    waitKey(0);  //停止执行，等待一个按键输入
    cout << "直接赋值并不会拷贝数据，修改image_another也会导致image发生变化！" << endl;

    //使用clone()函数拷贝数据
    Mat image_clone = image.clone();
    image_clone(Rect(0, 0, 100, 100)).setTo(255);  //将左上角100*100的块置为白色
    imshow("image", image);
    imshow("image_clone", image_clone);
    waitKey(0);
    cout << "使用clone()函数可以拷贝数据，修改图像image_clone并不会改变图像image！" << endl;

    last = clock();
    cout << "运行程序花费的时间为：" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒！" << endl;
    return 0;
}

  CMakeLists.txt文件内容如下，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRECTORIES})
add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  结果如下，

图像宽为：1200像素，图像高为：674像素，图像通道数为：3！
遍历图像用时：0.0128371秒！
直接赋值并不会拷贝数据，修改image_another也会导致image发生变化！
使用clone()函数可以拷贝数据，修改图像image_clone并不会改变图像image！
运行程序花费的时间为：192.463毫秒！

注：结果中省略了imshow()函数的显示！

2 图像去畸变

  cpp文件内容如下，

#include <iostream>
#include <string>
#include <cmath>
#include <ctime>

using namespace std;

#include <opencv2/opencv.hpp>

using namespace cv;

string path = "../distorted.png";  //图片路径
double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;  //相机内参
double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-5;  //畸变参数

clock_t start, last;

//本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变函数，但自己去实现一遍有助于理解
int main()
{
    start = clock();

    Mat image = imread(path, 0);  //0表示返回一张灰度图
    int rows = image.rows, cols = image.cols;
    Mat image_undistort(rows, cols, CV_8UC1);  //构造一张与图像image相同尺寸和相同类型的图像image_undistort

    //计算去畸变后的内容
    for(int v = 0; v < rows; v++)
        for(int u = 0; u < cols; u++)
        {
            //依据公式，计算像素点image_undistort(v,u)去畸变后的坐标image(v_distorted,u_distorted)
            double x = (u - cx) / fx, y = (v - cy) / fy;
            double r = sqrt(x * x + y * y);
            double r2 = pow(r, 2), r4 = pow(r, 4);
            double x_distorted = x * (1 + k1 * r2 + k2 * r4) + 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
            double y_distorted = y * (1 + k1 * r2 + k2 * r4) + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y;
            double u_distorted = fx * x_distorted + cx;
            double v_distorted = fy * y_distorted + cy;

            //将image(v_distorted,u_distorted)处的颜色信息赋值到image_undistort(v,u)处
            if(u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows)
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int)round(v_distorted), (int)round(u_distorted));
            else
                image_undistort.at<uchar>(v, u) = 0;  //超出范围像素点的颜色设置为黑色


        }

    //显示去畸变前后的图像
    imshow("原图", image);
    imshow("去畸变后的图像", image_undistort);
    waitKey(0);  //程序终止，等待一个按键输入

    last = clock();
    cout << "执行程序总共花费的时间为：" << double(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒！" << endl;
    return 0;
}

  CMakeLists.txt文件内容如下，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})
add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  结果如下，

执行程序总共花费的时间为：80.934毫秒！

3 双目视觉

  cpp文件内容如下，

#include <iostream>
#include <ctime>
#include <vector>
#include <unistd.h>

using namespace std;

#include <opencv2/opencv.hpp>

using namespace cv;

#include <Eigen/Core>  //Eigen核心模块

using namespace Eigen;

#include <pangolin/pangolin.h>

//相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157, b = 0.573;

//文件路径
string left_file = "../left.png";
string right_file = "../right.png";

//在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>>& pointcloud);

clock_t start, last;

int main()
{
    start = clock();

    Mat left = imread(left_file, 0);  //0表示返回一张灰度图
    Mat right = imread(right_file, 0);  //0表示返回一张灰度图

    //调用opencv中的sgbm算法来求解深度
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(
            0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32
            );  //来自网络上的关于sgbm算法的经典参数配置
    Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);  //注意disparity才是最后的视差图

    //生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;  //前三维为X,Y,Z，表示位置信息，后一维表示颜色信息，在此处为灰度
    for(int v = 0; v < left.rows; v++)
        for(int u = 0; u < left.cols; u++)
        {
            //设置一个Check，排除视差不在(10.0, 96.0)范围内的像素点
            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); //创建一个Vector4d类型的变量，前三维用来存储位置信息，后一维为归一化之后的灰度
            //根据双目模型恢复像素点的三维位置
            double Z = fx * b / (disparity.at<float>(v, u));
            double X = (u - cx) / fx * Z;
            double Y = (v - cy) / fy * Z;
            //将X,Y,Z赋值给point的前三维
            point[0] = X; point[1] = Y; point[2] = Z;

            pointcloud.push_back(point);


        }

    imshow("视差图", disparity / 96.0);  //disparity/96表示归一化之后的视差图像！
    waitKey(0);  //停止执行，等待一个按键输入

    //用pangolin画出点云
    showPointCloud(pointcloud);

    last = clock();
    cout << "执行程序总共花费的时间为：" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒！" << endl;
    return 0;

}


void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>>& pointcloud)
{
    if(pointcloud.empty())
    {
        cerr << "点云为空，返回！" << endl;
        return;
    }

    //创建一个pangolin窗口
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);  //根据物体远近，实现遮挡效果
    glEnable(GL_BLEND);  //使用颜色混合模型，让物体显示半透明效果
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    //GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示使用(1-源颜色的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)
            );
    //ProjectionMatrix()中参数依次表示相机观察视图的宽度和高度，fx, fy, cx, cy，最小深度和最大深度
    //ModelViewLookAt()中参数依次为相机光心位置，被观察点位置，相机坐标系各坐标轴的正方向(000表示右下前)

    //创建交互视图，显示上一帧图像内容
    pangolin::Handler3D handler(s_cam);  //交互视图句柄
    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(&handler);
    //SetBounds()中各参数表示交互视图范围、交互视图横纵比

    while(pangolin::ShouldQuit() == false)  //如果pangolin视图没有被关闭，则执行
    {
        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);  //设置线宽为2

        //绘制点云
        glBegin(GL_POINTS);
        for(auto &p : pointcloud)  //使用基于范围的for进行遍历
        {
            glColor3f(p[3], p[3], p[3]);  //设置颜色信息
            glVertex3d(p[0], p[1], p[2]);  //设置位置信息
        }
        glEnd();
        pangolin::FinishFrame();  //按照上面的设置执行渲染！
        usleep(5000);  //停止执行5毫秒
    }
    return;
}

  CMakeList.txt文件内容如下，

include_directories("/usr/include/eigen3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} 
                           ${Pangolin_LIBRARIES})

  结果如下，

上图为左图的视差图，黑色表示视差为0，白色表示视差为1。根据公式$Z=f_{x}b/d$可知，深度越大，视差越小，对应上图就是离相机越远的地方越黑。左边小部分区域由于没有相同的观测信息，所以无法计算视差，表现为黑色。
注：上图为点云显示图，为了更好的展示三维立体效果，本文提供了点云显示相关视频，见

双目视觉

如需要更高清的显示，请见bilibili投稿视频

执行程序总共花费的时间为：1669.74毫秒！

4 RGB-D视觉

  已知5张彩色图和它们对应的深度图，同时还知道它们的相对位姿，建立一张点云地图。
  cpp文件内容如下，

#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <ctime>

using namespace std;

#include <opencv2/opencv.hpp>

using namespace cv;

#include <sophus/se3.hpp>

using namespace Sophus;

#include <pangolin/pangolin.h>

typedef vector<SE3d, Eigen::aligned_allocator<SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

//在pangolin中绘制点云图
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>>& pointcloud);

//相机内参
double cx = 325.5, cy = 253.5, fx = 518.0, fy = 519.0;
double depthScale = 1000.0;   //现实世界中1米在深度图中存储为一个depthScale值

clock_t start, last;

int main()
{
    start = clock();

    vector<Mat> colorImages, depthImages;  //定义彩色图向量类容器和深度图向量类容器
    TrajectoryType poses;  //定义相机位姿

    ifstream fin("../data/pose.txt");  //创建文件输入流fin
    if(!fin)
    {
        cerr << "找不到文件位姿文件！" << endl;
        return 1;
    }

    for(int i = 0; i < 5; i++)  //有五张图
    {
        //把彩色图放到colorImages中，把深度图放到depthImages中！
        boost::format fmt("../data/%s/%d.%s");  //定义图像文件格式
        colorImages.push_back( imread( (fmt % "color" % (i + 1) % "png").str() ) );
        depthImages.push_back( imread( (fmt % "depth" % (i + 1) % "pgm").str() ) );

        //把位姿放到poses中！
        double tmp[7] = {0};  //用数组存储单个位姿SE3d
        for(auto &val : tmp)  //基于范围的for循环，auto表示自动类型推导
            fin >> val;  //fin表示文件输入流
            //用李群存储的单个位姿
            SE3d pose(Eigen::Quaterniond(tmp[6], tmp[3], tmp[4], tmp[5]),
                      Eigen::Vector3d(tmp[0], tmp[1], tmp[2]));
            poses.push_back(pose);
    }
    fin.close();  //关闭文件输入流fin

    //计算点云并拼接
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointCloud;  //创建一个vector<Vector6d>变量
    pointCloud.reserve(1000000);  //reserve()函数用来给vector预分配存储区大小，但不对该段内存进行初始化
    //遍历每张图像
    for(int i = 0; i < 5; i++)
    {
        cout << "将图像" << i + 1 << "中的点云转换到世界坐标系下！" << endl;
        Mat color = colorImages[i];
        Mat depth = depthImages[i];
        SE3d T = poses[i];  //用SE3d表示的从当前相机坐标系到世界坐标系的变换
        //遍历每个像素点
        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;  //d为0表示没有观测到！
                //计算点云在该帧相机坐标系下的坐标值
                double X, Y, Z;
                Z = d / depthScale;  //真实世界中的深度值
                X = (u - cx) * Z / fx;
                Y = (v - cy) * Z / fy;
                Vector3d point(X, Y, Z);
                Vector3d pointWorld = T * point;

                Vector6d p;  //前三维表示点云的位置，后三维表示点云的颜色
                p.head<3>() = pointWorld;  //head<n>()函数是对于Eigen库中的向量类型而言的，表示提取前n个元素

                //opencv中图像的data数组表示把其颜色信息按行优先的方式展成的一维数组！
                //color.step等价于color.cols
                //color.channels()表示图像的通道数
                p[5] = color.data[v * color.step + u * color.channels()];  //蓝色分量
                p[4] = color.data[v * color.step + u * color.channels() + 1];  //绿色分量
                p[3] = color.data[v * color.step + u * color.channels() + 2];  //红色分量

                pointCloud.push_back(p);

            }

    }


    cout << "总共有" << pointCloud.size() << "个点云！" << endl;
    showPointCloud(pointCloud);

    last = clock();
    cout << "程序总共花费的时间为：" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒！"  << endl;
    return 0;
}


void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>>& pointcloud)
{
    if(pointcloud.empty())
    {
        cerr << "点云是空的！" << endl;
        return;
    }

    //生成一个pangolin窗口
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);  //使能遮挡渲染
    glEnable(GL_BLEND);  //使用颜色混合模式
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    //使用(源图像的alpha值)作为权重因子，使用(1-源图像的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)
            );
    //ProjectionMatrix()中参数依次为pangolin窗口的宽度、pangolin窗口的高度、fx、fy、cx、cy、最小深度值和最大深度值
    //ModelViewLookAt()中各参数依次为相机位置、被观察点位置和相机各轴正方向(000表示右下前)

    //设置相机窗口
    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);  //设置相机视图背景颜色

        //绘制点云
        glBegin(GL_POINTS);  //绘制点开始指令
        for(auto &p : pointcloud)
        {
            glPointSize(2);  //设置点的大小
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();  //绘制点结束指令
        pangolin::FinishFrame();  //按照上述设置执行渲染，并显示在pangolin窗口中
        usleep(5000);  //程序停止执行5毫秒！
    }
    return;
}

  CMakeLists.txt文件内容如下，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_DIRECTORIES})

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES}
                           ${Pangolin_LIBRARIES})

  结果如下，

将图像1中的点云转换到世界坐标系下！
将图像2中的点云转换到世界坐标系下！
将图像3中的点云转换到世界坐标系下！
将图像4中的点云转换到世界坐标系下！
将图像5中的点云转换到世界坐标系下！
总共有1137349个点云！
程序总共花费的时间为：6756.35毫秒！

点云显示如下，
演示视频如下，

RGB-D视觉

由于只在有限个视角进行点云拼接，因此所得点云地图展示效果并不好，存在“断层”的现象！更高清的画质见 bilibili投稿视频