[视觉] Decompose Planar Homography Matrix

平面下Homography矩阵的分解

本篇解释下平面物体的homography矩阵分解，即得到相机的位姿，来分析相机是否正确放置，在机器视觉中有着较为广泛的应用。

原理

我们已知平面内同一物体在两图像坐标系的坐标为\(p_1 = (x_1, y_1)\)，\(p_2 = (x_2, y_2)\)。

工业场景上，\(p_1\)就可以认为是人工构造的坐标系，比如在cad模型的某个截面上的坐标系，一般不是相机拍摄得到的，但是为了套用相机模型，我们认为这个坐标系是一虚拟坐标系，内参为单位阵。而\(p_2\)就是真实相机拍摄之后成像在图像上的图像坐标系，其相机的内参可以表示为：

\[K= \begin{bmatrix}f_x & 0 & d_x \\ 0 & f_y & d_y \\ 0 & 0 & 1\end{bmatrix} \]

由于\(P_1\)在虚拟坐标系中，我们是可以随意规定坐标系原点的，而且观测的物体在一平面上，因此可以假定物体在世界系下的坐标为\(P_1 = (X_1, Y_1, Z_1) = (X_1, Y_1, 0)\)

根据相机模型

\[KX_2=x_2 \]

我们通过最小二乘法等方法求得的homography matrix \(\hat{H}\)表示从虚拟坐标系到图像坐标系的变换，表示为：

\[x_2 = \hat{H}X_1 \]

而根据定义，外参阵是在相机坐标系下的转换：

\[x_2 = \lambda K\begin{bmatrix}r_1&r_2&t\end{bmatrix}X_1 \]

所以：

\[\hat{H} = \lambda K\begin{bmatrix}r_1&r_2&t\end{bmatrix} \]

又由于内参矩阵可以分解为：

\[K= \begin{bmatrix}f_x & 0 & d_x \\ 0 & f_y & d_y \\ 0 & 0 & 1\end{bmatrix} = \begin{bmatrix}1 & 0 & d_x \\ 0 & 1 & d_y \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 1\end{bmatrix} \]

所以：

\[\hat{H} = \lambda \begin{bmatrix}1 & 0 & d_x \\ 0 & 1 & d_y \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}r_1&r_2&t\end{bmatrix} \]

因此考虑\(d_x\)和\(d_y\)相对较小时，可以用前两列来估计\(r_1\)和\(r_2\)

至于\(r_3\)，由于旋转矩阵本身就是正交矩阵，所以直接叉积就可以得到\(r_3=r_1\otimes r_2\); 至于t，实际上不清楚\(d_x\)和\(d_y\)，感觉无法得到一个相对准确的估计。

Code

可以简单得到旋转矩阵的一个估计：

cv::Mat ParseH(cv::Mat H) {
  cv::Mat R = cv::Mat(cv::Size(3, 3), CV_64F);
  cv::Mat c0 = H.col(0);
  cv::Mat c1 = H.col(1);
  double s = 0.5 * (cv::norm(c0) + cv::norm(c1));
  c0 = c0 / s;
  c1 = c1 / s;
  cv::Mat c2 = c0.cross(c1);
  for (int i = 0; i < 3; ++i) {
    R.at<double>(i, 0) = c0.at<double>(i, 0);
    R.at<double>(i, 1) = c1.at<double>(i, 1);
    R.at<double>(i, 2) = c2.at<double>(i, 2);
  }
  return R;
}

其中，s用前两列的norm的平均值，要稍微更鲁棒点。

尽管如此，我们还是不能完全保证前两列的正交性以及norm=1，即不能完全保证R正交，所以我们需要进一步估计一个旋转矩阵，使得其满足正交，先对R进行svd分解，此时我们可以直接令特征值都为1（因为正交矩阵的特征值均为±1），此时可以得到下面的估计:

\[R = svd(\hat{R}) = U\Sigma V^T =UV^T \]

cv::Mat ParseH(cv::Mat H) {
  cv::Mat R = cv::Mat(cv::Size(3, 3), CV_64F);
  cv::Mat c0 = H.col(0);
  cv::Mat c1 = H.col(1);
  double s = 0.5 * (cv::norm(c0) + cv::norm(c1));
  c0 = c0 / s;
  c1 = c1 / s;
  cv::Mat c2 = c0.cross(c1);
  for (int i = 0; i < 3; ++i) {
    R.at<double>(i, 0) = c0.at<double>(i, 0);
    R.at<double>(i, 1) = c1.at<double>(i, 1);
    R.at<double>(i, 2) = c2.at<double>(i, 2);
  }
	cv::Mat w,u,vt;
	cv::SVD::compute(R, w, u, vt);
	R = u * vt;
  return R;
}

实验

为了快速验证实验，用py快速写了个验证一下：

"""

==============================================================
Author: aoruxue <aoru45@t.shu.edu.cn>
Github: https://github.com/aoru45
License: MIT
==============================================================

"""
# coding=utf-8

import numpy as np
import cv2 as cv

# parameters to set
angles = [15. / 180 * np.pi, 25. / 180 * np.pi, 35. / 180 * np.pi]
shifts = [10, 20, 1]
K = np.array([[0.91, 0, 0], [0, 0.8, 0], [0, 0, 1]], dtype=np.float32)

rx = np.array([[1, 0, 0], [0, np.cos(angles[0]), -np.sin(angles[0])],
               [0, np.sin(angles[0]), np.cos(angles[0])]],
              dtype=np.float32)
ry = np.array(
    [[np.cos(angles[1]), 0, np.sin(angles[1])], [0, 1, 0],
     [-np.sin(angles[1]), 0, np.cos(angles[1])]],
    dtype=np.float32)
rz = np.array([[np.cos(angles[2]), -np.sin(angles[2]), 0],
               [np.sin(angles[2]), np.cos(angles[2]), 0], [0, 0, 1]],
              dtype=np.float32)
r = rz @ ry @ rx
t = np.array([[shifts[0]], [shifts[1]], [shifts[2]]])
rt = np.concatenate([r, t], axis=1)

# fake data
fake_src_points_array = np.random.randint(1, 100,
                                          size=(1000, 2)).astype(np.float32)
fake_src_points_array = np.append(fake_src_points_array,
                                  np.zeros((len(fake_src_points_array), 1)),
                                  axis=1)
fake_src_points_array = np.append(fake_src_points_array,
                                  np.ones((len(fake_src_points_array), 1)),
                                  axis=1)
fake_tgt_points_array = K @ rt @ fake_src_points_array.T
fake_tgt_points_array = fake_tgt_points_array.T
fake_tgt_points_array = fake_tgt_points_array / (fake_tgt_points_array[:, 2:])
fake_tgt_points_array = fake_tgt_points_array[:, :2]
fake_tgt_points_array += np.random.randn(fake_tgt_points_array.shape[0],
                                         2) * 0.2 - 0.1
print("src_points")
print(fake_src_points_array)
print("tgt_points")
print(fake_tgt_points_array)

H_hat, ret = cv.findHomography(fake_src_points_array[:, :2],
                               fake_tgt_points_array)
print("pred_points")
print(cv.perspectiveTransform(fake_src_points_array[None, :, :2], H_hat))
# exit()

def ParseH(H):
    R = np.zeros((3, 3), dtype=np.float64)
    c0 = H[:, 0]
    c1 = H[:, 1]
    s = 0.5 * (cv.norm(c0) + cv.norm(c1))
    # s = cv.norm(c0)
    c0 = c0 / s
    c1 = c1 / s
    c2 = np.cross(c0, c1)
    R[:, 0] = c0
    R[:, 1] = c1
    R[:, 2] = c2
    u, w, vt = np.linalg.svd(R)
    R = u @ vt
    det = np.linalg.det(R)
    if det < 0:
        R[:, 2] *= -1
    return R

def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def R2angle(R):
    assert (isRotationMatrix(R))
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2]) / np.pi * 180
        y = np.arctan2(-R[2, 0], sy) / np.pi * 180
        z = np.arctan2(R[1, 0], R[0, 0]) / np.pi * 180
    else:
        x = np.arctan2(-R[1, 2], R[1, 1]) / np.pi * 180
        y = np.arctan2(-R[2, 0], sy) / np.pi * 180
        z = 0
    return x, y, z

r_ = ParseH(H_hat)
print("#" * 20)
print(fake_tgt_points_array)
print(cv.perspectiveTransform(fake_src_points_array[:, :2][None, :, :], H_hat))
print("#" * 20)
print(r @ r_.T)
H = (K @ rt)[:, :3]
# H = H / H[2,2]
print("Origin R")
print(r)
print("Estimated R")
print(r_)
print("Origin t")
print(t)
print("Estimated t")
print(H_hat[:, 2:])
print("Origin angles")
print(R2angle(r))
print("Estimated angles")
print(R2angle(r_))

实验输出：

src_points
[[92. 52.  0.  1.]
 [74.  9.  0.  1.]
 [10.  7.  0.  1.]
 ...
 [49. 48.  0.  1.]
 [77. 72.  0.  1.]
 [71. 37.  0.  1.]]
tgt_points
[[ -1.7652921   -3.49674866]
 [ -2.06744601  -1.82636204]
 [ -8.28426383 -15.54311833]
 ...
 [ -2.52789181  -8.16613232]
 [ -2.21351135  -6.82825273]
 [ -2.0618189   -3.72601529]]
pred_points
[[[ -2.02439464  -3.59850013]
  [ -2.06919778  -1.98138974]
  [ -8.25387955 -15.83717279]
  ...
  [ -2.70025633  -8.28922665]
  [ -2.20021869  -6.73736389]
  [ -2.1440624   -3.58570354]]]
####################
[[ -1.7652921   -3.49674866]
 [ -2.06744601  -1.82636204]
 [ -8.28426383 -15.54311833]
 ...
 [ -2.52789181  -8.16613232]
 [ -2.21351135  -6.82825273]
 [ -2.0618189   -3.72601529]]
[[[ -2.02439464  -3.59850013]
  [ -2.06919778  -1.98138974]
  [ -8.25387955 -15.83717279]
  ...
  [ -2.70025633  -8.28922665]
  [ -2.20021869  -6.73736389]
  [ -2.1440624   -3.58570354]]]
####################
[[ 0.99960997  0.02475654 -0.01292825]
 [-0.02463118  0.99964892  0.00976371]
 [ 0.0131654  -0.00944149  0.99986878]]
Origin R
[[ 0.7424039  -0.46443212  0.48284504]
 [ 0.51983684  0.8539788   0.02213201]
 [-0.42261827  0.23456973  0.8754261 ]]
Estimated R
[[ 0.72374611 -0.48219726  0.49363688]
 [ 0.54202379  0.83996662  0.02581248]
 [-0.42708521  0.24888125  0.86928496]]
Origin t
[[10]
 [20]
 [ 1]]
Estimated t
[[ 9.00688684]
 [15.88149316]
 [ 1.        ]]
Origin angles
(15.000000417413029, 24.999998418958658, 34.99999983559887)
Estimated angles
(15.97671968062814, 25.28272195726095, 36.83006405710997)

可见估计的角度和真实角度差别并不是特别大，最多也就2点角度的差别。

我尝试修改了下相机的内参和外参的shift，得出了下面的实验性结论：

1. 外参的角度估计的较准确，外参的shift估计的相对不太可信。
2. 修改内参的shift对外参角度估计影响非常大，往往会得到不太可信的结果；符合理论结果。
3. 当内参shift为0时，修改内参的scale，外参角度的估计依然相对准确，但\(f_x\)和\(f_y\)相差较大时，估计偏差会变得比较大，因为我们用的前两列的norm的平均值估计的其scale，两者相差越大估计约不准确，符合理论结果。

总结而言，如果已知相机的内参矩阵，可以估计到一个相对很准确的结果；如果不知道相机的内参矩阵，但知道其内参shift很小，x轴和y轴scale相差较小，也可以利用上述方法估计到一个相对准确的外参角度。