1 前言
在使用单目相机的无人驾驶中，有时我们需要构建位姿与现实中某一个平面的关系，以便为后续其它操作提供基础！
2 原理
2.1 位姿与平面间的耦合
假设有相机1（\(s_1\)、\(R_1\)、\(t_1\)）与相机2（\(s_2\)、\(R_2\)、\(t_2\)），一个平面在相机1坐标系下的方程为
(1)
$$
n^Tp+d=0
$$
上面公式中\(n\)是相机平面法向量，\(p\)是一个在相机1坐标系下路平面上的点，\(d\)实际就是相机到平面的距离。对公式(1)进行变形有
(2)
$$
1=-\frac{n^Tp}{d}
$$
根据SLAM之位姿到位姿的相对测量中的公式(5)可以得到相机2与相机1的相对位姿关系
(3)
$$
\begin{aligned}
R &= R_2R_1^Ts_2/s_1
\\
t &= t_2 – Rt_1
\\
s_2R_2 &= s_1RR_1
\\
t_2 &= t + Rt_1
\end{aligned}
$$
现在(1)的平面上，有一个点被相机1与相机2看到，其在世界坐标系下的位置为\(p_w\)，其在相机1坐标系下的坐标为\(p_c\)，其在相机1平面上看到的坐标为\(c_1(u_1,v_1,1)\)，在相机2平面上看到点坐标为\(c_2(u_2,v_2,1)\)，假设相机1与相机2的内参分别为\(K_1\)、\(K_2\)，现在令
(4)
$$
p_1=K_1^{-1}c_1\\
p_2=K_2^{-1}c_2
$$
根据针孔相机模型可以得到
(5)
$$
z_1c_1 = K_1(s_1R_1p_w+t_1)=K_1p_c\\
\Downarrow \\
p_c = z_1K_1^{-1}c_1 = z_1p_1
$$
(6)
$$
\begin{aligned}
z_2c_2&=K_2(s_2R_2p_w+t_2)
\\
&=K_2(s_1RR_1p_w+t+Rt_1)
\\
&=K_2(R(s_1R_1p_w+t1)+t)
\\
&=K_2(Rp_c+t)
\\
&=K_2(Rp_c+t(-\frac{n^Tp_c}{d}))
\\
&=K_2(R-\frac{tn^T}{d})p_c
\\
&=K_2(R-\frac{tn^T}{d})z_1p_1
\end{aligned}
\\
\Downarrow
\\
z_2p_2=z_2K_2^{-1}c_2=(R-\frac{tn^T}{d})z_1p_1
$$
公式(6)建立了平面与位姿的关系。这里需要理解：
1)公式当中是2个位姿共同耦合1个平面。
2)耦合的方程中还包含两个相机共同观测到平面上的点。
3)公式当中的平面是相机1坐标系下的平面，如果要建立世界坐标系下平面与位姿的关系，则将平面方程进行坐标系转换一下即可（相机1坐标系转到世界坐标系）
2.2 根据观测求取相对测量与平面
公式(5)、(6)当中的\(z_1\)、\(z_2\)是使得相机归一化后的最后一维的值，这里令\(s = z_2/z_1\)则公式(6)最终可表示为
(7)
$$
sp_2=(R-\frac{tn^T}{d})p_1
$$
且复原到\(c_1\)、\(c_2\)有
(8)
$$
sc_2=K_2(R-\frac{tn^T}{d})K_1^{-1}c_1=Hc_1
$$
SLAM当中将\(H\)叫做单应矩阵，一般会使用这个矩阵来恢复出相对测量\(R\)、\(t\)，下面将公式(8)展开有
(9)
$$
s\begin{bmatrix}
u_2\\
v_2\\
1
\end{bmatrix}
=
\begin{bmatrix}
h_1 & h_2 & h_3\\
h_4 & h_5 & h_6\\
h_7 & h_8 & h_9
\end{bmatrix}
\begin{bmatrix}
u_1\\
v_1\\
1
\end{bmatrix}
\\
\Downarrow
\\
\begin{aligned}
su_2 &= h_1u_1+h_2v_1+h_3\\
sv_2 &= h_4u_1+h_5v_1+h_6\\
s&=h_7u_1+h_8v_1+h_9
\end{aligned}
$$
在公式(9)当中消去\(s\)有
(10)
$$
\begin{aligned}
u_2 &= \frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9}\\
v_2 &= \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}
\end{aligned}
$$
仔细观察公式(10)可以发现，对于\(H\)中的元素同时乘以一个非0的常数，并不改变公式(10)的结果，且这样操作后等价于\(H\)的特征值同时乘以这个非0常数。因此这里不妨假设\(h_9=1\)，当然也可以假设\(H\)的其它元素值为1(这里需要注意，假设的元素的值本身可能为0，所以保险做法是取几个元素置为1分别尝试计算\(R\)、\(t\))，因此最终方程(10)可变为
(11)
$$
\begin{bmatrix}
u_1 & v_1 & 1 & 0 & 0 & 0 & -u_1u_2 & -v_1u_2
\\
0 & 0 & 0 & u_1 & v_1 & 1 & -u_1v_2 & -v_1v_2
\end{bmatrix}
\begin{bmatrix}
h_1\\
h_2\\
h_3\\
h_4\\
h_5\\
h_6\\
h_7\\
h_8
\end{bmatrix}
=
\begin{bmatrix}
u_2\\
v_2
\end{bmatrix}
$$
当有多组观测点都落在这个平面时，可以通过组装的线性方程组求解出一个最佳的\(H\)，由于在求解过程中假设\(h_9=1\)，因此真实的\(H\)可表示为\(\alpha H\)
因此根据公式(8)最终可以得到
(12)
$$
(R-\frac{tn^T}{d}) = \alpha K_2^{-1}HK_1 = \alpha G
$$
对于公式(1)的平面方程来说，两边同时乘以一个非0系数，不改变平面性质，因此对于公式(12)当中的\(d\)，不妨令其等于1(实际当中一般需要建立相机与平面的关系时，这个相机一般距离平面有一定距离，因此参数\(d\)为0的可能性很小)，则公式(12)则变成
(13)
$$
(R-tn^T) = \alpha G
$$
在上面的公式中\(G\)已经得到，对于公式(13)的形式，根据Ezio Malis[1]的结果，最终可以求解出\(R\)、\(t\)、\(n\)、\(\alpha \)
2.3 已知相对测量求平面方程
和上面类似，仔细观察公式(7)可知，同时缩放\(n\)、\(d\)一个非0系数，它们的平面性质不会改变(公式(1)当中两边同时乘以一个非0常数，表示的平面不变)，这时不妨设$d=1$，则公式(7)最终变为
(14)
$$
sp_2=Rp_1-tn^Tp_1=Rp_1-tp_1^Tn
$$
将公式(14)中的未知数都移动到一边有
(15)
$$
tp_1^Tn+sp_2=Rp_1
$$
从公式(15)可以看出，这个里面有3个线性方程4个未知数(\(n\)为3个未知数，\(s\)为1个未知数)。
对于一个平面上的多个匹配点对，由于每个(匹配对)点对应的\(s\)不一样，因此如果平面上同时有\(m\)个点被两个相机同时看见，这时根据(15)可以构建一个$3m \times (3 + m)$的线性矩阵来求解出平面方程系数\(n\)。
如果两个相机的连线与所在平面平行，现在设两个相机连线在世界坐标系下的方向向量为\(v\)，则它们与公式(1)当中的\(n\)满足如下关系
(16)
$$
n^TR_1v=0
$$
上面公式(16)是一个3个未知数(\(n\))的线性方程，因此如果将公式(16)带入公式(15)，\(n\)的求解维度可以进一步降低到2维。进而可以进行求解。
参考
[1] Ezio Malis, Manuel Vargas. Deeper understanding of the homography decomposition for vision-based control. 2007