视觉SLAM理论到实践系列文章
下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此
视觉SLAM理论到实践系列文章链接
下面是专栏地址：
视觉SLAM理论到实践专栏
文章目录
前言
高翔博士的《视觉SLAM14讲》学习笔记的系列记录
视觉SLAM理论到实践系列（四）——相机模型
相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程能够用一个几何模型进行描述。
这个模型有很多种，其中最简单的称为针孔模型。
同时，由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变。因此，我们使用针孔和畸变两个模型来描述整个投影过程。
这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数（Intrinsics)。
针孔相机模型
小孔模型能够把三维世界中的蜡烛投影到一个二维成像平面，我们可以用这个简单的模型来解释相机的成像过程，如下图所示。
原始形式：
Z
f
=
−
X
X
′
=
−
Y
Y
′
.
{\frac{Z}{f}}=-{\frac{X}{X^{\prime}}}=-{\frac{Y}{Y^{\prime}}}.
fZ=−X′X=−Y′Y.
翻转到前面
Z
f
=
X
X
′
=
Y
Y
′
.
{\frac{Z}{f}}={\frac{X}{X^{\prime}}}={\frac{Y}{Y^{\prime}}}.
fZ=X′X=Y′Y.
整理之：
X
′
=
f
X
Z
Y
′
=
f
Y
Z
X^{\prime}=f{\frac{X}{Z}} \\ Y^{\prime}=f{\frac{Y}{Z}}
X′=fZXY′=fZY
像素坐标系通常的定义方式是：原点 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
.
\begin{cases}u=\alpha X'+c_x\\ v=\beta Y'+c_y\end{cases}.
{u=αX′+cxv=βY′+cy.
代入 { X ′ = f X Z Y ′ = f Y Z \begin{cases}X^{\prime}=f{\frac{X}{Z}}\\Y^{\prime}=f{\frac{Y}{Z}}\end{cases} {X′=fZXY′=fZY
得展开形式：
{
u
=
f
x
X
Z
+
c
x
v
=
f
y
Y
Z
+
c
y
.
\begin{cases}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{cases}.
{u=fxZX+cxv=fyZY+cy.
其中，
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
.
\begin{pmatrix}u\\ v\\ 1\end{pmatrix}=\frac{1}{Z}\begin{pmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{pmatrix}\begin{pmatrix}X\\ Y\\ Z\end{pmatrix}\triangleq\frac{1}{Z}K P.
uv1
=Z1
fx000fy0cxcy1
XYZ
≜Z1KP.
这里左侧是齐次坐标，右侧是非齐次坐标，中间矩阵称为内参数
我们习惯性地把
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
.
Z\begin{pmatrix}u\\ v\\ 1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{pmatrix}\begin{pmatrix}X\\ Y\\ Z\end{pmatrix}\triangleq KP.
Z
uv1
=
fx000fy0cxcy1
XYZ
≜KP.
该式中，我们把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics)矩阵K。
通常认为相机的内参在出厂之后是固定的，不会在使用过程中发生变化。
有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。（如著名的单目棋盘格张正友标定法）
注意：同一直线上的投影点仍是同一个
相机的位姿由它的旋转矩阵
R
R
R 和平移向量
t
t
t 来描述，除内参外，相机坐标系与世界坐标系还相差一个变换：
Z
P
u
v
=
Z
[
u
v
1
]
=
K
(
R
P
w
+
t
)
=
K
T
P
w
ZP_{uv}=Z\left[\begin{array}{l}u\\ v \\1 \end{array}\right]=K\left(RP_{w}+t\right)=KTP_{w}
ZPuv=Z
uv1
=K(RPw+t)=KTPw
这里
R
,
t
R, t
R,t 或
T
T
T 称为相机的外参数(Camera Extrinsics
注：右侧式子隐含了一次非齐次到齐次的变换（见书）P99
相比于不变的内参，外参会随着相机运动发生改变，也是SLAM中待估计的目标，代表着机器人的轨迹。
投影过程还可以从另一个角度来看。我们可以把一个世界坐标点先转换到相机坐标系，再除掉它最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点
P
P
P 在相机归一化平面上的投影：
(
R
P
w
+
t
)
=
[
X
,
Y
,
Z
]
T
⏟
相机坐标
→
[
X
/
Z
,
Y
/
Z
,
1
]
T
⏟
归一化坐标
.
(RP_{w}+t)=\underbrace{[X,Y,Z]^{\mathrm{T}}}_{相机坐标} \to \underbrace{[X/Z,Y/Z,1]^{\mathrm{T}}}_{归一化坐标}.
(RPw+t)=相机坐标
[X,Y,Z]T→归一化坐标
[X/Z,Y/Z,1]T.
归一化坐标可看成相机前方之=1处的平面上的一个点，这个
z
=
1
z=1
z=1 平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标
[
u
,
v
]
T
[u,v]^T
[u,v]T 看成对归一化平面上的点进行量化测量的结果。
注意，在实际计算中需要检查 Z Z Z 是否为正，因为负数 Z Z Z 也可以通过这个方式得到归一化平面上的一个点，但相机并不会拍到成像平面后方的景物。
从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。
投影顺序：世界——相机——归一化平面——像素
激光数据的观测模型更加简单
畸变模型
由透镜形状引起的畸变（Distortion,也叫失真)称为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。
越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类：桶形畸变和枕形畸变，如图所示。
桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行，所以也会引入切向畸变，如图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
d
i
s
t
o
r
t
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
d
i
s
t
o
r
t
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
\begin{gathered} x_{distorted} =x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}) \\ y_{distorted} =y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}) \end{gathered}
xdistorted=x(1+k1r2+k2r4+k3r6)ydistorted=y(1+k1r2+k2r4+k3r6)
其中，
[
x
d
i
s
t
o
r
t
e
d
,
y
d
i
s
t
o
r
t
e
d
]
T
[x_{distorted},y_{distorted}]^T
[xdistorted,ydistorted]T 是畸变后点的归一化坐标。
另外，对于切向畸变，可以使用另外两个参数
p
1
,
p
2
p_1,p_2
p1,p2 进行纠正：
x
d
i
s
t
o
r
t
e
d
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
d
i
s
t
o
r
t
e
d
=
y
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
\begin{gathered} x_{distorted} =x+2p_{1}x y+p_{2}\left(r^{2}+2x^{2}\right) \\ y_{distorted} =y+p_1(r^2+2y^2)+2p_2xy \end{gathered}
xdistorted=x+2p1xy+p2(r2+2x2)ydistorted=y+p1(r2+2y2)+2p2xy
因此，联合上面两式，对于相机坐标系中的一点P,我们能够通过5个畸变系数找到这个点在像素平面上的正确位置：
（1）将三维空间点投影到归一化图像平面。设它的归一化坐标为 [ x , y ] T [x,y]^T [x,y]T。
（2）对归一化平面上的点计算径向畸变和切向畸变。
{
x
d
i
s
t
o
r
t
e
d
=
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
d
i
s
t
o
r
t
e
d
=
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_{\mathrm{distorted}}=x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+2p_{1}x y+p_{2}(r^{2}+2x^{2})}\\ {y_{\mathrm{distorted}}=y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+p_{1}(r^{2}+2y^{2})+2p_{2}x y}\\ \end{array}\right.
{xdistorted=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
（3）将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
{
u
=
f
x
x
distorted
+
c
x
v
=
f
y
y
distoned
+
c
y
.
\begin{cases}u=f_x x_{\text{distorted}}+c_x\\ v=f_y y_{\text{distoned}}+c_y\end{cases}.
{u=fxxdistorted+cxv=fyydistoned+cy.
在上面的纠正畸变的过程中，我们使用了5个畸变项。实际应用中，可以灵活选择纠正模型比如只选择
k
1
,
p
1
,
p
2
k_1,p_1,p_2
k1,p1,p2 这3项。
小结
双目相机模型
仅根据一个像素，我们无法确定这个空间点的具体位置。这是因为，从相机光心到归一化平面连线上的所有点，都可以投影至该像素上。只有当 P P P 的深度确定时（比如通过双目或RGB-D相机），我们才能确切地知道它的空间位置，如图所示
测量像素距离（或深度）的方式有很多种，比如人眼就可以根据左右眼看到的景物差异（或称视差)判断物体与我们的距离。
双目相机的原理亦是如此：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。下面简单介绍双目相机的成像原理（如图所示)。
双目相机的成像模型： O L , O R O_L,O_R OL,OR 为左右光圈中心，方框为成像平面， f f f 为焦距。 u L u_L uL 和 u R u_R uR 为成像平面的坐标。请注意，按照图中坐标定义， u R u_R uR 应该是负数，所以图中标出的距离为 − u R -u_R −uR
双目相机一般由左眼相机和右眼相机两个水平放置的相机组成。当然也可以做成上下两个目，不过我们见到的主流双目都是做成左右形式的。
在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的，意味着两个相机的光圈中心都位于 x x x 轴上。两者之间的距离称为双目相机的基线（记作 b b b),是双目相机的重要参数。
考虑一个空间点
P
P
P ，它在左眼相机和右眼相机各成一像，记作
P
L
,
P
R
P_L,P_R
PL,PR。由于相机基线
的存在，这两个成像位置是不同的。理想情况下，由于左右相机只在
x
x
x 轴上有位移，所以
P
P
P 的像也只在
x
x
x 轴（对应图像的
u
u
u 轴）上有差异。记它的左侧坐标为
u
L
u_L
uL ，右侧坐标为
u
R
u_R
uR ，几何关系如上图右侧所示。根据
△
P
P
L
P
R
△PP_LP_R
△PPLPR 和
△
P
O
L
O
R
△PO_LO_R
△POLOR 的相似关系，有
z
−
f
z
=
b
−
u
L
+
u
R
b
.
\frac{z-f}{z}=\frac{b-u_\mathrm{L}+u_\mathrm{R}}{b}.
zz−f=bb−uL+uR.
稍加整理，得
z
=
f
b
d
,
d
=
def
u
L
−
u
R
.
z=\frac{fb}{d},\quad d\overset{\text{def}}{=}u_L-u_R.
z=dfb,d=defuL−uR.
其中d定义为左右图的横坐标之差，称为视差。
根据视差，我们可以估计一个像素与相机之间的距离。视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由 b b b 确定。
我们看到，基线越长，双目能测到的最大距离就越远；反之，小型双目器件则只能测量很近的距离。相似地，我们人眼在看非常远的物体时（如很远的飞机)，通常不能准确判断它的距离。
虽然由视差计算深度的公式很简洁，但视差 d d d 本身的计算却比较困难。
我们需要确切地知道左眼图像的某个像素出现在右眼图像的哪一个位置（即对应关系），这件事也属于“人类觉得容易而计算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。
RGB-D相机模型
相比于双目相机通过视差计算深度的方式，RGB-D相机的做法更“主动”，它能够主动测量
每个像素的深度。目前的RGB-D相机按原理可分为两大类（如图5-7所示）：
- 通过红外结构光(Structured Light)原理测量像素距离。例子有Kinect1代、Project Tango 1代、Intel RealSense等。
- 通过飞行时间（Time-of-Flight,ToF)原理测量像素距离。例子有Kinect2代和一些现有的ToF传感器等。
无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线（通常是红外光）。在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。
RGB-D相机能够实时地测量每个像素点的距离。但是，由于使用这种发射一接收的测量方
式，其使用范围比较受限。用红外光进行深度值测量的RGB-D相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。在没有调制的情况下，同时使用多个RGB-D相机时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D相机在成本、功耗方面，都有一些劣势。
补充
了解常见标定方法：
去了解如何使用matlab、opencv标定相机，弄清楚各种标定方法的精度和流程区别
学习Opencv库的基本使用：
了解snap安装方法，以及与apt的区别
了解docker吗，以后用ROS迟早会用到的：
本章习题
图像去畸变 (3 分，约 1 小时)
现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。本次作业，你将尝试如何对一张图像去畸变，得到畸变前的图像。
图 1: 测试图像
图1是本次习题的测试图像(code/test.png),来自EuRoC数据集凸。可以明显看到实际的柱子、箱子的直线边缘在图像中被扭曲成了曲线。这就是由相机畸变造成的。根据我们在课上的介绍，畸变前后的坐标变换为：
{
x
d
i
s
t
o
r
t
e
d
=
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
d
i
s
t
o
r
t
e
d
=
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_{\mathrm{distorted}}=x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+2p_{1}x y+p_{2}(r^{2}+2x^{2})}\\ {y_{\mathrm{distorted}}=y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})+p_{1}(r^{2}+2y^{2})+2p_{2}x y}\\ \end{array}\right.
{xdistorted=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
其中
x
,
y
x,y
x,y 为去畸变后的坐标，
x
d
i
s
t
o
r
t
e
d
,
y
d
i
s
t
o
r
t
e
d
x_{\mathrm{distorted}},y_{\mathrm{distorted}}
xdistorted,ydistorted 为去畸变前的坐标。现给定参数：
k
1
=
−
0.28340811
,
k
2
=
0.07395907
,
p
1
=
0.00019359
,
p
2
=
1.76187114
e
−
05.
k_1=-0.28340811,k_2=0.07395907,p_1=0.00019359,p_2=1.76187114e-05.
k1=−0.28340811,k2=0.07395907,p1=0.00019359,p2=1.76187114e−05.
以及相机内参
f
x
=
458.654
,
f
y
=
457.296
,
c
x
=
367.215
,
c
y
=
248.375.
f_x=458.654,f_y=457.296,c_x=367.215,c_y=248.375.
fx=458.654,fy=457.296,cx=367.215,cy=248.375.
请根据undistort_image.cpp文件中内容，完成对该图像的去畸变操作。
注：本题不要使用OpenCV自带的去畸变函数，你需要自己理解去畸变的过程。我给你准备的程序中已经有了基本的提示。作为验证，去畸变后图像如图目所示。如你所见，直线应该是直的。
图 2: 验证图像
双目视差的使用 (2 分，约 1 小时)
双目相机的一大好处是可以通过左右目的视差来恢复深度。课程中我们介绍了由视差计算深度的过程。本题，你需要根据视差计算深度，进而生成点云数据。本题的数据来自Ktti数据集。
Ktti中的相机部分使用了一个双目模型。双目采集到左图和右图，然后我们可以通过左右视图恢复出深度。经典双目恢复深度的算法有BM(Block Matching,)SGBM(Semi-Global Matching)等，但本题不探讨立体视觉内容（那是一个大问题）。我们假设双目计算的视差已经给定，请你根据双目模型，画出图像对应的点云，并显示到Pangolin中。
本题给定的左右图见code/left.png和code/right…png,视差图亦给定，见code/right.png。双目的参数如下：
f
x
=
718.856
,
f
y
=
718.856
,
c
x
=
607.1928
,
c
y
=
185.2157.
f_x=718.856,f_y=718.856,c_x=607.1928,c_y=185.2157.
fx=718.856,fy=718.856,cx=607.1928,cy=185.2157.
且双目左右间距（即基线）为：
d
=
0.573
m
d=0.573\space \text{m}
d=0.573 m
请根据以上参数，计算相机数据对应的点云，并显示到Pangolin中。程序请参考code/disparity.cpp文件。
图 3: 双目图像的左图、右图与视差
作为验证，生成点云应如图 4 所示。
图 4: 双目生成点云结果
214
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
