目录

一、图像

二、相机

A、单目相机

1.相机模型

​ 2. 成像过程

 3.opencv代码实战

4.图像去畸化  

B、双目相机

C、RGB-D相机

---

代码来源：gaoxiang12/slambook2: edition 2 of the slambook (github.com) 

一、图像

图像可以由一个二维数组来表示。

其中灰度图每一个元素的下标表示一个像素的坐标，元素的值代表0~255（unsigned short）的灰度读数。值得注意的是，我们讨论一个位于（x,y）的像素时，表示为image[y][x]，因为像素平面上x相右，y向下。

在彩色图中，一个像素占据3个字节，对应三个通道（RGB）。因此，image[y][x]表示像素的第一个字节在（x,y）。而image[y][x]，image[y+1][x]，image[y+2][x]的值表示三个通道的值。

二、相机

A、单目相机

1.相机模型

 2. 成像过程

先介绍一下归一化平面：如图的+1处，即相机坐标的X、Y都除以Z：（X/Z,Y/Z,1）

 

 3.opencv代码实战

1. 指向图像的变量（不知道可不可以理解为指针）

cv::Mat image=cv::imread("path of image",0);//后面这个0可有可无

2. 显示图像：cv::imshow("anything",image)

3. 图像基本信息

宽：image.cols        高：image.rows        通道数：image.channels()

 CV_8UC1表示单通道图片，即灰度图；CV_8UC3表示三通道，即彩色图

4. 用image.ptr<unsigned char>(y)获得指向y行的行指针

unsigned char *row_ptr = image.ptr<unsigned char>(y)

5. 置0：
image(cv::Rect(0, 0, 100, 100)).setTo(0); 

6. 复制（改变复制项不会对被复制项造成影响）

cv::Mat image_clone=image.clone();

直接复制相当于两个指针指向同一处

7. argv[1]指向在DOS命令行中执行程序名后的第一个字符串 

所以程序里输入./executable ./image.jpg

#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;
//CV_8UC1表示单通道图片，即灰度图，CV_8UC3表示三通道，即彩色图
    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 处的像素
//x * image.channels()表示单通道一个一个过，三通道一下跳3个
      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); 
//waitKey刷新图像的时间。waitKey(0)无限等待，waitKey(30)表示等待30ms

  // 使用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);

  cv::destroyAllWindows();
  return 0;
}

4.图像去畸化  

已知一张畸变之后的图像，内参，畸变参数，求去畸变的图像

思路：实现坐标的一一映射，再对灰度赋值

1. 设未畸变的图像的坐标为（u，v），畸变的图像坐标为（ud,vd）

2. 通过（u，v）->（x，y）-畸变公式->（xd,yd）->（ud,vd）

得到（u，v）和（ud，vd）的关系

3. 让（u，v）从（0，0）开始走，遍历到（rows，cols），确定对应的（ud，vd）的坐标

4. 把（ud，vd）对应的灰度值赋给（u，v）即得到，去畸变得图像

实现

 灰度值为image.at<uchar>(v,u),直接赋值

image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);

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

using namespace std;

string image_file = "./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_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++) {
      // 按照公式，计算点(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;
}

B、双目相机

|- (u,v)和单目相机相同

|- 

其中b为基线

代码实践 

1. 得出d：（其实代码看不太懂）
用sgbm算法得出，disparity

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

2. 得出深度depth=fb/disparity.at<float>(v,u);

这里disparity是一个视差图

3. 将数据存入一个向量Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);生成数据点

4. 将数据点存入动态数组 vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;生成点云。

这里代码的注释来自：(1条消息) 第5讲 相机与图像_YMWM_的博客-程序员宅基地

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

C、RGB-D相机

 这里代码的注释仍然来自：(1条消息) 第5讲 相机与图像_YMWM_的博客-程序员宅基地

 思路：

​​​​​​​

#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++)
            {
                //depth.ptr<unsigned short>(v)表示行指针
                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;
}