针孔相机模型公式推导

  针孔相机模型是使用的较多的模型，下面总结三维模型重建中的针孔相机成像过程。

  首先，空间中的一个点要在针孔相机的像平面重建，需要4步。

1.从世界坐标系变化到相机坐标系

  世界坐标系可以定义在任意位置，作为参考坐标系，相机坐标系以相机的光心看为远点，相机朝向为Z轴，朝上为Y轴，再根据右手定则确定X轴，如图（Ocam表示相机坐标系原点，Owor表示世界坐标系所选的点的原点）：

  空间中的一个点表示为
$X_w=\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]$
  这个空间中的点对应的相机中的点表达为：
$X_c=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$

  $X_c$和$X_w$之间可以通过刚体变换来进行转换（刚体变换即变换前后两点的的距离依旧保持不变的变换，可分为平移变换、旋转变换和镜像（或叫翻转）变换）：

1- 平移变换

  假设存在点(x,y,z)，将x移动a，y移动b，z移动c，到新的点$(x^{{}^{\prime }},y^{{}^{\prime }},z^{{}^{\prime }})$，则：
$x^{{}^{\prime }}=x+a$
$y^{{}^{\prime }}=y+b$
$z^{{}^{\prime }}=y+c$

  平移向量为：
$t=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$

  写成矩阵形式：
$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& a\\ 0& 1& 0& b\\ 0& 0& 1& c\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

  中间4x4的矩阵叫变换矩阵。可见，如果要平移坐标，要将坐标维度增加1，变成齐次坐标（齐次坐标（homogeneous coordinates）就是将一个原本是n维的向量用一个n+1维向量来表示，常用于投影几何）。

  在计算机图形学中，为了实现平移、旋转、缩放等图像操作，需要用到齐次坐标。

例1：世界坐标系wor相对相机坐标系cam的x、y、z分别平移了10，20，30，求次变换齐次矩阵。

$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 20\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 30\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 20\\ 0& 0& 1& 30\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

三个分量矩阵位置可以交换，因为是独立变量，互不影响。
所以，平移齐次矩阵为：$\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 20\\ 0& 0& 1& 30\\ 0& 0& 0& 1\end{array}\right]$

2- 旋转变换

例2：世界坐标系wor相对相机坐标系cam绕x轴顺时针旋转了$\alpha$（逆时针旋转为正，所以以下变换均为逆时针旋转）：
变换矩阵：$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\alpha & -sin\alpha & 0\\ 0& sin\alpha & cos\alpha & 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

再绕y轴顺时针旋转$\beta$（应该再左乘一个变换矩阵）：

$\left[\begin{array}{c}{x}^{{}^{\prime \prime }}\\ y^{{}^{\prime \prime }}\\ z^{{}^{\prime \prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\alpha & -sin\alpha & 0\\ 0& sin\alpha & cos\alpha & 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ 1\end{array}\right]$

  旋转变换有两种，一种是向量在当前坐标系内的旋转，一种是坐标系的旋转。这里推导坐标系旋转矩阵。

（1） 绕X轴旋转（逆时针）$\alpha$：

方程为：
$\{\begin{array}{l}{x}^{\prime }=x,\\ y^{\prime }=ycos\alpha +zsin\alpha ,\\ z^{\prime }=-ysin\alpha +zcos\alpha \end{array}$

写成矩阵形式：
$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\alpha & sin\alpha & 0\\ 0& -sin\alpha & cos\alpha & 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

（2） 绕Y轴旋转（逆时针）$\beta$：

方程为：
$\{\begin{array}{l}{x}^{\prime \prime }=x^{\prime }cos\beta +z^{\prime }sin\beta ,\\ y^{\prime \prime }=y^{\prime },\\ z^{\prime \prime }=-x^{\prime }sin\beta +z^{\prime }cos\beta \end{array}$

写成矩阵形式：
$\left[\begin{array}{c}{x}^{{}^{\prime \prime }}\\ y^{{}^{\prime \prime }}\\ z^{{}^{\prime \prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\beta & 0& sin\beta & 0\\ 0& 1& 0& 0\\ -sin\alpha & 0& cos\beta & 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right]$

（3）绕Z轴旋转（逆时针）$\gamma$：

方程为：
$\{\begin{array}{l}{x}^{\prime \prime \prime }=x^{\prime \prime }cos\gamma +y^{\prime \prime }sin\gamma ,\\ y^{\prime \prime \prime }=-x^{\prime \prime }sin\gamma +y^{\prime \prime }cos\beta ,\\ z^{\prime \prime \prime }=z^{\prime \prime }\end{array}$

写成矩阵形式：
$\left[\begin{array}{c}{x}^{{}^{\prime \prime \prime }}\\ y^{{}^{\prime \prime \prime }}\\ z^{{}^{\prime \prime \prime }}\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\gamma & sin\gamma & 0& 0\\ -sin\gamma & cos\gamma & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\\ z^{\prime \prime }\\ 1\end{array}\right]$

所以，坐标轴分别依次绕x，y，z轴旋转$\alpha$,$\beta$,$\gamma$的变换矩阵（前后用左乘来连接）：

$R=R_z(\gamma )R_y(\beta )R_x(\alpha )=\left[\begin{array}{cccc}cos\gamma & sin\gamma & 0& 0\\ -sin\gamma & cos\gamma & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}cos\beta & 0& sin\beta & 0\\ 0& 1& 0& 0\\ -sin\alpha & 0& cos\beta & 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\alpha & sin\alpha & 0\\ 0& -sin\alpha & cos\alpha & 0\\ 0& 0& 0& 1\end{array}\right]$

齐次矩阵$R$为$4\times 4$的矩阵。

3- 综合变换矩阵

  空间中的一个点表示为
$X_w=\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]$
  这个空间中的点对应的相机中的点表达为：
$X_c=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$

综合的（平移+旋转）变换矩阵可表示为：

$X_c=R{X}_w+t$ （此处及以下R为非齐次矩阵，$3\times 4$）

写成齐次矩阵形式（$X_c$下面的1是因为要考虑平移，所以要变成齐次矩阵才加进来的）：
$\left[\begin{array}{c}{X}_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\times \left[\begin{array}{c}{X}_w\\ 1\end{array}\right]$

($t$为平移向量：
$t=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$)

逆变换：
$X_w=R^T{X}_c-R^{T}t$

齐次矩阵形式：
$\left[\begin{array}{c}{X}_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]\times \left[\begin{array}{c}{X}_c\\ 1\end{array}\right]$

4.相机中心在世界坐标系中的位置

相机中心在相机坐标系中的位置$O_{cam}^c$

相机中心在世界坐标系中的位置$O_{cam}^w$

把相机原点转化到世界坐标系中：

$O_{cam}^c=0$

使用逆变换公式：

$X_w=R^T{X}_c-R^{T}t$

$O_{cam}^w=R^T{O}_{cam}^c-R^{T}t=-R^{T}t$

把世界坐标系原点转化到相机坐标系中：

世界坐标系原点在世界坐标系中的位置$O_{wor}^w$

世界坐标系原点在相机坐标系中的位置$O_{wor}^c$

$O_{wor}^w=0$

使用变换公式：

$X_c=R{X}_w+t$

$X{O}_{wor}^c=R{O}_{wor}^w+t=t$

可见：世界坐标系原点在相机坐标系中的表达就是平移向量$t$。

4- 相机朝向（Z轴）在世界坐标系中的方向

2. 相机坐标系到归一化像平面坐标系

  归一化像平面坐标系：是一个与物理像平面（通常是感光器件CCD所在的平面）平行，并且距离相机光心距离为1个单位的一个虚拟的坐标平面。见下图：

  归一化平面坐标$\hat{p}(x,y)$，由三角形（从三维来看是三棱锥相似）相似有：

$\frac{x_c}{x}=\frac{y_c}{y}=\frac{z_c}{z}$ (由归一化平面定义知：归一化平面的$z=1$)

即：
$x=\frac{x_c}{z_c}$
$y=\frac{y_c}{z_c}$

  写成矩阵形式：

$\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\end{array}\right]$

写成其次矩阵形式：
$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$

  经过归一化后，原来的在相机坐标下的三维信息$P_c(x_c,y_c,z_c)$变成了二维信息$\hat{p}(x,y)$，损失了z坐标的信息，由三维点变成了二维的点，这个过程即深度信息损失。

3. 归一化像平面坐标系到物理像平面坐标系

  得到归一化坐标$\hat{p}(x,y)$后，要将其转化到理像平面坐标系中（即成像原件所在平面），变成$p_{img}(x_{img},y_{img})$。注意：此处单位与归一化平面不一样，归一化平面与光心的距离可以是1米或毫米（此例子中取毫米），但是物理像平面的单位通常为像素。见下图：
  由于物理平面采用的单位是像素，需要把归一化平面的毫米单位转化为像素。

  相似三角形关系：

$\frac{X_{img}}{x}=\frac{Y_{img}}{y}=\frac{f}{z}$ (由归一化平面定义知：归一化平面的$z=1$；$X_{img}$、$Y_{img}$单位为mm)

$X_{img}=fx$
$Y_{img}=fy$ (单位mm)

$x_{img}=\alpha U=f\alpha x$
$y_{img}=\beta U=f\beta y$ (在物理平面内，mm坐标转化为像素单位的坐标)

  写成矩阵形式：

$\left[\begin{array}{c}{x}_{img}\\ y_{img}\end{array}\right]=\left[\begin{array}{c}f\alpha x\\ f\beta y\end{array}\right]=\left[\begin{array}{c}{f}_{\alpha }x\\ f_{\beta }y\end{array}\right]$

$f_{\alpha }=f\alpha$
$f_{\beta }=f\beta$

  写成其次矩阵：

$\left[\begin{array}{c}{x}_{img}\\ y_{img}\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_{\alpha }& 0& 0\\ 0& f_{\beta }& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_{\alpha }& 0& 0\\ 0& f_{\beta }& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$

  这就是相机中的点$P_c(x_c,y_c,z_c)$到物理像平面中对应点$p_{img}(x_{img},y_{img})$的变换矩阵。

  其中$f$为焦距，即物理平面到光心的距离，单位毫米。$\alpha$、$\beta$分别表示归一化平面上每单位（mm）对应包含物理平面上$x$、$y$方向像素的个数。所以，$f_{\alpha }$和$f_{\beta }$分别为以$x$、$y$方向上以像素为单位的焦距；$x_{img}$、$y_{img}$单位为像素。

4.把物理平面像坐标转到图像坐标系

  常用的图像坐标系为以图像左上角为原点，x方向朝右，y朝下的坐标系；但是物理坐标系原点在图像中央：

  设图像中央的坐标为$(u_0,v_0)$，则图像坐标（$(u,v)$，左上角为原点）为：
$\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}{x}_{img}+u_0\\ y_{img}+v_0\end{array}\right]=\left[\begin{array}{c}f\alpha x+u_0\\ f\beta y+v_0\end{array}\right]=\left[\begin{array}{c}{f}_{\alpha }x+u_0\\ f_{\beta }y+v_0\end{array}\right]$

  写成其次矩阵：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_{\alpha }& 0& u_0\\ 0& f_{\beta }& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$

  一般情况下，像素为正方形，则有$f=f_{\alpha }=f_{\beta }$

5.把上述步骤都串起来

  把上面四部串起来就是针孔相机的成像过程：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_{\alpha }& 0& u_0\\ 0& f_{\beta }& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_{\alpha }& 0& u_0\\ 0& f_{\beta }& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{X}_w\\ 1\end{array}\right]=\frac{1}{z_c}K\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

整个投影过程可变为$3\times 4$的矩阵：$P_{3\times 4}=K\left[\begin{array}{cc}R& t\end{array}\right]$

其中，$K=\left[\begin{array}{ccc}{f}_{\alpha }& 0& u_0\\ 0& f_{\beta }& v_0\\ 0& 0& 1\end{array}\right]$中包含的$f_{\alpha }$,$f_{\beta }$,$u_0$,$v_0$为相机的内参（还有径向畸变系数$k_1$,$k_2$）.

$R=R_z(\gamma )R_y(\beta )R_x(\alpha )=\left[\begin{array}{cccc}cos\gamma & sin\gamma & 0& \\ -sin\gamma & cos\gamma & 0& \\ 0& 0& 1& \end{array}\right]\times \left[\begin{array}{cccc}cos\beta & sin\beta & 0& \\ 0& 1& 0& \\ -sin\alpha & 0& cos\beta & \end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& \\ 0& cos\alpha & sin\alpha & \\ 0& -sin\alpha & cos\alpha & \end{array}\right]$

和平移向量$t=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$为外参。

外参确定的过程对应计算机视觉中的姿态估计任务，内参确定过程对应相机标定任务。

关于径向畸变系数

径向畸变成因：相机的透镜不是理想的透镜，因此会产生直线变弯曲现象。

公式：
$\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]=(1+k_1{r}^2+k_2{r}^4)\left[\begin{array}{c}x\\ y\end{array}\right]$ （可见径向畸变发生在归一化像平面上）

$r^2=x^2+y^2$

($x$,$y$为归一化平面上的点，$\tilde{x}$、$\tilde{y}$表示畸变后的在物理像平面上的点（单位为像素）)

$\left[\begin{array}{c}\tilde{u}\\ \tilde{v}\end{array}\right]=f\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]+\left[\begin{array}{c}{u}_0\\ v_0\end{array}\right]$

$k_1$,$k_2$为径向畸变系数；$r^2$表示图像距离中心点的距离的平方，由此可见，距离图像中心越远的点，畸变程度越大，见下图：

径向畸变系数估计方法—最小二乘法：

理想情况投影点的坐标：
$\left[\begin{array}{c}u\\ v\end{array}\right]=f\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{u}_0\\ v_0\end{array}\right]$

即：
$f\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}u-u_0\\ v-v_0\end{array}\right]$

畸变情况下的投影点的坐标：

$\left[\begin{array}{c}\tilde{u}\\ \tilde{v}\end{array}\right]=f\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]+\left[\begin{array}{c}{u}_0\\ v_0\end{array}\right]$

即：

$f\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]=\left[\begin{array}{c}\tilde{u}-u_0\\ \tilde{v}-v_0\end{array}\right]$

因为畸变是发生在归一化像平面上，对应的也就等价于发生在物理像平面上，所以由公式：
$\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]=(1+k_1{r}^2+k_2{r}^4)\left[\begin{array}{c}x\\ y\end{array}\right]$
得：
公式：
$f\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]=(1+k_1{r}^2+k_2{r}^4)f\left[\begin{array}{c}x\\ y\end{array}\right]$

即：
$\left[\begin{array}{c}\tilde{u}-u_0\\ \tilde{v}-v_0\end{array}\right]=(1+k_1{r}^2+k_2{r}^4)\left[\begin{array}{c}u-u_0\\ v-v_0\end{array}\right]$

即：
$\tilde{u}-u_0=(1+k_1{r}^2+k_2{r}^4)(u-u_0)$

$\tilde{u}=(1+k_1{r}^2+k_2{r}^4)u-(k_1{r}^2+k_2{r}^4)u_0$

$\tilde{u}-u=(k_1{r}^2+k_2{r}^4)u-(k_1{r}^2+k_2{r}^4)u_0$

$\tilde{u}-u=k_1{r}^2(u-u_0)+k_2{r}^4(u-u_0)$

同理：
$\tilde{v}-v=k_1{r}^2(v-v_0)+k_2{r}^4(v-v_0)$

写成矩阵：
$\left[\begin{array}{cc}(u-u_0)r^2& (u-u_0)r^4\\ (v-v_0)r^2& (v-v_0)r^4\end{array}\right]\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]=\left[\begin{array}{c}\tilde{u}-u\\ \tilde{v}-v\end{array}\right]$

左边表示无畸变的点，右边表示有畸变的点。所以，只需要知道$\left[\begin{array}{c}\tilde{u}\\ \tilde{v}\end{array}\right]$和$\left[\begin{array}{c}u\\ v\end{array}\right]$的对应关系，就可以用最小二乘法计算$k_1$和$k_2$了。