视觉SLAM十四讲学习笔记——ch5 相机与图像

5. 相机与模型

5.1 相机模型

理论参考博客:

1.《视觉SLAM十四讲》相机位姿与相机外参的区别与联系

2.《视觉SLAM十四讲》学习笔记：第5讲相机与图像

5.2 图像

理论参考博客:

1.视觉SLAM十四讲学习笔记-第五讲-图像和实践

5.3 实践

5.3.1 Open CV的基本使用方法

例程中演示了对 图像读取,显示,像素遍历,复制,赋值等.

代码及注释如下:

#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的块置零,左上角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);//左上角100*100变白色
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0); // 暂停程序,等待一个按键输入

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();//关闭所有打开窗口
  return 0;
}

编译运行,执行如下代码,(PS:图片存储位置需要替换!)

./imageBasics/imageBasics /home/lmf37/桌面/slambook2/ch5/imageBasics/ubuntu.png

执行结果如下:
(除了可以看到一张图片,还输出了图片的宽度\高度信息)

5.3.2 图像去畸变

由透镜形状引起的畸变叫做径向畸变，包括桶形畸变和枕形畸变
图像中一条直线，往里弯是桶形，往外弯是枕形【但个人习惯感觉桶形畸变是鼓起来，枕形相反,可不过也不用纠结,桶和枕名字取得很形象,遇到看图就明白了☺】
组装过程导致的成像平面与透镜不平行，带来的是切向畸变

去畸变的思路是这样的：

创建一个和原来一样大的图片区域
遍历这个新图片的所有像素（u v）
通过畸变参数，计算(u,v)处畸变后的坐标
把原图上的畸变后的坐标处的像素值赋值给新图片（涉及到插值）
遍历完成后，新图片就是去畸变的了

编译之后直接运行就好，注意修改代码中的图片路径
（与上边的实验不同，这里使用的路径是在程序里写好的）

代码及注释如下:

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

using namespace std;

string image_file = "/home/lmf37/桌面/slambook2/ch5/imageBasics/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;
}

**但是!!!**,不幸的报错了☹,之前调试是通过的,不知道为什么又报错了,错误如下:

OpenCV Error: Assertion failed (size.width>0 && size.height>0) in imshow, file /home/lmf37/SLAM/opencv-3.4.0/modules/highgui/src/window.cpp, line 331
terminate called after throwing an instance of 'cv::Exception'
  what():  /home/lmf37/SLAM/opencv-3.4.0/modules/highgui/src/window.cpp:331: error: (-215) size.width>0 && size.height>0 in function imshow

检查了一下是上面路径写的不对(尴尬,自己强调的事情,转身又掉坑里,不熟练掌握~)

//错误
string image_file = "./home/lmf37/桌面/slambook2/ch5/imageBasics/distorted.png/distorted.png"; 
//正确
string image_file = "/home/lmf37/桌面/slambook2/ch5/imageBasics/distorted.png"; 

执行结果如下:

5.4 3D视觉

5.4.1 双目视觉

代码及注释如下:

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>
//unistd.h为Linux/Unix系统中内置头文件，包含了许多系统服务的函数原型，例如read函数、write函数和getpid函数等。其作用相当于windows操作系统的"windows.h"，是操作系统为用户提供的统一API接口，方便调用系统提供的一些服务。
using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "/home/lmf37/桌面/slambook2/ch5/stereo/left.png";
string right_file = "../stereo/right.png";

// 在pangolin中画图，已写好，无需调整,定义绘制点云的函数，需要传入4维向量构成的点云集
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;
    // 基线 （就是两个相机光轴间的距离，单位是m）
    double b = 0.573;

   // 读取图像（以灰度图形式）
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(//SGBM是立体匹配算法中的半全局块匹配，得到的视差图相比于BM算法来说，减少了很多不准确的匹配点，尤其是在深度不连续区域，速度上SGBM要慢于BM算法；
        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);//由左右视图按照SGBM匹配方式计算得到视差图
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);//将16位符号整形的视差Mat转换为32位浮点型Mat

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;  //定义4维形式的点云向量容器

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)   //遍历左视图
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

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

            // 根据双目模型计算 point 的位置,计算的是左视图点的相机位置
            double x = (u - cx) / fx;          //该公式计算的是归一化在相机Zc=1平面的相机坐标
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));//由视差，双目的基计算像素点对应的实际距离（深度信息）
            point[0] = x * depth;    //由深度信息获取真实相机坐标系下的Xc
            point[1] = y * depth;   //由深度信息获取真实相机坐标系下的Yc
            point[2] = depth;         //相机坐标系下的Zc

            pointcloud.push_back(point);   //获得的是相机坐标系下的点云位置
        }

    cv::imshow("disparity", disparity / 96.0);  //把视差值限定在0-96
    cv::waitKey(0);
    // 画出点云
    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) { //pointcloud容器中的每一个元素从前往后枚举出来，并用 p 来表示
            glColor3f(p[3], p[3], p[3]);       //点的颜色
            glVertex3d(p[0], p[1], p[2]);   //点的相机坐标
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

执行结果如下:😦注意第一行图是像差图,点击任意键盘按键可显示点云图)😃

**插播一条小乐趣**
:)这个符号可以显示 😃 :( 可以显示 😦 ,码字の小乐趣

还有这张图有种莫名的美感,!!! 今晩（こんばん）の月（げつ）は绮丽（きれい）ですね :)
!!! 注意 可以看到相差图中左边有一条黑色,这是因为,左侧相机看到了一部分右侧相机未看到的内容,所以对应的视差是空的.😃

5.4.2 RGB-D视觉

color/有5张RGB图,depth/有5张对应的深度图,pose.txt文件给出了5张图像的相机外参位姿(Twc),为平移向量加旋转四元数:

本节代码主要从完成两件事:

1. 根据相机内参计算一对RGB-D图像对应的点云
2. 根据各张图的相机位姿(即外参),讲点云加一起,组成地图

代码及注释如下:

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <sophus/se3.h>
#include <pangolin/pangolin.h>

using namespace std;
typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

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

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    TrajectoryType poses;       // 相机位姿

    ifstream fin("/home/lmf37/桌面/slambook2/ch5/rgbd/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("/home/lmf37/桌面/slambook2/ch5/rgbd/%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;
        Sophus::SE3 pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000);

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;

        
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Sophus::SE3 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;

                Vector6d p;
                p.head<3>() = pointWorld;
                // 赋值图像中某个像素点的rgb值（注意： opencv中图像颜色为BGR，取相应值的时候要注意顺序）
                p[5] = color.data[v * color.step + u * color.channels()];   // blue
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
                pointcloud.push_back(p);
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &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) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

执行结果如下:
(记得修改文件位置)

PS:可以用鼠标平移旋转点云

5.5 利用 KDevelop IDE编译器 编译执行文件

1.点击构建
其中运行第一个 open CV 读文件需注意,要将文件名参数,和位置传入,具体如下:
程序正确运行

要换不同的程序,可以在运行–>配置启动-->当前启动配置 进行切换即可,然后点击调试即可得到结果
例如切换RGB-D程序