三角测量(sfm 双视图重建)

1.极限约束与本征矩阵

设世界坐标系中有一点$p$ ，坐标为 $X$，它在左相机的图像坐标系为$x_1^{\prime }$像素坐标系为$x_1$，在右相机中的图像坐标系为 $x_1^{\prime }$则$x=K{x}^{\prime }$其中$K$为相机的内参矩阵，如图1（图中的$x_1$图像坐标系下的坐标，和公式的不同）设 $p$到两个相机光心垂直距离分别为 $s_1$和$s_2$，且两个相机有相同的内参矩阵$K$ ，世界坐标系到两个相机坐标系的变换关系为 $[R_1,T_1]$和$[R_2,T_2]$ ，根据相机成像原理我们可以得到下面两个等式：
$$s_1{x}_1=K(R_{1}X+T_1)$$$$s_2{x}_2=K(R_{2}X+T_2)$$
由于$K$是可逆的得到$$s_1{x}_1^{\prime }=R_{1}X+T_1$$$$s_2{x}_2^{\prime }=R_{2}X+T_2$$设世界坐标系的原点为相机1的相机坐标系则有$$s_1{x}_1^{\prime }=X$$$$s_2{x}_2^{\prime }=R_{2}X+T_2$$
公式1带入公式2得到：$$s_2{x}_2^{\prime }=s_1{R}_2{x}_1^{\prime }+T_2$$
对上式两边同时叉乘$T_2$ (叉乘方向为右手系垂直方向，大小为向量组成的平行四边形面积,向量叉乘自身为0)得到：
$$s_2\widehat{T_2}{x}_2^{\prime }=s_1\widehat{T_2}{R}_2{x}_1^{\prime }$$
然后上式与$x_2^{\prime }$做内积得到($\widehat{T_2}{x}_2^{\prime }$方向垂直与$x_2^{\prime }$):
$$0=x_2^{\prime }\widehat{T_2}{R}_2{x}_1^{\prime }$$
其中$E=\widehat{T_2}R$ 由上式可以看出空间中的同一点在两个相机的图象坐标系满足的关系，和空间中的坐标点以及点到相机的距离都没有关系。上式也被称为极线约束， $E$被称为本征矩阵。
然后通过5点法或者8点法求解 ，通过SVD求解 $R$,$T$ 。通过SVD求解的$R$ ，$T$ 没有正负性需要正则化，所以尺度无法确定。

2. 三角化

求解到 $R$，$T$ 后，由前面公式得到$s_2{x}_2^{\prime }=R_{2}X+T_2$ ，公式中未知元素为 $x_2$，$x$ 两边对$x_2^{\prime }$ 做叉积消除$s_2$ 得：$$\widehat{x_2^{\prime }}{R}_{2}X=-\widehat{x_2^{\prime }}{T}_2$$化为其次方程得：$$\widehat{x_2}[R_2,T_2][X,1{]}^T=0$$然后用SVD分解法求解 $X$左侧0空间再把最后一个元素归一化到1求得$x$ 然后把$R,T$开始的两个公式求得真实世界$X$。
这里实现了一个python版本的demo：sfm_python