相机之针孔模型

20170928-0929更新：修改一些内容上的错误，添加世界坐标系坐标的内容
20170908更新：公式都采用LaTex编辑

[注]
本文的内容基本上是基于《视觉SLAM十四讲》第五讲的部分内容写的，加深自己的理解。

---

引言

现在跟着高翔博士的一起做RGB-D SLAM系列学习，通过深度相机，得到一张RGB图和一张深度图。
先说一下什么是灰度图（gray scale image），比如用1个字节长度（8位）来表示1张图片的黑白程度，255代表白色，0代表黑色。
RGB图，就是红，绿，蓝三原色，每个颜色都用1个字节来表示该种颜色的深浅，比如黄色的RGB值是（255， 255， 0）。
深度图存放的深度数据d则和真实世界的距离z成某一比例。

小孔成像

小孔成像应该是大家在小学或者中学时期都做过的一个物理小实验（可是我并没有印象啊…）。
这里有一个很有趣的小视频：小孔成像科普

相机坐标系与物理成像平面

我们来定义一个相机坐标系 O-x-y-z，一个物理成像平面坐标系O’-x’-y’-z’，单位都是米。
如下图所示（手画的）：

在相机坐标系中一个点P坐标为：

$\mathbf{\begin{bmatrix}x&y&z\end{bmatrix}}^\mathrm{T}$

经过小孔O投影后落在物理成像平面上，成像点为P’坐标为:

$\mathbf{\begin{bmatrix}x\prime&y\prime\end{bmatrix}}^\mathrm{T}$

（正是许许多多个成像点组成后变成了一个完整的像）
根据三角形相似的关系，可以得到以下关系式 ：

$$\frac{f}{z}=-\frac{x\prime}{x}=-\frac{y\prime}{y}$$

为了简化模型，把物理成像平面对称到相机平面的另一边，这样一来，式子中的负号就没了。如下：

$$\begin{cases}x\prime = f\frac{x}{z} \\ y\prime = f\frac{y}{z}\end{cases}$$

以后，为了方便表达，我们也称之为针孔相机模型。

像素坐标系

在相机中，最终我们得到的是一个个像素，这就需要在物理成像平面上对像进行采样和量化。
图像是由一个个像素点组成，传统的像素坐标系是这样定义的：

这里我们定义像素点的坐标为：

$$\begin{bmatrix} u & v\end{bmatrix}^\mathrm{T}$$

从物理成像平面到像素坐标系，它们之间的关系是缩放和平移。
物理成像平面的x’轴缩放α倍后得到像素坐标系的u轴；
物理成像平面的y’轴缩放β倍后得到像素坐标系的v轴；
物理成像平面的原点平移 后得到像素坐标系的原点；
可以得到以下关系式：

$$\begin{cases}u = \alpha x\prime +c_{x} \\ v = \beta y\prime +c_{y}\end{cases}$$
将x’和y’用x,y的式子替换得到：
$$\begin{cases}u = \alpha f \frac{x}{z} +c_{x} \\ v = \beta f \frac{y}{z}+c_{y}\end{cases}$$

然后通过合并：

$$\begin{cases} f_{x} = \alpha f \\ f_{y} = \beta f \end{cases}$$

得到下面的式子：

$$\begin{cases}u = f_{x} \frac{x}{z} +c_{x} \\ v = f_{y} \frac{y}{z}+c_{y}\end{cases}$$

这样一来，我们就得到了像素坐标和相机坐标之间的关系式，不过要明白式子中参数的单位：米，像素，像素/米。

针孔模型的矩阵形式

有了上面的关系式，让我们先把它写成矩阵的形式：

$$\begin{bmatrix} u \\v \\1 \end{bmatrix} = \frac {1}{z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\y \\z \end{bmatrix} = \frac{1}{z} \textbf {KP}$$
按照习惯，把z放到等式的左边：
$$z \begin{bmatrix} u \\v \\1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\y \\z \end{bmatrix} = \textbf {KP}$$

对于这个式子作以下2点说明：

• 中间的矩阵我们把它叫做相机的内参数矩阵（camera intrinsics），好奇的宝宝可能马上就会问了，那有没有外参数矩阵呀？（下面就会讲了哦！）；
• 式子左边的是像素坐标的齐次坐标形式，这里等式的等号是严格成立的，只不过左边是齐次坐标，右边没有用到非齐次坐标。

在上面的式子中，$\begin{bmatrix} x\\y\\z \end{bmatrix}$指的是相机坐标系下的坐标，但是，我们用个办法把它换成世界坐标系下的坐标。
这就要用到旋转矩阵那一部分的知识啦！
可以参考这个帖子：旋转矩阵和变换矩阵

我们把式子写成了以下的形式：

$$z \textbf P_{uv} = z \begin{bmatrix} u \\ v \\1 \end{bmatrix}= \textbf K (\textbf R \textbf P_w+\textbf t )= \textbf KT \textbf P_w$$

• $\textbf P_{uv}$ 是像素坐标的齐次形式
• 其中$\textbf P_w$ 是世界坐标系中的坐标
• 这里的R，t 就是名为相机外参数的东西啦！也就是相机的位姿，也是SLAM中带估计的目标，代表了机器人的轨迹。

因为我们前面讲，齐次坐标形式乘以一个非零的常数都是表示一个点或者向量，那么我们可以把z丢掉，得到以下式子：

$$\textbf P_{uv} = \textbf {KT} \textbf P_w$$
但是这样一来，等号就变成了在齐次坐标系下才相等的概念，不是严格想等了，所以我们还是要想个办法，让等号能够成立。

• 第一步：我们曾经讲过Ra 和Ta 出现在等号的两头时，隐含了齐次和非齐次间的一个变换，在这里就是：
  $$\textbf K (\textbf R \textbf P_w+\textbf t )= \textbf KT \textbf P_w$$

$\textbf T \textbf P_w$ 就是又把齐次形式的世界坐标变换到相机坐标系，为了能和K 相乘，需要把$\textbf T \textbf P_w$ ，也就是齐次形式的相机坐标系坐标的最后一维扔掉，又变成了相机坐标系坐标的非齐次形式：$\begin{bmatrix} x\\y\\z \end{bmatrix}$

• 第二步：上面讲因为丢了一个z使得等式不是严格成立，那么我们在等式右边也扔掉一个z，找个什么借口呢？对呀，你看这个第一步得到的相机坐标系坐标的非齐次形式，它的第三维也是z，那么现在我们认为，这个三维坐标是某个二维平面坐标的非齐次形式变得，也就是说，我认为现在这个$\begin{bmatrix} x\\y\\z \end{bmatrix}$ 是个齐次坐标。
  那么按照齐次和非齐次的转换得到：
  $$\begin{bmatrix} x/z\\y/z\\1\end{bmatrix}$$
  这样以来，我在等式两边相当于同时除以z，那么这个等号就是严格成立的了。

归一化平面和归一化坐标

上面等式左边的$\textbf P_{uv}$ 我们看作是像素坐标的齐次形式，等号右边的看作是什么呢？可以看作是位于相机前面z=1处的平面上的点。
这个平面就叫做归一化平面，那么，我们就可以把像素坐标$\begin{bmatrix} u\\v \end{bmatrix}$看作是在归一化平面上的点进行量化测量的结果。