单应矩阵的两种形式（两帧运动模型与棋盘格模型）

单应矩阵的两种形式（两帧运动模型与棋盘格模型）

一、引言

最近做视觉的充电桩自动对接项目，先搭建基于棋盘格的demo出来。由于棋盘格的角点都是平面的，理所当然的应当用单应矩阵去计算位姿。由于单应矩阵这块不是很熟，所以拿出了高翔的视觉十四讲准备对着这个编写。高翔大佬的书上对这块介绍不多，计算完单应矩阵后就没有详细讲如何分解并选择为R和t，所以打算上网搜搜（偷懒.jpg）。百度相关的介绍不多，并且都是与分解R和t不相关的。只好去谷歌搜一下。

但是，在opencv的例程中，我看到了一些不一样的解释和方法，这种方法针对棋盘格标定，泛用性不是那么强，但是很有意思，在关于棋盘格定位的特定任务下简单快捷，因此想和大家分享一下。

二、两帧运动模型的单应矩阵

这个模型推导详见高翔的书上，这里就不在献丑了。从原理上来说，这种方法和求解基础矩阵毫无二致，这里基础矩阵被定义为：
$$\begin{gathered} p_{t1}=H_{t1}^{t2}{p}_{t2} \\ where:H_{t1}^{t2}=K{(}^c{R}_{t1}^{t2}-\frac{t{n}^T}{d})K^{-1} \end{gathered}$$
这里的$p_t$表示t时刻目标角点在像素平面的投影；${}^c{R}_1^2$表示相机坐标$c$下从$t1$时刻到$t2$时刻的旋转矩阵；$n^T$表示平面法向量，理论上相机与平面平行时$n^T=1$；$d$为平面截距。分解方法可以参考ORB程序（虽然很长），也可以参考十四讲中提到的引用文献。

三、棋盘格模型

这个模型的主要想法是把单应矩阵作为一种仿射变换，直接从像素平面变换到世界坐标系下。假设世界坐标系为棋盘格坐标系，其Z全部为零，因此$P=\left[\begin{array}{c}X\\ Y\\ Z=0\\ 1\end{array}\right]$退化为$\tilde{P}=\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$，并且在矩阵乘法中将R的第三列消去。具体表达如下：
$$\begin{gathered} p_t={}_w^c{H}_t\tilde{P} \\ where:{}_w^c{H}_t=K\left[\begin{array}{ccc}{r}_{11}& r_{12}& t_x\\ r_{12}& r_{22}& t_y\\ r_{31}& r_{32}& t_z\end{array}\right] \end{gathered}$$
之后，分解H矩阵，求解到R和t：
$$\begin{gathered} H=K\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right] \\ R=[r_1{r}_2{r}_1\times r_2] \end{gathered}$$
值得一提的是，opencv的H矩阵也是类似方法计算的。这样做的好处是能够仅利用单帧信息求解R和t。然而，这样得到的R矩阵不满足旋转矩阵必须是单位正交阵的要求，因此需要对其进行修正。有三种思路：

• 建立带有惩罚项的优化方程，在优化环节处理
• 施密特正交化
• 先进行归一化，再进行SVD分解正交处理

个人理解2、3应该是差不多的，但是本人数学菜逼，不确定对不对。opencv例程给的是SVD分解方法。代码如下：https://docs.opencv.org/3.4/d9/dab/tutorial_homography.html

注意这里例程中使用了undistortPoints 这个函数，并且没有添加参数P，因此效果是将$p\to K^{-1}{p}^{{}^{\prime }}$ 而非矫正后的$p^{{}^{\prime }}$。因此最后求解H时候没有左乘K.inv()。

四、总结

两种方法对比，在已知世界坐标系下平面位置的时候，明显是第二种更加方便。并且，opencv为第二种方法提供了接口和简单的教程。但是在slam中，由于缺少标定板这种已知的先验条件，第一种方法更加普遍。东北大学吴博曾经在泡泡机器人公开课上对第一种方法求解单应矩阵做过详细的分析，如果有需要可以参考他的讲解学习。