通过单应矩阵(Homography)求解旋转和平移

作者: 轻骑兵1390 | 来源:发表于2020-08-26 15:09 被阅读0次

本文参考Motion and structure from motion in a piecewise planar environment和ORB-SLAM代码共同完成, 部分证明还需要结合原文, 这里仅仅梳理算法流程.

1. 单应(Homography)矩阵的定义

假定空间中有一个平面, 落在平面上的点 $P$ 满足方程满足:
$\mathbf{n}^T P = d$

一个空间点 $P$ , 在相机1内的坐标为 $P_1$ , 在两个相机内的投影分别为
$\begin{aligned} p_1 &= K P_1 \\ p_2 &= K(R_{21}P_1 + t_{21}) \\ \end{aligned}$
其中, $R_{21}$ , $t_{21}$ 分别代表两时刻的旋转和平移关系. $K$ 为内参数矩阵, 假定两个相机的内参数矩阵相同. 注意大写为空间点, 小写为图像坐标系的齐次坐标.

$\begin{aligned} p_2 &= K(R_{21}P_1 + t_{21}\cdot\frac{\mathbf{n}^TP_1}{ d }) \\ &= K(R_{21} + t_{21}\cdot\frac{\mathbf{n}^T}{d})K^{-1}p_1 \end{aligned}$
考虑到 $p_1$ , $p_2$ 均为归一化坐标, 因此可以对其乘以 $d$
$p_2 = K(d\cdot R_{21} + t_{21}\cdot{\mathbf{ n }^T})K^{ -1 }p_1$
$H = K(d \cdot R_{21} + t_{21}\cdot{\mathbf{n}^T})K^{ -1 }$
$H$ 为单应矩阵, 满足 $p_2 = Hp_1$ . 单应矩阵可以通过4组匹配点, 可以通过线性变换发直接求解其8个变量.

2.通过单应矩阵求解平移和旋转

根据Motion and structure from motion in a piecewise planar environment, 先将单应矩阵 $H$ 转换为 $A$ 矩阵:
$A = d \cdot R_{21} + t_{21}\cdot{\mathbf{n}^T}$
对应代码为:

  cv::Mat A = invK*H21*K;

那么, $A = K^{-1}HK$ , 三维空间点满足 $P_2 = A \cdot P_1$ .

对 $A$ 进行SVD分解, $A = U\Lambda V^T$ , 这里( $U^TU = V^TV =I$ ), $\Lambda$ 由 $AA^T$ 的特征值的平方根组成(奇异值). $A\in R^{3\times3}$ , 因此存在三个奇异值 $d_1\geq d_2 \geq d_3$ .
使用奇异值分解可以将 $A = d\cdot R + t\cdot \mathbf{n}^{T}$ 化简为:
$\Lambda = d^\prime R^\prime + r^\prime \mathbf{n}^{\prime^T}$
新的 $()^\prime$ 和旧的符号之间的关系为:
$\begin{array}{l} {R}={s} {U} {R}^{\prime} {V}^{t} \\ {t}={U} {t}^{\prime} \\ \mathbf{n}={V} \mathbf{n}^{\prime} \\ d={s} d^{\prime} \\ {s}=\operatorname{det}{U} \cdot \operatorname{det} {V} \end{array}$
这一组公式是后续重要的计算 $R$ 和 $t$ 的依据.

可以证明, $d^\prime = \plusmn d_2$ , 见原文. 进而求得
$\mathbf{n}^\prime = \begin{bmatrix} \varepsilon_1 \sqrt{\frac{d_1^2 - d_2^2}{d_1^2-d_3^2}} & 0 & \varepsilon_3 \sqrt{\frac{d_2^2 - d_3^2}{d_1^2-d_3^2}} \end{bmatrix}^T$
其中 $\varepsilon_1, \varepsilon_3 = \plusmn 1$ .

至此, 代码描述如下:

cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
float s = cv::determinant(U)*cv::determinant(Vt);

float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};

$\mathbf{n}^\prime = [x_1, 0, x_3]^T$ . 那么 $\mathbf{n}^\prime$ 有4中排列组合, 在考虑 $d^\prime$ 的正负, 一共有8中排列组合.

那么下面根据 $d^\prime$ 的正负进行分类处理

2.1 $d^\prime > 0$

那么
$R = \begin{bmatrix} cos\theta & 0 & -sin\theta \\ 0 & 1 & 0 \\ sin\theta& 0 & cos\theta \end{bmatrix}$
其中,
$\begin{aligned} sin\theta &= \varepsilon_1\varepsilon_3\frac{\sqrt{(d_1^2-d_2^2)(d_2^2-d_3^2)}}{(d_1+d_3)d_2} \\ cos\theta &= \frac{d_2^2 + d_1d_3}{(d_1+d_3)d_2} \end{aligned}$

float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
for(int i=0; i<4; i++)
{
  cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
  Rp.at<float>(0,0)=ctheta;
  Rp.at<float>(0,2)=-stheta[i];
  Rp.at<float>(2,0)=stheta[i];
  Rp.at<float>(2,2)=ctheta;

  cv::Mat R = s*U*Rp*Vt;

${R}={s} {U} {R}^{\prime} {V}^{t}$

  cv::Mat tp(3,1,CV_32F);
  tp.at<float>(0)=x1[i];
  tp.at<float>(1)=0;
  tp.at<float>(2)=-x3[i];
  tp*=d1-d3;

  cv::Mat t = U*tp;

$t^\prime = (d_1 + d_3) \cdot \begin{bmatrix} x_1 & 0 & x_3\end{bmatrix}^T$ , ${t}={U} {t}^{\prime}$ .

2.2 $d^\prime < 0$

见论文.

通过单应矩阵(Homography)求解旋转和平移

1. 单应(Homography)矩阵的定义

2.通过单应矩阵求解平移和旋转

2.1 $d^\prime > 0$

2.2 $d^\prime < 0$

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读