史上最简SLAM零基础解读(4) - 单应性Homography →公式推导与细节理解

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注

一、前言

该篇博客，主要是对 Homography 矩阵进行一个细致的讲解，主要分为三个部分: 基本介绍、参数设定、公式推导、八点法求解。废话也不多说，那么我们下面就开始吧。

二、基本介绍

单应性矩阵 $\mathbf{H}$(Homography), 其约束了同一 3D 空间点，在两个 $像素平面$ 的 2D $齐次坐标$，现在我们假设 $p_{a1},p_{b1}$ 分别是两个像素平面 $A$, $B$ 的像素坐标,以及 单应性矩阵 ${\mathbf{H}}_{ba}$(能够把像素坐标$p_{a1}$变换到$p_{b1}$的矩阵):
$$p_{b1}=\left[\begin{array}{c}{u}_{b1}\\ v_{b1}\\ 1\end{array}\right]p_{b1}=\left[\begin{array}{c}{u}_{a1}\\ v_{a1}\\ 1\end{array}\right]{\mathbf{H}}_{ba}=\left[\begin{array}{lll}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\text{\hspace{1em}(01)}$$
其上的 $p_{a1},p_{b1}$ (一般是已知量)是两张图像对应的像素齐次坐标，也可以为理解为一对匹配特征点，那么他们对应关系如下：$$p_{b1}\propto \mathbf{H}{p}_{a1}\text{\hspace{1em}(02)}$$其上正比于符号 $\propto$(这里并非是等号,大家需要注意一点) 约束了 $p_{b1}$ 与 ${\mathbf{H}}_{ba}{p}_{a1}$ 的方向是同方向，但是没有约束尺度。那么可以通过叉乘计算是可以消去尺度因子，因此上式还可以转化为等式，表达如下:
$$p_{b1}\times {\mathbf{H}}_{ba}{p}_{a1}=0\text{\hspace{1em}(03)}$$如果两个向量方向相同则叉乘为 $0$ 向量，不是很明白的朋友可以百度一下向量叉乘的相关知识。经过详细推导，最终可以获得如下公式：
$${\mathbf{H}}_{ba}={\mathbf{K}}_b\left({\mathbf{R}}_{ba}-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}={\mathbf{K}}_b{\mathbf{R}}_{ba}\left(\mathbf{E}+\frac{1}{d_a}\cdot {\mathbf{t}}_{ab}{\mathbf{n}}_a^T\right){\mathbf{K}}_a^{-1}\text{\hspace{1em}(04)}$$
其上的 ${\mathbf{R}}_{ba}$ 表示相机坐标

三、参数设定

在相机坐标系下，3D 空间坐标为 $\mathbf{X}$，投影到像素坐标系的齐次坐标为p,$\mathbf{K}$ 为相机内参，分别表示如下，：$$\mathbf{X}=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]p=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\cdot \mathbf{K}\mathbf{X}\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(05)}$$
另外 3D 空间点 $\mathbf{X}$ 所在平面，在相机坐标系得下参数: $\mathbf{n}$ 表示该平面的法向量，$d$ 表示相机坐标系原点到平面的距离。3D 点 $\mathbf{X}$ 位于平面 { n , d } {mathbf n,d} {n,d} 上, 其方程表达式为：$${\mathbf{n}}^T\cdot \mathbf{X}+d=0\text{\hspace{1em}(06)}$$
上面我们已经相机系坐标转像素系坐标，那么反过来像素系坐标转相机系坐标公式如下：
$$\mathbf{X}=Z\cdot {\mathbf{K}}^{-1}p\text{\hspace{1em}(07)}$$那么这里出现了一个问题。就是说，如果我们知道一个相机坐标系下的三维点与相机内参的时候，根据公式(5)中的第二项是可以求解出对应的像素坐标的。但是在已知像素坐标的情况下，无法求解出该像素点对应相机坐标系下的三维点。根据公式(07)可知，还缺少一个变量 $Z$, 只要当 $Z$ 已知的情况下，才能求解出相机系下对应的三维点。如果知道3D点所在平面参数，也就是(06)式中的 { n , d } {mathbf n,d} {n,d}。那么结合(06),(07)整理得：$$Z\cdot {\mathbf{n}}^T{\mathbf{K}}^{-1}p+d=0Z=-\frac{d}{{\mathbf{n}}^T{\mathbf{K}}^{-1}p}\text{\hspace{1em}(08)}$$也就是，如果知道3D点所在平面的相关参数，那么我们就有办法求解出3D点的深度 $Z$，把 (08)式 代入 (07)式 可得：$$\mathbf{X}=-\frac{d}{{\mathbf{n}}^T{\mathbf{K}}^{-1}p}\cdot {\mathbf{K}}^{-1}p\text{\hspace{1em}(09)}$$

四、公式推导

在前面提到，Homography 是像素坐标系之间的转换。比如(02)表示的就是由像素坐标系 $a$ 转到 像素坐标系 $b$。 下面我们来推导其具体过程，根据相机成像原理，相机坐标系a下的点 ${\mathbf{X}}_{\mathbf{a}}$, 和相机坐标系下的 ${\mathbf{X}}_{\mathbf{b}}$,可知公式如下：$$\left[\begin{array}{c}{\mathbf{X}}_b\\ 1\end{array}\right]=\underset{{\mathbf{T}}_{ba}}{\underbrace{\left[\begin{array}{cc}{\mathbf{R}}_{ba}& {\mathbf{t}}_{ba}\\ 0& 1\end{array}\right]}}\left[\begin{array}{c}{\mathbf{X}}_a\\ 1\end{array}\right]\text{\hspace{1em}(10)}$$$${\mathbf{X}}_b={\mathbf{R}}_{ba}{\mathbf{X}}_a+{\mathbf{t}}_{ba}\text{\hspace{1em}(12)}$$其中：
        ${\mathbf{R}}_{ba}$ →表示 a系 到 b系的坐标旋转变换(也可以理解为，b系下a系的姿态)
        ${\mathbf{t}}_{ba}$   →表示 a系 到 b系的坐标平移变换(也可以理解为，b系下a系的位置)。

再根据(07)式子，可得$$Z_b\cdot {\mathbf{K}}_b^{-1}{p}_b=Z_a\cdot {\mathbf{R}}_{ba}{\mathbf{K}}_a^{-1}{p}_a+{\mathbf{t}}_{ba}\text{\hspace{1em}(13)}$$那么继续化简，可得到由 a系下 表达 b系像素：$$p_b=\frac{Z_a}{Z_b}\cdot {\mathbf{K}}_b{\mathbf{R}}_{ba}{\mathbf{K}}_a^{-1}{p}_a+\frac{1}{Z_b}\cdot {\mathbf{K}}_b{\mathbf{t}}_{ba}\text{\hspace{1em}(14)}$$假设其上的 ${\mathbf{R}}_{ba}$,${\mathbf{t}}_{ba}$ 为已知量, 但是跟前面结论一样，因为存在未知参数 $Z_b,Z_a$, 所以无法直接通过 a系像素 得到 b系像素。那么我们根据前面的推导(08),引入a系中平面参数:$$\begin{array}{rl}{p}_b& =\frac{Z_a}{Z_b}\cdot {\mathbf{K}}_b{\mathbf{R}}_{ba}{\mathbf{K}}_a^{-1}{p}_a+\frac{1}{Z_b}\cdot {\mathbf{K}}_b{\mathbf{t}}_{ba}\\ & =\frac{Z_a}{Z_b}\cdot {\mathbf{K}}_b\left({\mathbf{R}}_{ba}{\mathbf{K}}_a^{-1}{p}_a+\frac{{\mathbf{t}}_{ba}}{Z_a}\right)\\ & =\frac{Z_a}{Z_b}\cdot {\mathbf{K}}_b\left({\mathbf{R}}_{ba}{\mathbf{K}}_a^{-1}{p}_a-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T{\mathbf{K}}_a^{-1}{p}_a}{d_a}\right)\\ & =\frac{Z_a}{Z_b}\cdot {\mathbf{K}}_b\left({\mathbf{R}}_{ba}-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{p}_a\end{array}\text{\hspace{1em}(15)}$$到这个位置呢，可以说是基本推导完成了，另外如果 $p_b$ 使用齐次像素坐标 $(x,y,1{)}^T$ 与 $\lambda (x,y,1{)}^T$ 以及 $(\lambda x,\lambda y,\lambda {)}^T$ 其对应的都是空间中的同一三维点，也就是所谓的齐次坐标尺度不变性。根据这个性质，可以省略掉 $\frac{Z_a}{Z_b}$ 这个尺度因子。那么上式可以写成:$$p_b\propto {\mathbf{K}}_b\left({\mathbf{R}}_{ba}-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{p}_a\text{\hspace{1em}(16)}$$然后再进行符号简化,如下：$${\mathbf{H}}_{ba}={\mathbf{K}}_b\left({\mathbf{R}}_{ba}-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{p}_b={\mathbf{H}}_{ba}{p}_a\text{\hspace{1em}(17)}$$另外我们根据变换矩阵${\mathbf{T}}_{ab}{\mathbf{T}}_{ba}=\mathbf{E}$(单位矩阵),如下：$${\mathbf{T}}_{\mathbf{a}\mathbf{b}}=\left[\begin{array}{cc}{\mathbf{R}}_{\mathbf{a}\mathbf{b}}& {\mathbf{t}}_{\mathbf{a}\mathbf{b}}\\ & \\ 0^{\mathbf{T}}& 1\end{array}\right]{\mathbf{T}}_{\mathbf{b}\mathbf{a}}=\left[\begin{array}{cc}{\mathbf{R}}_{\mathbf{b}\mathbf{a}}& {\mathbf{t}}_{\mathbf{b}\mathbf{a}}\\ & \\ 0^{\mathbf{T}}& 1\end{array}\right]\text{\hspace{1em}(18)}$$我们可以推导出$${\mathbf{R}}_{ba}{\mathbf{t}}_{ab}=-{\mathbf{t}}_{ba}\text{\hspace{1em}(19)}$$
把这个带入到(19)式子当中继续推导如下:$$\begin{array}{rl}{\mathbf{H}}_{ba}& ={\mathbf{K}}_b\left({\mathbf{R}}_{ba}-\frac{{\mathbf{t}}_{ba}{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}\\ & ={\mathbf{K}}_b\left({\mathbf{R}}_{ba}+{\mathbf{R}}_{ba}{\mathbf{t}}_{ab}\frac{{\mathbf{n}}_a^T}{d_a}\right){\mathbf{K}}_a^{-1}{p}_a\\ & ={\mathbf{K}}_b{\mathbf{R}}_{ba}\left(\mathbf{E}+\frac{1}{d_a}\cdot {\mathbf{t}}_{ab}{\mathbf{n}}_a^T\right){\mathbf{K}}_a^{-1}{p}_a\\ & ={\mathbf{K}}_b{\mathbf{R}}_{ba}\left(\mathbf{E}+\frac{1}{d_a}\cdot {\mathbf{t}}_{ab}{\mathbf{n}}_a^T\right){\mathbf{K}}_a^{-1}{p}_a\end{array}\text{\hspace{1em}(20)}$$这样结合起来就得到前面(04)式。竟然我们推导出 $\mathbf{H}$ 矩阵，那么如何进行求解呢，如下推导。

五、八点法求解

根据前面的公式(02) $p_{b1}\propto \mathbf{H}{p}_{a1}$，其表示的是一对匹配点,使用齐次坐标展开如下：$$\left[\begin{array}{c}{u}_{b1}\\ v_{b1}\\ 1\end{array}\right]\propto \left[\begin{array}{lll}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]\left[\begin{array}{c}{u}_{a1}\\ v_{a1}\\ 1\end{array}\right]\text{\hspace{1em}(21)}$$转化为方程组如下：
$$\{\begin{array}{rl}{u}_{b1}& =h_{11}{u}_{a1}+h_{12}{v}_{a1}+h_{13}\\ v_{b1}& =h_{21}{u}_{a1}+h_{22}{v}_{a1}+h_{23}\\ 1& =h_{31}{u}_{a1}+h_{32}{v}_{a1}+h_{33}\end{array}\Rightarrow \{\begin{array}{rl}{u}_{b1}& =\frac{u_{b1}}{1}=\frac{h_{11}{u}_{a1}+h_{12}{v}_{a1}+h_{13}}{h_{31}{u}_{a1}+h_{32}{v}_{a1}+h_{33}}\\ v_{b1}& =\frac{v_{b1}}{1}=\frac{h_{21}{u}_{a1}+h_{22}{v}_{a1}+h_{23}}{h_{31}{u}_{a1}+h_{32}{v}_{a1}+h_{33}}\end{array}\text{\hspace{1em}(22)}$$进一步转换如下：$$\{\begin{array}{rl}{u}_{b1}(h_{31}{u}_{a1}+h_{32}{v}_{a1}+h_{33})& =h_{11}{u}_{a1}+h_{12}{v}_{a1}+h_{13}\\ & \\ v_{b1}(h_{31}{u}_{a1}+h_{32}{v}_{a1}+h_{33})& =h_{21}{u}_{a1}+h_{22}{v}_{a1}+h_{23}\end{array}\text{\hspace{1em}(23)}$$然后我买了转化为一般式:$$\{\begin{array}{r}{h}_{11}{u}_{a1}+h_{12}{v}_{a1}+h_{13}-h_{31}{u}_{a1}{u}_{b1}-h_{32}{v}_{a1}{u}_{b1}-h_{33}{u}_{b1}=0\\ \\ h_{21}{u}_{a1}+h_{22}{v}_{a1}+h_{23}-h_{31}{u}_{a1}{v}_{b1}-h_{32}{v}_{a1}{v}_{b1}-h_{33}{v}_{b1}=0\end{array}\text{\hspace{1em}(24)}$$ 因为使用齐次坐标，那么 $\mathbf{H}$ 的作用与 $a\mathbf{H}$ 是一样的，也就是 $\frac{1}{h33}\mathbf{H}$ 与 $\mathbf{H}$ 效果一致，或者也可以令 $h_{33}=1$ 写成如下形式。$$\mathbf{H}=\left[\begin{array}{lll}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& 1\end{array}\right]\text{\hspace{1em}(25)}$$
八对特征点的方程组如下：
$$\{\begin{array}{r}{h}_{11}{u}_{a1}+h_{12}{v}_{a1}+h_{13}-h_{31}{u}_{a1}{u}_{b1}-h_{32}{v}_{a1}{u}_{b1}-h_{33}{u}_{b1}=0\\ h_{21}{u}_{a1}+h_{22}{v}_{a1}+h_{23}-h_{31}{u}_{a1}{v}_{b1}-h_{32}{v}_{a1}{v}_{b1}-h_{33}{v}_{b1}=0\\ h_{11}{u}_{a2}+h_{12}{v}_{a2}+h_{13}-h_{31}{u}_{a2}{u}_{b2}-h_{32}{v}_{a2}{u}_{b2}-h_{33}{u}_{b2}=0\\ h_{21}{u}_{a2}+h_{22}{v}_{a2}+h_{23}-h_{31}{u}_{a2}{v}_{b2}-h_{32}{v}_{a2}{v}_{b2}-h_{33}{v}_{b2}=0\\ h_{11}{u}_{a3}+h_{12}{v}_{a3}+h_{13}-h_{31}{u}_{a3}{u}_{b3}-h_{32}{v}_{a3}{u}_{b3}-h_{33}{u}_{b3}=0\\ h_{21}{u}_{a3}+h_{22}{v}_{a3}+h_{23}-h_{31}{u}_{a3}{v}_{b3}-h_{32}{v}_{a3}{v}_{b3}-h_{33}{v}_{b3}=0\\ h_{11}{u}_{a4}+h_{12}{v}_{a4}+h_{13}-h_{31}{u}_{a4}{u}_{b4}-h_{32}{v}_{a4}{u}_{b4}-h_{33}{u}_{b4}=0\\ h_{21}{u}_{a4}+h_{22}{v}_{a4}+h_{23}-h_{31}{u}_{a4}{v}_{b4}-h_{32}{v}_{a4}{v}_{b4}-h_{33}{v}_{b4}=0\end{array}\text{\hspace{1em}(26)}$$：上面的方程组就可以转化成矩阵形式:$$\left[\begin{array}{ccccccccc}{u}_{a1}& v_{a1}& 1& 0& 0& 0& -u_{a1}{u}_{b1}& -v_{a1}{u}_{b1}& -u_{b1}=0\\ 0& 0& 0& u_{a1}& v_{a1}& 1& -u_{a1}{v}_{b1}& -v_{a1}{v}_{b1}& -v_{b1}=0\\ u_{a2}& v_{a2}& 1& 0& 0& 0& -u_{a2}{u}_{b2}& -v_{a2}{u}_{b2}& -u_{b2}=0\\ 0& 0& 0& u_{a2}& v_{a2}& 1& -u_{a2}{v}_{b2}& -v_{a2}{v}_{b2}& -v_{b2}=0\\ u_{a3}& v_{a3}& 1& 0& 0& 0& -u_{a3}{u}_{b3}& -v_{a3}{u}_{b3}& -u_{b3}=0\\ 0& 0& 0& u_{a3}& v_{a3}& 1& -u_{a3}{v}_{b3}& -v_{a3}{v}_{b3}& -v_{b3}=0\\ u_{a4}& v_{a4}& 1& 0& 0& 0& -u_{a4}{u}_{b4}& -v_{a4}{u}_{b4}& -u_{b4}=0\\ 0& 0& 0& u_{a4}& v_{a4}& 1& -u_{a4}{v}_{b4}& -v_{a4}{v}_{b4}& -v_{b4}=0\end{array}\right]\left[\begin{array}{l}{h}_{11}\\ h_{12}\\ h_{13}\\ h_{21}\\ h_{22}\\ h_{23}\\ h_{31}\\ h_{32}\\ h_{33}\end{array}\right]=0\text{\hspace{1em}(27)}$$因为使用齐次坐标, 可以令$h_{33}=1$,那么我们只需要求解八个未知数，上式我们简写成$${\mathbf{A}}_{(8,9)}{\mathbf{H}}_{(9,1)}=0\text{\hspace{1em}(28)}$$

该公式在已知 ${\mathbf{A}}_{(8,9)}$ 的情况下，我们通过 SVD奇异值分解，即可求解出单应性 ${\mathbf{H}}_{(9,1)}$ 矩阵。

六、结语

通过该篇博客，我们已经对 单应性Homography 矩阵进行了详细的讲解。下面我们会结合前面的本质矩阵与基本矩阵进行一个对比，然后讲解他们的适用场景。或者说，什么情况下适合 单应性Homography 矩阵，什么情况下适合本质或者基本矩阵。
 
 
 

最后

以上就是喜悦冷风为你收集整理的史上最简SLAM零基础解读(4) - 单应性Homography →公式推导与细节理解的全部内容，希望文章能够帮你解决史上最简SLAM零基础解读(4) - 单应性Homography →公式推导与细节理解所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。