视觉SLAM十四讲笔记第四讲第一部分相机模型

qq_57225906

于 2024-05-08 11:21:06 发布

阅读量1.4k

点赞数 21

分类专栏： SLAM 文章标签：数码相机笔记人工智能计算机视觉 opencv

本文链接：https://blog.csdn.net/qq_57225906/article/details/138476765

版权

SLAM 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

前言

通过前面第二讲和第三讲内容我们了解了相机位姿的表达方式和进行优化计算所必备的数学工具。在这讲内容中，我们主要学习针孔相机模型以及图像如何表示：相机是如何将一个三维空间中的一个点投影到二维图片上的。

针孔相机模型

针孔相机模型就是我们初中物理所学的小孔成像模型，如图所示：

记O-xyz坐标系为相机坐标系，O为相机光心（也就是针孔相机的针孔），三维空间中的点 $P$ 通过针孔相机模型投影到成像平面 ${P}'$ .（这里的P‘还是物理成像平面上实际的点，并不是像素坐标）；记记光心O到物理成像平面的距离为焦距f；根据图片上的相似三角形关系，可以得到如下关系式：由于实际中，我们在相机中看到的图像是正的，所以这里我们将物理成像平面假设的移到光心前面，翻转到前面后，表达式如下这样一来，完成了第一步：三维空间点转换到物理成像平面上；第二步是物理成像平面坐标到像素坐标的转换；因为三维空间坐标到物理成像平面坐标都是真实尺度下的大小，以米、毫米为单位。因相机输出的是像素坐标，故需要将物理成像平面上的坐标转换为像素坐标。

设物理成像平面 $o-{x}'{y}'$ 同一平面固定像素平面 ${o}'-uv$ 。而像素坐标系的原点o'习惯性放在左上角，u轴与x’平行，v轴与y‘轴平行。所以像素平面和物理成像平面差一个缩放和一个平移。如下表示。代入：用矩阵形式表述如下定义K，为相机的内参矩阵。

因为相机现实中是实时移动的，所以世界坐标系到相机坐标系还需要添加一个旋转和平移。描述旋转用旋转矩阵R，平移为t，变换矩阵为T，这里的R，t，T记为相机的外参。根据前两讲的知识，我们可以得到：

NOTE：从另一个角度看上面的式子：同除Z，使其归一化于归一化平面上，归一化平面可以看作相机前方Z=1平面的一个点，其左乘内参矩阵可以直接得到像素坐标。从这个式子可以看到，式子同乘任意常数，归一化坐标都不变，像素坐标一致，说明这个投影模型丢失了深度这个维度的信息。

畸变模型

现实中，相机为了追求更广阔的视野，会安装透镜。然而透镜由于本身的形状和位置都会使光线发生折射，从而导致成像的变化。由透镜形状引起的变化称为径向畸变：径向畸变导致成像出来的直线不再是直线，而是曲线，且越靠近成像边缘曲线弯曲的越明显。畸变主要分为两种：一种是桶形畸变，另一种是枕形畸变。如图所示径向畸变的数学模型表述如下经过径向畸变后，有切向畸变后：综合径向畸变和切向畸变可以得到数学模型，其中k，p为畸变系数。

双目相机模型

通过上述对单目相机的介绍，我们知道单目相机是无法获取空间点的真实深度的，因为光心到归一化平面的连线及其延长线上的点都会被投影到同一个像素上---单目相机无法获取深度信息；实际上，我们要完成定位和建图，必须要根据相机得到的信息，获取这些像素在世界坐标系下的坐标，从而去达到我们的目的。接下来我们介绍的深度相机就可以得到深度信息。双目相机模型如图所示根据第二幅图中，两个三角形的相似关系，我们可以得出：

这里的d为左右相机的视差，描述同一个点在左右目上成像距离（视差最小为1个像素，所以双目相机可测深度有理论最大值）。双目相机的计算量很大，因为要估计每个像素，首先要进行匹配，话要对每一对匹配点进行计算，所以要做到实时性很难。

RGB-D相机模型

RGB-D相机是通过物理手段去测量深度，按照原理可以分为红外结构光和ToF。两种都是通过向外发射结构光，再根据返回的光束与发射光束进行对比，并经过算法处理，从而得到距离信息。

相比于双目相机，RGB-D相机得到深度信息要简单很多，因为RGB-D相机已经把彩色图和深度图配准了，可以直接使用图像信息和距离信息。RGB-D相机的缺点是发射红外光束到透明材料反射效果差，以及室外强光会产生干扰，这就限制了RGB-D相机的应用场景。

图像

相机成像后，生成图像。图像在计算机中是以矩阵（二维数组）的形式存储的，矩阵中的每个元素描述一个像素。比如说灰度相机，矩阵中的每个元素都代表一个灰度值。

实践：OpenCV/图像拼接

实践部分演示OpenCV图像基本操作以及RGB-D拼接和点云拼接程序，了解内参和外参相关方面的知识。

首先介绍的是OpenCV图像基本操作，代码如下：

#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 的拷贝
  // 直接赋值并不会拷贝数据（叫做C++中的浅拷贝和深拷贝，OpenCV默认的是浅拷贝），使用复制构造函数的时候，把对象A复制给对象B的时候，A和B实际上指向的是同一块区域，而不是直接复制图像数据，直接复制图像数据的话工作量比较大。
  cv::Mat image_another = image;
  // 修改 image_another 会导致 image 发生变化，进行区块赋值
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零；（0，0，100，100代表x，y，长，宽）；setTo(0)代表将区块设为0；
  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;
}

代码运行结果如下：

实验结果：

显示原始图像：

对浅拷贝结果进行数据修改，会影响原始图像，结果如下：

而深拷贝结果进行数据修改，则不会影响原始图像，结果如下：

接下来的实验是如何根据RGB-D图像生成点云，RGB-D图像会提供给R、G、B图像以及一张深度图，深度图描述了图像中每个像素的深度，实践中也给定相机内参，所以可以得到每个像素所在的空间点在什么地方，并且实践中会提供一个pose.txt给定五对图的pose。至此实践中内参和外参都已经给定了，所以可以算出每个点的世界坐标，从而生成点云图。代码如下：

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


using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> 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("./pose.txt"); //在pose.txt中把pose都读进来
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    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读取原始图像

        double data[7] = {0};
        for (auto &d:data)
            fin >> d;
        Sophus::SE3d 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::SE3d 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;
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy; //计算归一化平面的x、y坐标再乘以深度值，得到在相机坐标系下的坐标
                Eigen::Vector3d pointWorld = T * point; //同时因为相机的位姿是已知的，这里用Twc*Tc得到Tw

                Vector6d p;
                p.head<3>() = pointWorld;
                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;
}

实验结果如下：