1. 共线原理
-
双相机立体重建时, 利用物体点-光心-像点三者共线的原理, 在找到左右匹配的像点并且完成去畸变之后, 就能够在三维空间中形成两条直线, 物体点就是这两个直线的交点.
-
交点指最小二乘交点, 因为这两个直线由于偏差通常不会交于一点. 共线方程描述了物体点-光心-像点三者共线的含义, 也相当于三维点到二维点的投影过程, 该投影过程可表达为
$$ \left[\begin{array}{l}\mathrm{x}_{\mathrm{c}} \\\mathrm{y}_{\mathrm{c}} \\\mathrm{z}_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{llll}\mathrm{R}_{11} & \mathrm{R}_{12} & \mathrm{R}_{13} & \mathrm{~T}_{\mathrm{x}} \\\mathrm{R}_{21} & \mathrm{R}_{22} & \mathrm{R}_{23} & \mathrm{~T}_{\mathrm{y}} \\\mathrm{R}_{31} & \mathrm{R}_{32} & \mathrm{R}_{33} & \mathrm{~T}_{\mathrm{z}}\end{array}\right]\left[\begin{array}{c}\mathrm{x}_{\mathrm{w}} \\\mathrm{y}_{\mathrm{w}} \\\mathrm{z}_{\mathrm{w}} \\1\end{array}\right] \quad\quad \text{(1)} $$
2. 解超定线性方程组
-
将公式 $1$ 转换为非齐次方程组
$$ \left\{\begin{array}{l}\mathrm{x}_{\mathrm{c}}=\mathrm{R}_{11} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{12} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{13} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{x}} \\\mathrm{y}_{\mathrm{c}}=\mathrm{R}_{21} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{22} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{23} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{y}} \\\mathrm{z}_{\mathrm{c}}=\mathrm{R}_{31} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{32} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{33} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{z}}\end{array}\right. $$ -
作归一化使 $z_c$ 为1
$$ \left\{\begin{array}{l}\hat{\mathrm{x}_{\mathrm{c}}}=\mathrm{x}_{\mathrm{c}} / \mathrm{z}_{\mathrm{c}}=\frac{\mathrm{R}_{11} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{12} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{13} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{x}}}{\mathrm{R}_{31} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{32} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{33} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{z}}} \\\hat{\mathrm{y}_{\mathrm{c}}}=\mathrm{y}_{\mathrm{c}} / \mathrm{z}_{\mathrm{c}}=\frac{\mathrm{R}_{21} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{22} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{23} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{y}}}{\mathrm{R}_{31} \mathrm{x}_{\mathrm{w}}+\mathrm{R}_{32} \mathrm{y}_{\mathrm{w}}+\mathrm{R}_{33} \mathrm{z}_{\mathrm{w}}+\mathrm{T}_{\mathrm{z}}}\end{array}\right. \quad\quad \text{(2)} $$ -
将公式 $2$ 转换为1组线性方程式
$$ \left\{\begin{array}{l}\left(\hat{\mathrm{x}_{\mathrm{c}}} \mathrm{R}_{31}-\mathrm{R}_{11}\right) \mathrm{x}_{\mathrm{w}}+\left(\hat{\mathrm{x}_{\mathrm{c}}} \mathrm{R}_{32}-\mathrm{R}_{12}\right) \mathrm{y}_{\mathrm{w}}+\left(\hat{\mathrm{x}_{\mathrm{c}}} \mathrm{R}_{33}-\mathrm{R}_{13}\right) \mathrm{z}_{\mathrm{w}}+\left(\hat{\mathrm{x}_{\mathrm{c}}} \mathrm{T}_{\mathrm{z}}-\mathrm{T}_{\mathrm{x}}\right)=0 \\\left(\hat{\mathrm{y}_{\mathrm{c}}} \mathrm{R}_{31}-\mathrm{R}_{21}\right) \mathrm{x}_{\mathrm{w}}+\left(\hat{\mathrm{y}_{\mathrm{c}}} \mathrm{R}_{32}-\mathrm{R}_{22}\right) \mathrm{y}_{\mathrm{w}}+\left(\hat{\mathrm{y}_{\mathrm{c}}} \mathrm{R}_{33}-\mathrm{R}_{23}\right) \mathrm{z}_{\mathrm{w}}+\left(\hat{\mathrm{y}_{\mathrm{c}}} \mathrm{T}_{\mathrm{z}}-\mathrm{T}_{\mathrm{y}}\right)=0\end{array}\right. $$ -
三角测量使用两个二维点 $(\hat{\mathrm{x}_{c1}},\hat{\mathrm{y}_{c1}}),(\hat{\mathrm{x}_{c2}},\hat{\mathrm{y}_{c2}})$ 来确定一个三维点, 所以能够生成两组线性方程式, 转换为矩阵方程
$$ \left[\begin{array}{llll}\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{31_1}-\mathrm{R}_{11_1} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{32_1}-\mathrm{R}_{12_1} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{33_1}-\mathrm{R}_{13_1} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{T}_{\mathrm{z_1}}-\mathrm{T}_{\mathrm{x_1}} \\ \hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{31_1}-\mathrm{R}_{21_1} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{32_1}-\mathrm{R}_{22_1} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{33_1}-\mathrm{R}_{23_1} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{T}_{\mathrm{z_1}}-\mathrm{T}_{\mathrm{y_1}} \\\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{31_2}-\mathrm{R}_{11_2} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{32_2}-\mathrm{R}_{12_2} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{R}_{33_2}-\mathrm{R}_{13_2} &\hat{\mathrm{x}_{\mathrm{c1}}} \mathrm{T}_{\mathrm{z_2}}-\mathrm{T}_{\mathrm{x_2}} \\ \hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{31_2}-\mathrm{R}_{21_2} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{32_2}-\mathrm{R}_{22_2} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{R}_{33_2}-\mathrm{R}_{23_2} &\hat{\mathrm{y}_{\mathrm{c1}}} \mathrm{T}_{\mathrm{z_2}}-\mathrm{T}_{\mathrm{y_2}} \end{array}\right]\left[\begin{array}{l}\mathrm{x}_\mathrm{w} \\\mathrm{y}_\mathrm{w} \\\mathrm{z}_\mathrm{w} \\1 \end{array}\right]=\left[\begin{array}{l}0 \\0 \\0 \\0 \end{array}\right] $$ -
需要求得系数矩阵就是左边的矩阵 $A$. 对该矩阵进行奇异值分解 $A=U D V^T$. $V$ 矩阵的最后一列就是方程解为 $[X, Y, Z, W]$, 归一化之后就是三维坐标系下的点坐标.