《视觉SLAM十四讲》读书笔记（三）

相机与图像

5.1 相机模型
- 5.1.1 针孔相机模型
- 5.1.2 畸变模型
- 5.1.3 双目相机模型
- 5.1.4 RGB-D相机模型

5.1 相机模型

5.1.1 针孔相机模型

设 O − x − y − z O-x-y-z O−x−y−z为相机坐标系，习惯上我们让 z z z轴指向相机前方， x x x向右， y y y向下（此图我们应该站在左侧看右侧）。 O O O为摄像机的光心，也是针孔模型中的针孔。现实世界的空间点 P P P，经过小孔 O O O 投影之后，落在物理成像平面 O ′ − x ′ − y ′ O'-x'-y' O′−x′−y′上，成像点为 P ′ P' P′。设 P P P 的坐标为 [ X ′ , Y ′ , Z ′ ] T [X', Y', Z']^T [X′,Y′,Z′]T， P ′ P' P′ 为 [ X ′ ′ , Y ′ , Z ′ ] T [X′',Y',Z']^T [X′′,Y′,Z′]T，并且设物理成像平面到小孔的距离为 f f f（焦距）。那么，根据三角形相似关系，有：
Z f = − X X ′ = − Y Y ′ (5.1) \frac{Z}{f}=-\frac{X}{X'}=-\frac{Y}{Y'} \tag{5.1} fZ=−X′X=−Y′Y(5.1)
其中负号表示成的像是倒立的。不过，实际相机得到的图像并不是倒像（否则相机的使用会非常不
方便）。为了让模型更符合实际，我们可以等价地把成像平面对称地放到相机前方，和三维空间点一
起放在摄像机坐标系的同一侧，如图5-2所示。这样做可以把公式中的负号去掉，使式子更加简洁：
Z f = X X ′ = Y Y ′ (5.2) \frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'} \tag{5.2} fZ=X′X=Y′Y(5.2)

把 X ′ X' X′, Y ′ Y' Y′ 放到等式左侧，整理得：
X ′ = f X Z Y ′ = f Y Z (5.3) X'=f \frac{X}{Z}\\ Y'=f \frac{Y}{Z}\tag{5.3} X′=fZXY′=fZY(5.3)

为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面 o − u − v o-u-v o−u−v。我们在像素平面得到了 P ′ P' P′ 的像素坐标： [ u , v ] T [u, v]^T [u,v]T。

像素坐标系的定义： 原点 o ′ o' o′ 位于图像的左上角， u u u 轴向右与 x x x 轴平行， v v v 轴向下与 y y y 轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在 u u u 轴上缩放了 α \alpha α倍，在 v v v 上缩放了 β \beta β倍。同时，原点平移了 [ c x , c y ] T [c_x,c_y]^T [cx,cy]T。那么， P ′ P' P′ 的坐标与像素坐标 [ u , v ] T [u, v]^T [u,v]T 的关系为：
{ u = α X ′ + c x v = β Y ′ + c y (5.4) \left\{\begin{array}{l} u=\alpha X^{\prime}+c_{x} \\ v=\beta Y^{\prime}+c_{y} \end{array}\right.\tag{5.4} {u=αX′+cxv=βY′+cy(5.4)
代人式(5.3)并把 α f \alpha f αf 合并成 f x f_{x} fx , 把 β f \beta f βf 合并成 f y f_{y} fy , 得:
{ u = f x X Z + c x v = f y Y Z + c y (5.5) \left\{\begin{array}{l} u=f_{x} \frac{X}{Z}+c_{x} \\ v=f_{y} \frac{Y}{Z}+c_{y} \end{array}\right.\tag{5.5} {u=fxZX+cxv=fyZY+cy(5.5)
其中, f f f 的单位为米, α , β \alpha, \beta α,β 的单位为像素/米, 所以 f x , f y f_{x}, f_{y} fx,fy 和 c x , c y c_{x}, c_{y} cx,cy 的单位为像素。把该式写成矩阵形式会更加简洁, 不过左侧需要用到齐次坐标, 右侧则是非齐次坐标:
( u v 1 ) = 1 Z ( f x 0 c x 0 f y c y 0 0 1 ) ( X Y Z ) ≜ 1 Z K P (5.6) \left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) \triangleq \frac{1}{Z} \boldsymbol{K} \boldsymbol{P}\tag{5.6} ⎝⎛uv1⎠⎞=Z1⎝⎛fx000fy0cxcy1⎠⎞⎝⎛XYZ⎠⎞≜Z1KP(5.6)
我们按照传统的习惯把 Z Z Z 挪到左侧:
Z ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( X Y Z ) ≜ K P (5.7) Z\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) \triangleq \boldsymbol{K} \boldsymbol{P}\tag{5.7} Z⎝⎛uv1⎠⎞=⎝⎛fx000fy0cxcy1⎠⎞⎝⎛XYZ⎠⎞≜KP(5.7)
该式中，我们把中间的量组成的矩阵称为相机的内参数矩阵（Camera Intrinsics） K K K。

考虑到在式 (5.6) 中我们使用的是 P P P 在相机坐标系下的坐标。由于相机在运动, 所以 P P P 的相机坐标应该是它的世界坐标（记为 P w \boldsymbol{P}_{w} Pw ）根据相机的当前位姿变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵 R R R 和平移向量 t t t 来描述。那么有:
Z P u v = Z [ u v 1 ] = K ( R P w + t ) = K T P w (5.8) Z \boldsymbol{P}_{u v}=Z\left[\begin{array}{c} u \\ v \\ 1 \end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}_{w}+\boldsymbol{t}\right)=\boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{w}\tag{5.8} ZPuv=Z⎣⎡uv1⎦⎤=K(RPw+t)=KTPw(5.8)
相机的位姿 R , t R,t R,t 又称为相机的外参数（Camera Extrinsics）。

投影过程还可以从另一个角度来看。式 ( 5.8 ) (5.8) (5.8) 表明，我们可以把一个世界坐标点先转换到相机坐标系, 再除掉它最后一维的数值（即该点距离相机成像平面的深度 ), 这相当于把最后一维进行归一化处理, 得到点 P P P 在相机归一化平面上的投影:
( R P w + t ) = [ X , Y , Z ] T ⏟ 相机坐标 → [ X / Z , Y / Z , 1 ] T ⏟ 归一化坐标 . (5.9) \left(\boldsymbol{R} \boldsymbol{P}_{w}+\boldsymbol{t}\right)=\underbrace{[X, Y, Z]^{\mathrm{T}}}_{\text {相机坐标 }} \rightarrow \underbrace{[X / Z, Y / Z, 1]^{\mathrm{T}}}_{\text {归一化坐标 }} .\tag{5.9} (RPw+t)=相机坐标 [X,Y,Z]T→归一化坐标 [X/Z,Y/Z,1]T.(5.9)
归一化坐标可看成相机前方 z = 1 z=1 z=1 处的平面上的一个点，这个 z = 1 z=1 z=1 平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标, 所以我们可以把像素坐标 [ u , v ] T [u, v]^{\mathrm{T}} [u,v]T 看成对归一化平面上的点进行量化测量的结果。从这个模型中也可以看出, 如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的, 这说明点的深度在投影过程中被丟失了, 所以单目视觉中没法得到像素点的深度值。

5.1.2 畸变模型

径向畸变： 由透镜形状引起的畸变。他们主要分为两大类：桶形畸变和枕形畸变，如图5-3所示。桶形畸变是由于图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

切向畸变： 相机在组装过程中不能使透镜和成像面严格平行。如图5-4所示。

考虑归一化平面上的任意一点 p p p，它的坐标为 [ x , y ] T [x, y]^T [x,y]T，也可写成极坐标的形式 [ r , θ ] T [r, \theta]^T [r,θ]T，其中 r r r 表示点 p p p 与坐标系原点之间的距离， θ \theta θ表示与水平轴的夹角。径向畸变可看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。通常假设这些畸变呈多项式关系，即：
x corrected = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y corrected = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) \begin{aligned} x_{\text {corrected }} &=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ y_{\text {corrected }} &=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{aligned} xcorrected ycorrected =x(1+k1r2+k2r4+k3r6)=y(1+k1r2+k2r4+k3r6)
其中 [ x corrected , y corrected ] T \left[x_{\text {corrected }}, y_{\text {corrected }}\right]^{\mathrm{T}} [xcorrected ,ycorrected ]T 是校正后点的归一化坐标（书里是讲反了）。另一方面, 对于切向畸变，可以使用另外的两个参数 p 1 , p 2 p_{1}, p_{2} p1,p2 来进行纠正:
x corrected = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y corrected = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{aligned} x_{\text {corrected }} &=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {corrected }} &=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{aligned} xcorrected ycorrected =x+2p1xy+p2(r2+2x2)=y+p1(r2+2y2)+2p2xy
因此, 联合式(5.10)和式(5.11), 对于相机坐标系中的一点 P \boldsymbol{P} P , 我们能够通过 5 个畸变系数找到这个点在像素平面上的正确位置:

将三维空间点投影到归一化图像平面。设它的归一化坐标为 [ x , y ] T [x, y]^{\mathrm{T}} [x,y]T 。
对归一化平面上的点计算径向畸变和切向畸变。
{ x corrected = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y corrected = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \left\{\begin{array}{l} x_{\text {corrected }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {corrected }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array}\right. {xcorrected =x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ycorrected =y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
将畸变后的点通过内参数矩阵投影到像素平面, 得到该点在图像上的正确位置
{ u = f x x corrected + c x v = f y y corrected + c y \left\{\begin{array}{l} u=f_{x} x_{\text {corrected }}+c_{x} \\ v=f_{y} y_{\text {corrected }}+c_{y} \end{array}\right. {u=fxxcorrected +cxv=fyycorrected +cy
在上面的纠正畸变的过程中, 我们使用了 5 个畸变项。实际应用中, 可以灵活选择纠正模型, 比如只选择 k 1 , p 1 , p 2 k_{1}, p_{1}, p_{2} k1,p1,p2 这 3 项等。

空间中某点 P P P在单目相机中的成像过程：

首先, 世界坐标系下有一个固定的点 P P P , 世界坐标为 P w P_{w} Pw 。
由于相机在运动, 它的运动由 R, t 或变换矩阵 T ∈ S E ( 3 ) T \in \mathrm{SE}(3) T∈SE(3) 描述。 P P P 的相机坐标为 P ~ c = R P w + t \tilde{P}_{c}= R P_{w}+t P~c=RPw+t
这时的 P ~ c \tilde{P}_{c} P~c 的分量为 X , Y , Z X, Y, Z X,Y,Z , 把它们投影到归一化平面 Z = 1 Z=1 Z=1 上, 得到 P P P 的归一化坐标: P c = [ X / Z , Y / Z , 1 ] T \boldsymbol{P}_{c}=[X / Z, Y / Z, 1]^{\mathrm{T}} Pc=[X/Z,Y/Z,1]T
有畸变时, 根据畸变参数计算 P c \boldsymbol{P}_{c} Pc 发生畸变后的坐标。
最后, P P P 的归一化坐标经过内参后, 对应到它的像素坐标: P u v = K P c ∘ P_{u v}=K P_{c \circ} Puv=KPc∘

图像去畸变的部分代码：

for each pixel// 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)double x = (u - cx) / fx, y = (v - cy) / fy; // 像素平面转归一化平面double r = sqrt(x * x + y * y); // 计算极坐标的rdouble x_corrected = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);double y_corrected = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;double u_corrected = fx * x_distorted + cx;double v_corrected = fy * y_distorted + cy;

整体代码（书里的符号命名搞混了）：

#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_8UC1int rows = image.rows, cols = image.cols;cv::Mat image_corrected = 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_corrected = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);double y_corrected = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;double u_corrected = fx * x_distorted + cx;double v_corrected = fy * y_distorted + cy;// 赋值 (最近邻插值)if (u_corrected >= 0 && v_corrected >= 0 && u_corrected < cols && v_corrected < rows) {image_corrected.at<uchar>(v, u) = image.at<uchar>((int) v_corrected, (int) corrected);} else {image_undistort.at<uchar>(v, u) = 0;}}}// 画图去畸变后图像cv::imshow("distorted", image);cv::imshow("undistorted", image_corrected);cv::waitKey();return 0;
}

5.1.3 双目相机模型

基线（记作 b b b）： 对于水平放置的双目相机，两者光圈中心的距离。

双目相机深度估计方法：
根据 Δ P P L P R \Delta P P_L P_R ΔPPLPR和 Δ P O L O R \Delta PO_L O_R ΔPOLOR的相似关系，有
z − f z = b − u L + u R b \frac{z-f}{z}=\frac{b-u_{L}+u_{R}}{b} zz−f=bb−uL+uR
稍加整理, 得：
z = f b d , d ≜ u L − u R z=\frac{f b}{d}, \quad d \triangleq u_{L}-u_{R} z=dfb,d≜uL−uR
视差（记作 d d d）： 左右图的很坐标之差。视差与距离成反比，视差越大，距离越近。同事，由于视差的单位为像素，因此双目的深度存在一个理论上的最大值，由 f b fb fb缺点。基线越长，双目测量距离越远，反之，则越近。

虽然视差计算深度的公式很简洁，但视差 d d d本身的计算比较困难。我们需要确切地知道左眼图像地某个像素出现在右眼图像地哪一个位置（即图像匹配）。当我们想计算每个像素地深度时，其计算量与精度都将称为问题，而且只有在图像纹理信息丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGA来实时计算。

双目生成深度图的代码：

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

5.1.4 RGB-D相机模型

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

通过红外结构光（Structured Light）原理测量像素距离。例子有Kinect 1 代、Project Tango 1 代、Intel RealSense 等。
通过飞行时间（Time-of-Flight，ToF）原理测量像素距离。例子有Kinect 2 代和一些现有的ToF传感器等。

无论是哪种类型，RGB-D 相机都需要向探测目标发射一束光线（通常是红外光）。在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。而在ToF 原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。ToF 的原理和激光传感器十分相似，只不过激光是通过逐点扫描获取距离的，而ToF 相机则可以获得整个图像的像素深度，这也正是RGB-D 相机的特点。

在测量深度之后，RGB-D 相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D 相机坐标，生成点云（Point Cloud）。既可以在图像层面对RGB-D数据进行处理，也可在点云层面处理。本讲的第二个实验将演示RGB-D 相机的点云构建过程。

RGB-D 相机能够实时地测量每个像素点的距离。但是，由于使用这种发射− 接收的测量方式，其使用范围比较受限。用红外光进行深度值测量的RGB-D 相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。在没有调制的情况下，同时使用多个RGB-D 相机时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D 相机在成本、功耗方面，都有一些劣势。