视觉SLAM十四讲笔记-第五讲相机与图像

Ⅰ.单目相机模型

一、针孔相机模型及公式推导

二、畸变模型

三、成像过程

Ⅱ.双目相机模型

Ⅲ.RGB-D 相机模型

Ⅳ.计算机中图像的表示

Ⅴ.牛刀小试-代码实践

一、读取图像数据（灰度图为例）

二、畸变模型的矫正

三、双目相机

由于理论部分比较简单，仅涉及部分数学相似图形推理，故这部分只给出图形与公式，方便以后回顾。

Ⅰ.单目相机模型

一、针孔相机模型及公式推导

关于针孔相机模型，我们需要事先理清楚一共涉及到的坐标系。涉及到的坐标系共4个：

（PS：图像坐标系和像素坐标系位于同一平面，二者相差一个缩放因子及偏移量。）

主体模型如图所示，根据特殊结构可获得对应物理关系：

我们可设三维空间点 P 坐标为 P = [ X , Y , Z ]T，投影到图像坐标系为 P' = [ X' , Y' , Z' ]T，且易得知 Z'= f （焦距）。由相似三角形相关知识，可进行以下推导：

针孔模型会获得一个倒像，但相机本身会对图形进行预处理，因此我们可以将负号去掉：

整理得：

像素坐标系通常的定义方式是：原点 o′ 位于图像的左上角， u 轴向右与 x 轴平行， v轴向下与 y 轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在 u 轴上缩放了 α 倍，在 v 上缩放了 β 倍。同时，原点平移了 [cx , cy]T。那么， P ′ 的坐标与像素坐标 [u , v]T 的关系为：

代入可得：

其中， f 的单位为米， α 和 β 的单位为 像素每米 ，所以 fx，fy 的单位为像素。把该式写成矩阵形式，会更加简洁，不过左侧需要用到齐次坐标：

值得一提的是，单目相机无法直接获取深度信息，只好将其归一化，令Z=1，可以理解为在同一线上不同深度的点都会被投影到同一处地方，导致缺少了Z深度信息，所以这里的Z不会起到表示深度的作用。

该式中，我们把中间的量组成的矩阵称为相机的内参数矩阵（Camera Intrinsics） K。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。

有内参也会有外参，外参由R（旋转矩阵）、t（平移向量）或T（变换矩阵）表示：

上式两侧都是齐次坐标。因为齐次坐标乘上非零常数后表达同样的含义，所以可以简单地把 Z 去掉：

归一化处理：

这时 Pc 可以看成一个二维的齐次坐标，称为归一化坐标。它位于相机前方 z = 1 处的平面上。该平面称为归一化平面。由于 Pc 经过内参之后就得到了像素坐标，所以可以把像素坐标 [u; v]T，看成对归一化平面上的点进行量化测量的结果。

小结提炼**：

比较重要的公式，由归一化坐标（ Z = 1）到像素坐标的转换式子，注意fx和fy表示的相对意义：

二、畸变模型

1.发生畸变的原因：为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响: 一是透镜自身的形状对光线传播的影响，二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像面时的位置发生变化。

2.不同畸变的影响：主要分为两大类，桶形畸变和枕形畸变

对于径向畸变，无论是桶形畸变还是枕形畸变，由于它们都是随着离中心的距离增加而增加。可以用一个多项式函数来描述畸变前后的坐标变化：这类畸变可以用和距中心距离有关的二次及高次多项式函数进行纠正：

需要了解的是，对于畸变较小的图像中心区域，畸变纠正主要是 k1起作用。而对于畸变较大的边缘区域主要是 k2 起作用。普通摄像头用这两个系数就能很好的纠正径向畸变。对畸变很大的摄像头，比如鱼眼镜头，可以加入 k3 畸变项对畸变进行纠正。

对于切向畸变，可以使用另外的两个参数 p1，p2 来进行纠正:

需要熟悉和了解的也就是上述两个方程组。

将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

三、成像过程

首先，世界坐标系下有一个固定的点 P，世界坐标为 Pw；
由于相机在运动，它的运动由 R， t 或变换矩阵 T 描述。 P 的相机坐标为：Pc= RPw + t。
这时的 Pc 仍有 X， Y， Z 三个量，把它们投影到归一化平面 Z = 1 上，得到 P 的归一化相机坐标： Pc = [X/Z; Y /Z; 1]T.
P 的归一化坐标经过内参后，对应到它的像素坐标： Pu,v = KPc。

Ⅱ.双目相机模型

主体模型如图所示：

双目模型较为简单，只有两条等式。根据三角形 P -PL -PR 和 P -OL -OR的相似关系，有：

Ⅲ.RGB-D 相机模型

目前的 RGB-D 相机按原理可分为两大类：

通过红外结构光（Structured Light）来测量像素距离的。
通过飞行时间法（Time-of-flight, ToF）原理测量像素距离的。

Ⅳ.计算机中图像的表示

从最简单的图像——灰度图开始说起。在一张灰度图中，每个像素位置 (x; y) 对应到一个灰度值 I，所以一张宽度为 w，高度为 h 的图像，数学形式可以记成一个矩阵：

例如常见的灰度图中，我们用 0-255 之间整数（即一个 unsigned char，一个字节）来表达图像的灰度大小在程序中，图像以一个二维数组形式存储。它的第一个下标则是指数组的行，而第二个下标是列。在图像中，数组的行数对应图像的高度，而列数对应图像的宽度。

需要注意一下，RGB-D通道的顺序可能有所不同。对于每一个像素，就要记录它的 R,G,B 三个数值，每一个数值就称为一个通道。例如，最常见的彩色图像有三个通道，每个通道都由 8 位整数表示。在这种规定下，一个像素占据了 24 位空间。如果我们还想表达图像的透明度时，就使用 R,G,B,A 四个通道来表示它。

Ⅴ.牛刀小试-代码实践

在例程中，演示如下几个操作：图像读取、显示、像素遍历、拷贝、赋值等。

使用OpenCV前，需要在CMakeLists.txt中加入如下项：

project(ProjectName)

add_executable(ProjectName imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(ProjectName ${OpenCV_LIBS})

一、读取图像数据（灰度图为例）

#include <iostream>
#include <chrono>       //用于计算时间戳

using namespace std;

#include <opencv2/core/core.hpp>    //关于opencv的头文件
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char **argv) {// 1.读取argv[1]指定的图像cv::Mat image;image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像
// 2.判断图像文件是否正确读取if (image.data == nullptr) { //数据不存在,可能是文件不存在cerr << "文件" << argv[1] << "不存在." << endl;return 0;}
// 文件顺利读取, 首先输出一些基本信息//    可以看出Mat类型变量在读取图像的时候可获取一定量的信息cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;cv::imshow("image", image);      // 用cv::imshow显示图像cv::waitKey(0);                  // 暂停程序,等待一个按键输入后继续进行下面程序
// 3.判断image的类型if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {// 图像类型不符合要求cout << "请输入一张彩色图或灰度图." << endl;return 0;}
// 4.遍历图像, 请注意以下遍历方式亦可使用于随机像素访问// 4.1 使用 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个通道的值}}}// 4.2 记录结束时间t2chrono::steady_clock::time_point t2 = chrono::steady_clock::now();// 4.3 相减得到程序运行的大致时间chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;
return 0;
}

二、畸变模型的矫正

展示一个大致流程，实际相机内参需要根据具体相机而定：

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

using namespace std;
int main(int argv , char** argc){// 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。string image_file = "the_path_to_image";   // 输入图像路径// 畸变参数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_8UC1int 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;          //原始数据x和ydouble r = sqrt(x * x + y * y);                       //r的定义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;       //修正后的x和ydouble 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;}

三、双目相机

#include <opencv2/opencv.hpp>
#include <vector>
#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";

// 在pangolin中画图，已写好，无需调整
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;
// 读取图像cv::Mat left = cv::imread(left_file, 0);cv::Mat right = cv::imread(right_file, 0);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);
// 生成点云vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2for (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;double y = (v - cy) / fy;double depth = fx * b / (disparity.at<float>(v, u));point[0] = x * depth;point[1] = y * depth;point[2] = depth;
pointcloud.push_back(point);}
cv::imshow("disparity", disparity / 96.0);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) {glColor3f(p[3], p[3], p[3]);glVertex3d(p[0], p[1], p[2]);}glEnd();pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}return;
}

---------------------------------------------------------------------------------------------------------------------------

以上为笔者学习高博《视觉SLAM十四讲》所做笔记，若有误请见谅。