双目视觉模型和三角定位

任务	场景数据	相机数据	输入	输出
Pose Estimation	已知	测得	相机视野内各特征点在空间中的位置	相机位姿
Triangulation	测得	已知	双目相机位姿和相机视野内特征点在图片上的位置	各特征点在空间中的位置
Epipolar Geometry	部分已知	测得	双目相机视野内特征点在图片上的位置	相机位姿和各特征点在空间中的位置
Reconstruction	测得	测得	相机视野内2D图片	相机视野内各点在空间中的位置

三角定位 Triangulation

Triangulation解决的是从两张图片中计算对应点的三维坐标的问题。

这个问题的假设是：

• 两张图片对应的相机内参和外参都是已知的
• 相机中对应点关系是已知的并且正确的

这样我们就可以从这两张图片中计算出指向该点方向的两条光线，进而求其交点就得这个点对应的三维坐标

理想情况的三角定位 triangulation in simplified case

我们首先研究简化的情况，这个情况的假设就是两个相机的内参矩阵是 完全一样 的，而且两个相机 在x方向是完全对齐的。

这样我们就可以有如下的模型。

其中b被称为baseline，代表的是两个相机的光心对应的距离。图中分别以$C_lP$和$C_rP$为斜边可见左右两个直角三角形，其直角边可以写出如下的关系式：

$\begin{aligned} \frac{f}{Z_P}&=\frac{u_l}{X_P}&\text{(left image)}\\ \frac{f}{Z_P}&=\frac{-u_r}{b-X_P}&\text{(right image)}\\ \end{aligned}$

左图公式写为：

$X_p=\frac{u_lZ_P}{f}$

带入右图公式：

$\frac{f}{Z_P}=\frac{-u_r}{b-\frac{u_lZ_P}{f}}$

解得：

$Z_P=\frac{bf}{u_l-u_r}$

进一步解得：

$X_P=\frac{bu_l}{u_l-u_r}$

而已知了$Z_P$，$Y_P$也就好解了：

很显然左右两图中看目标点在y轴上的位置应该是一样的，即$v_l=v_r$，这里以$v_l$为准：

$Y_P=\frac{v_lZ_P}{f}$

我们管 $u_l-u_r$ 叫做“disparity”。其最大值就是图像本身的宽度，所以三角测量法存在一个最近距离，比最近距离还近的点无法同时出现在左右两图中所以没法三角测量。而对于无穷远的点，disparity=0。

误差？

这里我们又注意到一个问题，disparity本身是由像素点估算出来的，像素有大小是离散的，势必会带来一些误差。我们用量化的方法研究一下 baseline, disparity, 还有点的距离Z对误差的影响 。

$Z_P$ 关于D是一个非线性函数，我们一般使用函数的一阶泰勒展开来估算函数自变量不确定性带来的因变量的不确定性

$\Delta Z_P = \vert\frac{\mathrm{d}Z_p}{\mathrm{d}D}\vert \Delta D = |-\frac{bf}{D^2}|\Delta D \\$

如果我们将的表达式带入的话，可以得到

$\Delta Z_P = |-\frac{Z_P^2}{bf}|\Delta D \\$

所以我们可以得出一下几个结论：

• 对于同一个3D点，disparity越大，误差越小
• 对于固定位置关系的两张图片，3D点离得越近误差越小

我们同时还可以总结一下baseline对他们的影响：

• b越大，triangulate的误差越小
• b越大，最近可测量距离变大

那么我们就知道怎样来提高双目相机系统的精度了：

• 增大baseline
• 增大焦距
• 尽量测量近距离的点

带误差的三角定位 triangulation in general case

理想情况的三角定位很好，但是在现实生活中，由于生产和装配误差，再好的双目相机，两个相机之间也不可能是完全对齐的，求出来的光线也不一定能精准相交。于是，我们需要在相没对齐光线也不能相交的情况下求解点的坐标：

此时三角定位问题描述为：

• 已知场景中的某个点在左侧和右侧相机照片上的位置分别为$(\bm x,\bm x')$
• 已知相机左右侧相机参数分别为$\bm P,\bm P'$
• 求该点在场景中的位置$X$

由于误差的存在，位置$X$不能精确求解，即$\bm x=\bm P\bm X$和$\bm x'=\bm P'\bm X$不能同时满足。 于是，三角定位问题就成为了一个最优化问题，即：

$\mathop{min}\limits_{\bm X} |\bm x-\bm P\bm X|+|\bm x-\bm P'\bm X|$

回忆一下之前相机参数与坐标系变换里讲的公式：

$z_p \left[ \begin{matrix} u\\v\\1 \end{matrix} \right] =\bm P\cdot\bm X = \left[ \begin{matrix} p_{11}&p_{12}&p_{13}&p_{14}\\p_{21}&p_{22}&p_{23}&p_{24}\\p_{31}&p_{32}&p_{33}&p_{34} \end{matrix} \right] \cdot \left[ \begin{matrix} x_w\\y_w\\z_w\\1 \end{matrix} \right]$

可拆为：

$\left\{ \begin{aligned} u&=\frac{1}{z_p}\bm p_1^T\cdot\bm X\\ v&=\frac{1}{z_p}\bm p_2^T\cdot\bm X\\ 1&=\frac{1}{z_p}\bm p_3^T\cdot\bm X\\ \end{aligned} \right.$

式1,2两边均乘上式3，可得两个约束：

$\left\{ \begin{aligned} \frac{u}{z_p}\bm p_3^T\cdot\bm X&=\frac{1}{z_p}\bm p_1^T\cdot\bm X\\ \frac{v}{z_p}\bm p_3^T\cdot\bm X&=\frac{1}{z_p}\bm p_2^T\cdot\bm X\\ \end{aligned} \right. \Rightarrow \left\{ \begin{aligned} u\bm p_3^T\cdot\bm X&=\bm p_1^T\cdot\bm X\\ v\bm p_3^T\cdot\bm X&=\bm p_2^T\cdot\bm X\\ \end{aligned} \right.$

将约束写成线性方程形式：

$\left\{ \begin{aligned} \bm p_1^T\cdot\bm X&-u\bm p_3^T\cdot\bm X&=0\\ \bm p_2^T\cdot\bm X&-v\bm p_3^T\cdot\bm X&=0\\ \end{aligned} \right.$

进而可以写成矩阵形式：

$\left[ \begin{matrix} \bm p_1^T-u\bm p_3^T\\ \bm p_2^T-v\bm p_3^T\\ \end{matrix} \right] \cdot \bm X = \left[ \begin{matrix} 0\\0 \end{matrix} \right]$

和相机参数与坐标系变换里讲的DLT很像了。 但不一样的是，这次是$\bm P$已知求解$\bm X$，所以：

• 不会有更多的$\bm X$，而是会有更多的$\bm P$组成方程
• $\bm X$有三个值要解，一个相机两个方程是无穷多解，两个相机四个方程就成了超定方程了

于是就解此超定方程：

$\left[ \begin{matrix} \bm p_1^T-u\bm p_3^T\\ \bm p_2^T-v\bm p_3^T\\ \bm p_1'^T-u'\bm p_3'^T\\ \bm p_2'^T-v'\bm p_3'^T\\ \end{matrix} \right] \cdot \bm X = \left[ \begin{matrix} 0\\0\\0\\0 \end{matrix} \right]$

应用SVD求解即可。

对极几何（Epipolar Geometry）

刚才说的是知道了内参外参和点的对应关系之后，怎样进行三角定位。那往回推一步，怎样找点之间的对应关系呢？

如果直接在两张图里面找对应提取关键点进行匹配，只有在左右两边都找到的关键点才有可能匹配成功。如果对任意一点进行匹配的话，马上就会变成一个对左右两张图片所有像素的穷举搜索，复杂度为$O(hwh'w')$，其中$h,w$和$h',w'$分别为左右两图片的长款。

在已知相对位姿的双目相机中，能不能简化一些？ 能！现在有了相对的位姿约束，我们可以将这 个二维的搜索问题，降低到一维。

如图所示，两个相机之间的相对位姿，为我们提供了一个很好的先验知识： 已知相机位姿的情况下，从左边相机中发出的指向某个特征点的射线在右边相机中的投影可以计算出来，所以直接在这条投影线上搜索特征点即可。

• 这个约束叫做对极约束（epipolar constraint）
• $c_l，c_r，p$ 组成的平面成为极平面 (epipolar plane)
• 极平面与两个图像的交线成为极线 (epipolar line)
• 所有的极线的交点我们成为极点（epipole），也是baseline和像平面的交点

通过这个方法，我们就可以将二维搜索问题降低到一维。

对极约束

具体来说，以左侧相机坐标系为世界坐标系，右侧相机的外参为$[\bm R|\bm t]$，设待定位的点在世界坐标系下的坐标为$\bm X=[X,Y,Z]^T$、其在左右相机坐标系下z轴的坐标分别为$z_1$和$z_2$，于是根据相机参数与坐标系变换中的建模可以知道该点在左右相机所拍摄图像上的位置：

$\begin{aligned} \bm x&=\bm K_1\bm X\\ \bm x'&=\bm K_2(\bm R\bm X+\bm t)\\ \end{aligned}$

去掉相机内参，还原出对应点在左右两侧相机坐标系下的坐标$\bm x_1,\bm x_2$：

$\begin{aligned} {\bm{x}_1} = {\bm{K}^{ - 1}_1}{\bm x}&=\bm X\\ {\bm{x}_2} = {\bm{K}^{ - 1}_2}{\bm x'}&=\bm R\bm X+\bm t\\ \end{aligned}$

那么右侧相机的$\bm x_2$就可从左侧相机的$\bm x_1$经过变换得到：

${\bm{x}_2} = \bm{R} {\bm{x}_1} + \bm{t}$

根据相机参数与坐标系变换中的建模，这里的$\bm{R} {\bm{x}_1}$也可以看作是在右侧相机眼中的向量$\bm{x}_1$（图中向量$\overrightarrow{OX}$），而$\bm t$就是右侧相机相对于左侧相机的位置向量$\overrightarrow{O'O}$，所以${\bm{x}_2} = \bm{R} {\bm{x}_1} + \bm{t}$也就可以理解为上图的向量求和。

那么很显然，$\bm t$、$\bm R\bm x_1$、$\bm x_2$三向量共面。 接下来回忆两个关于向量的知识：

1. 向量叉积$\bm a\times\bm b$是求向量$\bm a,\bm b$所成平面的法向量
2. 相互垂直的向量点积为0

于是，对于三个共面向量，其任意两个向量的叉积与第三个向量的点积为0。这里就取$\bm t$和$\bm R\bm x_1$叉积与$\bm x_2$的点积：

$(\bm t\times\bm R\bm x_1)\cdot\bm x_2=0$

根据$\bm a\cdot\bm b=\bm a^T\bm b=\bm b^T\bm a$写成矩阵形式，再将叉乘等价于用反对称矩阵来点乘，从而把括号里面的$\bm x_1$拿出来：

$\bm x_2^T(\bm t_\times\bm R)\bm x_1=0\qquad \bm t_\times= \left[ \begin{matrix} 0&-t_3&t_2\\t_3&0&-t_1\\-t_2&t_1&0 \end{matrix} \right]$

此即对极约束方程。或重新带入$\bm x,\bm x'$：

$\bm x'^T(\bm K_2^{-1})^T(\bm t_\times\bm R)\bm K_1^{-1}\bm x=0$

即是对极约束方程的另一种形式。

本质矩阵 essential matrix

矩阵论中存在一种Longuet-Higgins方程：

$\bm x'^T\bm E\bm x=0$

其中的$\bm E$称为“本质矩阵”（Essential Matrix）。在上述对极约束方程中，本质矩阵为$\bm E=\bm t_\times\bm R$或$\bm E=(\bm K^{-1}_2)^T(\bm t_\times\bm R)\bm K^{-1}_1$。其性质可以表示为：

$\left\{ \begin{aligned} \bm x^T\bm l&=0\\ \bm l&=\bm E^T\bm x' \end{aligned} \right. \qquad \left\{ \begin{aligned} \bm x'^T\bm l'&=0\\ \bm l'&=\bm E\bm x \end{aligned} \right.$

进一步解析可见对极约束方程的物理含义，以$\bm E=\bm t\times\bm R$为例，此时的：$\bm x,\bm x'$是目标点在相机坐标系下的坐标：

$\begin{aligned} {\bm{x}_1} = {\bm{K}^{ - 1}_1}{\bm x}&=\bm X\\ {\bm{x}_2} = {\bm{K}^{ - 1}_2}{\bm x'}&=\bm R\bm X+\bm t\\ \end{aligned}$

其关系如图所示：

简言之，此处先回忆本科学的解析几何知识：“空间中的点$\bm x=[x,y,z]^T$位于平面$\bm l=[a,b,c]^T$上”等价于$ax+by+cz=0$，即$\bm x^T\bm l=0$。所以在像平面不确定的情况下，$\bm l=[a,b,c]^T$实际上描述了极平面位置，而显然像平面方程为“$z=\text{常数}$”的形式，其与极平面相交即为极线$ax+by=\text{常数}$。

所以，有了$\bm l=[a,b,c]^T$，对任意给定的像平面都能求出要找的极线。

基础矩阵 fundamental matrix

虽然$\bm E=\bm t_\times\bm R$和$\bm E=(\bm K^{-1}_2)^T(\bm t_\times\bm R)\bm K^{-1}_1$都是本质矩阵，但$\bm E=(\bm K^{-1}_2)^T(\bm t_\times\bm R)\bm K^{-1}_1$通常叫做基础矩阵(fundamental matrix)，记为$\bm F$。

求基础矩阵：八点算法 8-point algorithm

求解基础矩阵$\bm F=(\bm K^{-1}_2)^T(\bm t_\times\bm R)\bm K^{-1}_1$即是在求左右两相机的相对位姿和各自的内参。

写下基础矩阵的Longuet-Higgins方程：

$\bm x'^T\bm F\bm x=0$

共9个变量要解：

$[x',y',z'] \left[ \begin{matrix} f_1&f_2&f_3\\f_4&f_5&f_6\\f_7&f_8&f_9 \end{matrix} \right] \left[ \begin{matrix} x\\y\\z \end{matrix} \right] =0$

展开：

$x'xf_1+x'yf_2+x'zf_3+y'xf_4+y'yf_5+y'zf_6+z'xf_7+z'yf_8+z'zf_9=0$

每一对点就有一个方程，所以共需要9对点吗？并不，基础矩阵内部存在内在联系，实际上只需要8对点。

矩阵自由度分析：

• 本质矩阵$\bm E=\bm t_\times\bm R$
  • 3x3矩阵，9个变量，自由度9？
  • 6个独立参数，自由度6？（$\bm t$有3个独立参数、$\bm R$是描述三维旋转的矩阵，有3个自由度）
  • 尺度等价，自由度-1，自由度5？
  • 自由度5！
• 基础矩阵$\bm F=(\bm K^{-1}_2)^T(\bm t_\times\bm R)\bm K^{-1}_1$
  • 3x3矩阵，9个变量，自由度9？
  • 13个独立参数，自由度9？（自由度5的本质矩阵、$\bm K_1,\bm K_2$各有4个独立参数）
  • 尺度等价，自由度-1，自由度8？
  • 自由度8！

于是8对点解基础矩阵：

$\left[ \begin{matrix} x_1'x_1&x_1'y_1&x_1'z_1&y_1'x_1&y_1'y_1&y_1'z_1&z_1'x_1&z_1'y_1&z_1'z_1\\ x_2'x_2&x_2'y_2&x_2'z_2&y_2'x_2&y_2'y_2&y_2'z_2&z_2'x_2&z_2'y_2&z_2'z_2\\ x_3'x_3&x_3'y_3&x_3'z_3&y_3'x_3&y_3'y_3&y_3'z_3&z_3'x_3&z_3'y_3&z_3'z_3\\ x_4'x_4&x_4'y_4&x_4'z_4&y_4'x_4&y_4'y_4&y_4'z_4&z_4'x_4&z_4'y_4&z_4'z_4\\ x_5'x_5&x_5'y_5&x_5'z_5&y_5'x_5&y_5'y_5&y_5'z_5&z_5'x_5&z_5'y_5&z_5'z_5\\ x_6'x_6&x_6'y_6&x_6'z_6&y_6'x_6&y_6'y_6&y_6'z_6&z_6'x_6&z_6'y_6&z_6'z_6\\ x_7'x_7&x_7'y_7&x_7'z_7&y_7'x_7&y_7'y_7&y_7'z_7&z_7'x_7&z_7'y_7&z_7'z_7\\ x_8'x_8&x_8'y_8&x_8'z_8&y_8'x_8&y_8'y_8&y_8'z_8&z_8'x_8&z_8'y_8&z_8'z_8\\ \end{matrix} \right] \left[ \begin{matrix} f_1\\f_2\\f_3\\f_4\\f_5\\f_6\\f_7\\f_8\\f_9 \end{matrix} \right] =0$

这八个方程构成了一个线性方程组。它的系数矩阵$\bm A$由特征点位置构成，大小为 8 × 9，解之即得基础矩阵$\bm F$。由于测量误差的存在，还是应用SVD求解。根据SVD求最优解的方法，直接取$\bm A^T\bm A$的最小特征值对应的特征向量即得$\bm F$。

还没结束：$\bm F$的秩

但是，如果看了下面一节就知道，对于左图上任意一点$\bm x$，在右图上对应的极线$\bm l'=\bm F\bm x$都经过同一点，所以$\bm F$秩为2（为什么？）。然而系数矩阵是有误差的，求出来的$\bm F$不一定秩为2。因此，对于求出的$\bm F$还需要进一步操作让它秩为2，即求一个和$\bm F$最接近的$\hat{\bm F}$，使$\hat{\bm F}$秩为2。

这个操作同样也可以用SVD实现，还记得介绍SVD时讲的它和PCA的关系么？$\bm\Sigma$矩阵中所描述的值实际上相当于成分占比，其对应的特征矩阵就是成分。所以，用SVD实现令$\hat{\bm F}$秩为2且和$\bm F$接近的方法就是删除$\bm F$中的最小一个成分：

1. 先对$\bm F$进行SVD，$\bm F=\bm U\bm\Sigma\bm V^T$
2. 将$\bm\Sigma$中最后一个值置0，得$\hat{\bm\Sigma}$
3. $\hat{\bm F}=\bm U\hat{\bm\Sigma}\bm V^T$

这样得到的$\hat{\bm F}$就和$\bm F$很接近，且秩为2。

还没结束：Normalization

通常，求SVD时要先把坐标缩放到$[-1,1]$，但是为什么要Normalization？原因是为了稳定性：

若设缩放坐标用的变换是$\bm T$，变换后坐标$\hat{\bm x}=\bm T\bm x$和$\hat{\bm x}'=\bm T'\bm x'$，那么求出来的$\hat{\bm F}$就是：

$\begin{aligned} \hat{\bm x}'^T\hat{\bm F}\hat{\bm x}&=0\\ (\bm x'^T\bm T'^T)\hat{\bm F}(\bm T\bm x)&=0\\ \bm x'^T(\bm T'^T\hat{\bm F}\bm T)\bm x&=0\\ \end{aligned}$

所以真正的$\bm F=\bm T^T\hat{\bm F}\bm T$。

还没结束：已知基础矩阵和相机内参求本质矩阵和相机外参

求本质矩阵简单：$\bm E=\bm K_2^T\bm F\bm K_1$

求外参就得分解本质矩阵：$\bm E=\bm t_\times\bm R$

由于$\bm R$

实例：已知基础矩阵和左图特征点位置求右图特征点搜索空间

就是求右图极线

实例：已知基础矩阵求极点位置

设左相机的极点坐标为$e$，而极点物理含义是左右相机中心连线$\overrightarrow{OO'}$与相机成像平面的交点，其显然在极线上，且其相当于右侧相机（即坐标原点）在左侧成像平面上的位置：

首先求右侧相机在左侧相机坐标系下的位置，用$\bm R$和$\bm t$列个方程，$\bm x_{o'}$表示右侧相机在左侧坐标系下的坐标，以$\bm x_{o'}'$表示右侧相机在右侧坐标系下的坐标（即$[0,0,0]^T$），则可以列如下方程：

$\bm x_{o'}'=[0,0,0]^T=\bm R\bm x_{o'}+\bm t$

为了产生$\bm F$先两边叉乘$\bm t$：

$0=\bm t\times\bm R\bm x_{o'}$

于是得到一个关于$\bm x_{o'}$的方程：

$\bm E\bm x_{o'}=0$

而$\bm e,\bm e'$是在成像平面上的点，所以还要加个内参$\bm e=\bm K_1\bm x_{o'}$和$\bm e'=\bm K_2\bm x_{o'}$于是可以写出方程并硬凑出来一个$\bm F$，且这里$e'$依然是原点，不管怎么变都是0：

$\begin{aligned} \bm K_2^{-1}\bm e'&=\bm R\bm K_1^{-1}\bm e+\bm t\\ \bm t\times\bm K_2^{-1}\bm e'&=\bm t\times\bm R\bm K_1^{-1}\bm e\\ (K_2^{-1})^T\bm t\times\bm K_2^{-1}\bm e'&=(K_2^{-1})^T\bm t\times\bm R\bm K_1^{-1}\bm e\\ 0=\bm F\bm e \end{aligned}$

即得方程$\bm F\bm e=0$，SVD解之即得$\bm e$。

单应矩阵 Homography

除了基本矩阵和本质矩阵，我们还有一种称为单应矩阵$\bm H$的东西，它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性来进行运动估计。这种情况在无人机携带的俯视相机，或扫地机携带的顶视相机中比较常见。

单应矩阵通常描述处于共同平面上的一些点，在两张图像之间的变换关系。 设左右相机成像平面的特征点对$\bm x_1,\bm x_2$所对应的点在世界坐标系下的坐标为$\bm X=[X,Y,Z]$，其在某平面上，该平面方程为：

$\bm n^T\bm X+d=0$

整理一下：

$\frac{\bm{n}^T \bm{X} }{d} = 1$

$\begin{aligned} \bm{x}_2 &= \bm{K}_2 ( \bm{R} \bm{X} + \bm{t} ) \\ &= \bm{K}_2 \left( \bm{R} \bm{X} + \bm{t} \cdot (- \frac{\bm{n}^T \bm{X} }{d}) \right) \\ &= \bm{K}_2 \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{X} \\ &= \underbrace{\bm{K}_2 \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{K}_1^{-1}}_{\bm{H}} \bm{x}_1. \end{aligned}$

单应矩阵即$\bm H=\bm{K}_2 \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{K}_1^{-1}$。 这个矩阵可以将平面$\bm n^T\bm X+d=0$上的任一点$\bm X$在左侧相机成像平面上的坐标$\bm x_1$转化为右侧相机成像平面上的坐标$\bm x_2$：

$\bm{x}_2 = \bm{H} \bm{x}_1$

不同于本质矩阵和基础矩阵将点转换为线，单应矩阵将点转化为点，因此得名。

矩阵自由度分析：

• 单应矩阵$\bm H=\bm{K}_2 \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{K}_1^{-1}$
  • 3x3矩阵，9个变量，自由度9？
  • 14个独立参数，自由度9？（$\bm t$有3个独立参数、$\bm R$是描述三维旋转的矩阵，有3个自由度、$\bm K_1,\bm K_2$各有4个独立参数）
  • 尺度等价，自由度-1，自由度8？
  • 自由度8！

双目矫正 stereo rectification

根据上面三角测量和对极几何的知识，我们可以看到，完全平行的双目相机计算是很有优势的。 其极线全都是相同位置的水平直线，在三角测量的时候也不再需要求解SVD之类的麻烦东西了。 所以我们就希望想方法，将拍到的图像投影到一个这样理想的双目相机模型中，这个过程就叫做双目矫正。

双目矫正之后极线全都是相同位置的水平直线，不用每次都解SVD找极线：

问：从其他视角矫正来的图和在矫正后的视角拍摄的图是等价的吗？

• 如果的把相机转到矫正后的视角，拍摄出来的图和从其他视角矫正来的图一样吗？
• 视角矫正的相机位姿变化+场景中物体的遮挡$\Rightarrow$矫正来的图$\not=$从矫正后的视角出发拍摄的图？

答：等价。

• 以小孔成像作为相机模型，这里的矫正实际上是在矫正小孔后面的成像平面的位姿。
• 在已知小孔后成像平面位姿的情况下，由图片可以推知打到小孔的每一束光线的方向，进而可以推出任意位姿成像平面上的图片。
• 只要这个小孔位置不变，其成像中的物体的遮挡关系就不会改变，拍出来的图片也就可以相互转化。
• 两点成一条直线，所以双目相机总能找到一个和两个相机小孔连线平行的成像平面。

双目矫正公式推导

双目矫正的所谓“将拍到的图像投影到理想双目相机模型”的过程从根本上讲就是找一个从拍摄的图像坐标$[u,v,w]^T$到对应的理想双目相机模型图像坐标$[\hat u,\hat v,\hat w]^T$的变换。设场景中的点在世界坐标系下坐标表示为$\bm X=[X,Y,Z]^T$，其在左侧和右侧相机成像平面上的坐标分别表示为$\bm x_l=[u_l,v_l,w_l]^T$和$\bm x_r=[u_r,v_r,w_r]^T$，而左右侧相机内参分别为$\bm K_l$和$\bm K_r$，外参分别为$[\bm R_l|\bm t_l]$和$[\bm R_r|\bm t_r]$，于是先来一手相机参数与坐标系变换：

$\begin{aligned} \bm x_l&=\bm K_l(\bm R_l\bm X+\bm t_l)\\ \bm x_r&=\bm K_r(\bm R_r\bm X+\bm t_r)\\ \end{aligned}$

要求将左侧和右侧相机照出来的点映射到共面的成像平面上，即映射后的相机具有相同的旋转矩阵$\hat{\bm R}$（方向相同）且具有相同的内参$\hat{\bm K}$。 再回忆相机参数与坐标系变换中所介绍的c2w变换矩阵$[\bm R|\bm t]$中平移向量$\bm t$的含义：世界坐标系原点在相机坐标系下的位置。所以当旋转矩阵发生变化时，平移向量的值也会发生变化，设为$\hat{\bm t}_l,\hat{\bm t}_r$：

$\begin{aligned} \hat{\bm x_l}&=\hat{\bm K}(\hat{\bm R}\bm X+\hat{\bm t}_l)\\ \hat{\bm x_r}&=\hat{\bm K}(\hat{\bm R}\bm X+\hat{\bm t}_r)\\ \end{aligned}$

为了求$\hat{\bm x_l},\hat{\bm x_r}$和$\bm x_l,\bm x_r$间的关系，得先把$\bm X$消掉，所以先这样：

$\begin{aligned} \bm X&=\bm R_l^{-1}\bm K_l^{-1}\bm x_l-\bm R_l^{-1}\bm t_l\\ \bm X&=\bm R_r^{-1}\bm K_r^{-1}\bm x_r-\bm R_r^{-1}\bm t_r\\ \end{aligned}$

$\begin{aligned} \bm X&=\hat{\bm R}^{-1}\hat{\bm K}^{-1}\hat{\bm x}_l-\hat{\bm R}^{-1}\hat{\bm t}_l\\ \bm X&=\hat{\bm R}^{-1}\hat{\bm K}^{-1}\hat{\bm x}_r-\hat{\bm R}^{-1}\hat{\bm t}_r\\ \end{aligned}$

换一个角度理解，上面4个式子其实是在利用c2w矩阵将相机坐标系下的坐标向世界坐标系转化，其中的$\bm K_l^{-1}\bm x_l,\bm K_r^{-1}\bm x_r$分别表示点$\bm X$在左右相机坐标系下的坐标，同理，$\hat{\bm K}^{-1}\hat{\bm x}_l,\hat{\bm K}^{-1}\hat{\bm x}_r$分别表示点$\bm X$在矫正后的左右相机坐标系下的坐标。

此外，由于$\bm t_l$表示“世界坐标系原点相在相机坐标系下的位置”，而$\bm R_l^{-1}$是相机坐标系到世界坐标系的旋转，所以$-\bm R_l^{-1}\bm t_l$正是“相机坐标系原点在世界坐标系下的位置”，$-\bm R_r^{-1}\bm t_r$、$-\hat{\bm R}^{-1}\hat{\bm t}_l$、$-\hat{\bm R}^{-1}\hat{\bm t}_r$均同理。 而由于双目矫正中相机位置不变，于是必有$\bm R_l^{-1}\bm t_l=\hat{\bm R}^{-1}\hat{\bm t}_l$和$\bm R_r^{-1}\bm t_r=\hat{\bm R}^{-1}\hat{\bm t}_r$。

于是将c2w矩阵记为：

$\begin{aligned} [{}_w\bm R_l|\bm c_l]&=[\bm R_l^{-1}|-\bm R_l^{-1}\bm t_l]\\ [{}_w\bm R_r|\bm c_r]&=[\bm R_r^{-1}|-\bm R_r^{-1}\bm t_r]\\ [{}_w\hat{\bm R}|\bm c_l]&=[\hat{\bm R}^{-1}|-\hat{\bm R}^{-1}\hat{\bm t}_l]\\ [{}_w\hat{\bm R}|\bm c_r]&=[\hat{\bm R}^{-1}|-\hat{\bm R}^{-1}\hat{\bm t}_r]\\ \end{aligned}$

再设点$\bm X$在矫正后的左右相机坐标系下的坐标：

$\begin{aligned} {}_c\bm x_l&=\bm K_l^{-1}\bm x_l\\ {}_c\bm x_r&=\bm K_r^{-1}\bm x_r\\ {}_c\hat{\bm x}_l&=\hat{\bm K}^{-1}\hat{\bm x}_l\\ {}_c\hat{\bm x}_r&=\hat{\bm K}^{-1}\hat{\bm x}_r \end{aligned}$

那么上式可以简写为$\bm X=\bm R\bm x+\bm t$的形式：

$\begin{aligned} \bm X&={}_w\bm R_l\cdot{}_c\bm x_l+\bm c_l\\ \bm X&={}_w\bm R_r\cdot{}_c\bm x_r+\bm c_r\\ \end{aligned}$

$\begin{aligned} \bm X&={}_w\hat{\bm R}\cdot{}_c\hat{\bm x}_l+\bm c_l\\ \bm X&={}_w\hat{\bm R}\cdot{}_c\hat{\bm x}_r+\bm c_r \end{aligned}$

于是：

$\begin{aligned} {}_w\hat{\bm R}\cdot{}_c\hat{\bm x}_l&=\bm X-\bm c_l&={}_w\bm R_l\cdot{}_c\bm x_l\\ {}_w\hat{\bm R}\cdot{}_c\hat{\bm x}_r&=\bm X-\bm c_r&={}_w\bm R_r\cdot{}_c\bm x_r \end{aligned}$

再带入${\bm x_l},{\bm x_r},\hat{\bm x_l},\hat{\bm x_r}$：

$\begin{aligned} \hat{\bm R}^{-1}\hat{\bm K}^{-1}\hat{\bm x}_l&=\bm R_l^{-1}\bm K_l^{-1}\bm x_l\\ \hat{\bm R}^{-1}\hat{\bm K}^{-1}\hat{\bm x}_r&=\bm R_r^{-1}\bm K_r^{-1}\bm x_r \end{aligned}$

即得：

$\begin{aligned} \hat{\bm x}_l&=\hat{\bm K}\hat{\bm R}\bm R_l^{-1}\bm K_l^{-1}\bm x_l\\ \hat{\bm x}_r&=\hat{\bm K}\hat{\bm R}\bm R_r^{-1}\bm K_r^{-1}\bm x_r \end{aligned}$

这样，如果给定两个双目矫正矩阵$\hat{\bm K}\hat{\bm R}\bm R_l^{-1}\bm K_l^{-1}$和$\hat{\bm K}\hat{\bm R}\bm R_r^{-1}\bm K_r^{-1}$，就可将左右相机照片上的点映射到矫正成像平面上，从而实现双目矫正操作。 接下来就是要求解这两个双目矫矩阵。

双目矫正矩阵求解

显然，双目矫正过程中相机位置并不发生改变，而共面不光需要相机方向相同，其方向还必须使得两个成像平面重合。

如图所示，显然，与两相机位置连线平行的平面都可以作为成像平面，所以成像平面有无穷多个。

若设左右侧相机在世界坐标系下的坐标分别为$\bm c_l$和$\bm c_r$，通常直接取下列方程对应的成像平面：

$\begin{aligned} \hat{\bm K}&=\frac{\bm K_l+\bm K_r}{2}\\ {}_w\hat{\bm R}&=[\hat{\bm r}_1,\hat{\bm r}_2,\hat{\bm r}_3]\\ \hat{\bm r}_1&=\frac{\bm c_l-\bm c_r}{\|\bm c_l-\bm c_r\|}\\ \hat{\bm r}_2&=\bm r_3\times\hat{\bm r}_1\\ \hat{\bm r}_3&=\hat{\bm r}_1\times\hat{\bm r}_2\\ \end{aligned}$

其中$\bm r_3$是左边相机的w2c旋转矩阵${}_w\bm R_l$中的第三列列向量。

回忆相机参数与坐标系变换中对旋转矩阵本质的介绍，旋转矩阵的列向量为xyz坐标轴单位向量在原坐标系中的向量值，可以理解上面这个成像平面为什么能满足要求：

• $\hat{\bm r}_1=\frac{\bm c_l-\bm c_r}{\|\bm c_l-\bm c_r\|}$表示相机坐标系的x轴的方向为左右两相机位置相减再归一化，即保证矫正后的两相机成像平面与相机位置的连线平行
• $\hat{\bm r}_2=\bm r_3\times\hat{\bm r}_1$让相机坐标系的y轴的方向与左侧相机的z轴（相机朝向）垂直，也可以看成是与其y轴平行
• 因为是旋转矩阵，三个列向量必须正交，所以xy轴确定后z就唯一确定$\hat{\bm r}_3=\hat{\bm r}_1\times\hat{\bm r}_2$

这个过程相当于是将左侧相机中的坐标系沿y轴旋转到x轴与两相机位置的连线平行，如上图所示，很明显满足双目矫正的要求。

有了${}_w\hat{\bm R}$就能知道$\hat{\bm R}={}_w\hat{\bm R}^{-1}$，和$\hat{\bm K}$一起带入双目矫正矩阵$\hat{\bm K}\hat{\bm R}\bm R_l^{-1}\bm K_l^{-1}$和$\hat{\bm K}\hat{\bm R}\bm R_r^{-1}\bm K_r^{-1}$，就可将左右相机照片上的点映射到矫正成像平面上，从而实现双目矫正操作。