双目摄像头标定与测距（一）-- 齐次坐标，世界坐标到像素坐标

最近重新研究了一下双目的标定，然而回头一看，不找找代码自己都没法实现出来了，即使有学过但都记忆模糊。果然对新手而言，还是得做笔记。
对于标定而言，如果只是想单纯得到相机参数，用matlab会比opencv实现的效果更准确，matlab自带的相机标定工具箱也是简单上手的，使用教程也有很多资源。以下仅是对原理的简单描述。

齐次坐标

在之前得先了解一下 ‘齐次坐标’ 这个概念，其主要应用在计算机图形计算上，简单理解就是为了简化几何变换在计算机上的计算。例如：

平移 $t$ ：坐标 $(x, y)$ 分别向上，向右平移 $dx,dy$ 单位，即 $(x' - y')=(x+dx,y+dy)$ ，用矩阵表示：
$\left[ {\begin{array}{*{20}{c}}{x'}\\{y'}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}x\\y \end{array}} \right] + \left[ {\begin{array}{*{20}{c}}{dx}\\{dy}\end{array}} \right]\$
旋转 $R$ ：在二维空间中，对于旋转角度为 $\theta$ ，有 $\left[ {\begin{array}{*{20}{c}}{x'}\\{y'}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{\cos \theta }&{ - \sin \theta }\\{\sin \theta }&{\cos \theta }\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}x\\y\end{array}} \right]\$
为了合并矩阵运算的乘法与加法，所以引用了齐次坐标，即：
$\left[ {\begin{array}{*{20}{c}}{x'}\\{y'}\\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{\cos \theta }&{ - \sin \theta }&{\rm{0}}\\{\sin \theta }&{\cos \theta }&{\rm{0}}\\0&0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}{\rm{1}}&{\rm{0}}&{tx}\\{\rm{0}}&{\rm{1}}&{ty}\\{\rm{0}}&{\rm{0}}{\rm{1}} \end{array}} \right] * \left[ {\begin{array}{*{20}{c}}x\\y\\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{\cos \theta }&{ - \sin \theta }&{\left( {1 - \cos \theta } \right)tx + ty * \sin \theta }\\{\sin \theta }&{\cos \theta }&{\left( {1 - \cos \theta } \right)ty - tx * \sin \theta }\\0&0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}x\\y\\1\end{array}} \right]$
另一方面，在实际的投影空间里笛卡尔坐标难以进行表达，因为两条平行线会绝对平行，放在投影空间则不然，例如笛卡尔坐标下笔直平行的马路在视野里最终会无限逼近于无穷远一点，楼再建高点就能互通了。。

齐次坐标就是讲原来n维的向量用n+1维表示，用 $p'=Ap$ ( $A$ 指 $R$ 和 $t$ 组成的转换矩阵)方式表示几何变换。将 $(x, y)$ 附加第三个坐标，于是每个坐标用一个三元组 $(x,y,w)$ 表示，称为点 $(x,y)$ 的齐次坐标。一般来说当 $w$ 不为0时，采用 $w=1$ ，并将 $(\frac{x}{w},\frac{y}{w})$ 称为齐次点 $(x,y,w)$ 的笛卡尔坐标。
有关齐次坐标这里只是片面的解释与理解

坐标转换关系图

世界坐标与相机坐标

从世界坐标系 $\left( {{X_w},{Y_w},{Z_w}} \right)$ 转换到相机坐标系，属于刚体变换(在三维空间中，把一个几何物体作旋转，平移的运动，称之刚体变换)，参考齐次坐标的示范，当拓展到三维时 $\left( {{X_W},{Y_W},{Z_W}} \right) \to\left( {{X_C},{Y_C},{Z_C}} \right)$

$\left[ {\begin{array}{*{20}{c}}{{X_C}}\\{{Y_C}}\\{{Z_C}}\\1\end{array}} \right] = \left[{\begin{array}{*{20}{c}}{{l_{00}}}&{{l_{01}}}&{{l_{02}}}&{{t_x}}\\{{l_{10}}}&{{l_{11}}}&{{l_{12}}}&{{t_y}}\\{{l_{20}}}&{{l_{21}}}&{{l_{22}}}&{{t_z}}\\0&0&0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}} {{X_W}}\\{{Y_W}}\\{{Z_W}}\\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}R&T\\0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}{{X_W}}\\{{Y_W}}\\{{Z_W}}\\1 \end{array}} \right]$

$R$ 为3x3的旋转矩阵， $T$ 为平移向量， $R,T$ 两个外参组成世界坐标到相机坐标的转换矩阵

相机坐标到图像坐标

image.png

摄影机坐标系的原点为摄像机光心，x轴与y轴与图像的X,Y轴平行，z轴为摄像机光轴，它与图像平面垂直，以此构成的空间直角坐标系称为摄像机坐标系，也称为相机坐标系，摄像机坐标系是三维坐标系。
从相机坐标系到图像坐标系，是3D到2D的转换，属于透视投影，下图更清晰表现出来

由相似三角形，得:
$\frac{{AB}}{{oC}} = \frac{{PB}}{{pC}} = \frac{{A{O_C}}}{{o{O_C}}} \to \frac{{{X_C}}}{x} = \frac{{{Y_C}}}{y} = \frac{{{Z_C}}}{f}$ $\downarrow$ $x = f\frac{{{X_C}}}{{{Z_C}}},y = f\frac{{{Y_C}}}{{{Z_C}}}$ 通过变换可得： ${Z_C}\left[ {\begin{array}{*{20}{c}}x\\y\\1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{x{Z_C}}\\{y{Z_C}}\\{{Z_C}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}}f&0&0&0\\0&f&0&0\\0&0&1&0 \end{array}} \right] * \left[ {\begin{array}{*{20}{c}}{{X_C}}\\{{Y_C}}\\{{Z_C}}\\1\end{array}} \right]$
鉴于文章开头齐次坐标的概念， $Zc$ 作为系数，在图像坐标系上， $\left[ {\begin{array}{*{20}{c}} x\\y\\1\end{array}} \right] = {Z_C}\left[ {\begin{array}{*{20}{c}}x\\y\\1\end{array}} \right]$

图像坐标到像素坐标

在做图像处理的时候，我们对于图片像素点的读取，都是以图像左上角为原点，向右向下分别为 $x,y$ 的正方向，为了方便操作，我们需要从图像坐标转换到像素坐标。

其中 ${u_0},{v_0}$ 代表图像坐标原点在像素坐标的位置，图像坐标的单位是mm，像素坐标的单位是pix，对于两者的转换， ${d_x},{d_y}$ 分别表示一个pix在行列上等于多少个mm。对此则有：
$\left\{ {\begin{array}{*{20}{c}}{u = \frac{x}{{dx}} + {u_0}}\\{v = \frac{y}{{dy}} + {v_0}}\end{array}} \right\}$ $\left[ {\begin{array}{*{20}{c}}u\\v\\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\frac{1}{{dx}}}&0&{{u_0}}\\0&{\frac{1}{{dy}}}&{{v_0}}\\0&0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}x\\y\\1\end{array}} \right]$

总结

如此一来，相机的成像原理以及对应的坐标关系可以梳理成：

$\begin{array}{l}{Z_C}\left[ {\begin{array}{*{20}{c}}u\\v\\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{\frac{1}{{dx}}}&0&{{u_0}}\\0&{\frac{1}{{dy}}}&{{v_0}}\\0&0&1\end{array}} \right] * {Z_C}\left[ {\begin{array}{*{20}{c}}x\\y\\1\end{array}} \right]\\ = \left[ {\begin{array}{*{20}{c}} {\frac{1}{{dx}}}&0&{{u_0}}\\0&{\frac{1}{{dy}}}&{{v_0}}\\0&0&1\end{array}} \right]\left[ {\begin{array}{*{20}{c}}f&0&0&0\\0&f&0&0\\ 0&0&1&0 \end{array}} \right] * \left[ {\begin{array}{*{20}{c}} {{X_C}}\\{{Y_C}}\\{{Z_C}}\\1\end{array}} \right]\\ = \left[ {\begin{array}{*{20}{c}} {\frac{1}{{dx}}}&0&{{u_0}}\\0&{\frac{1}{{dy}}}&{{v_0}}\\0&0&1\end{array}} \right]\left[ {\begin{array}{*{20}{c}}f&0&0&0\\0&f&0&0\\0&0&1&0\end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{l_{00}}}&{{l_{01}}}&{{l_{02}}}&{{t_x}}\\{{l_{10}}}&{{l_{11}}}&{{l_{12}}}&{{t_y}}\\ {{l_{20}}}&{{l_{21}}}&{{l_{22}}}&{{t_z}}\\0&0&0&1\end{array}} \right] * \left[ {\begin{array}{*{20}{c}}{{X_W}}\\ {{Y_W}}\\{{Z_W}}\\1\end{array}} \right]\end{array}$

其中，相机内参： $\left[ {\begin{array}{*{20}{c}}{\frac{1}{{dx}}}&0&{{u_0}}\\0&{\frac{1}{{dy}}}&{{v_0}}\\0&0&1\end{array}} \right]\left[ {\begin{array}{*{20}{c}}f&0&0&0\\0&f&0&0\\0&0&1&0\end{array}} \right]$ ，相机外参： $\left[ {\begin{array}{*{20}{c}} {{l_{00}}}&{{l_{01}}}&{{l_{02}}}&{{t_x}}\\{{l_{10}}}&{{l_{11}}}&{{l_{12}}}&{{t_y}}\\{{l_{20}}}&{{l_{21}}}&{{l_{22}}}&{{t_z}}\\0&0&0&1\end{array}} \right]$

以上是不考虑畸变情况下的转换，关于标定矫正将会在下一篇继续研究

参考：https://www.cnblogs.com/zyly/p/9366080.html