Decompose Planar Homography Matrix
本篇解释下平面物体的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 <[email protected]>
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，得出了下面的实验性结论：
- 外参的角度估计的较准确，外参的shift估计的相对不太可信。
- 修改内参的shift对外参角度估计影响非常大，往往会得到不太可信的结果；符合理论结果。
- 当内参shift为0时，修改内参的scale，外参角度的估计依然相对准确，但\(f_x\)和\(f_y\)相差较大时，估计偏差会变得比较大，因为我们用的前两列的norm的平均值估计的其scale，两者相差越大估计约不准确，符合理论结果。
总结而言，如果已知相机的内参矩阵，可以估计到一个相对很准确的结果；如果不知道相机的内参矩阵，但知道其内参shift很小，x轴和y轴scale相差较小，也可以利用上述方法估计到一个相对准确的外参角度。
标签：Planar,Homography,Matrix,print,points,fake,np,array,cv From： https://www.cnblogs.com/aoru45/p/17120154.html