[VP] 证明多视觉几何中平行线段长度比不变

---

一、算法原理

首先有四个点 $p_1,p_2,q_1,q_2$，经过单应矩阵 $H_a$ 变换后得到新的点：
$$p_1^{{}^{\prime }}=H_a{p}_1,p_2^{{}^{\prime }}=H_a{p}_2$$

$$q_1^{{}^{\prime }}=H_a{q}_1,q_2^{{}^{\prime }}=H_a{q}_2$$

然后计算平行线段长度D

$$D_1=[p_1-p_2{]}^T[p_1-p_2]$$

$$D_1^{{}^{\prime }}=[p_1^{{}^{\prime }}-p_2^{{}^{\prime }}{]}^T[p_1^{{}^{\prime }}-p_2^{{}^{\prime }}]$$

$$D_2=[q_1-q_2{]}^T[q_1-q_2]$$

$$D_2^{{}^{\prime }}=[q_1^{{}^{\prime }}-q_2^{{}^{\prime }}{]}^T[q_1^{{}^{\prime }}-q_2^{{}^{\prime }}]$$

最后证明下式：

$$\frac{D_1}{D_2}=\frac{D_1^{{}^{\prime }}}{D_2^{{}^{\prime }}}$$

二、代码实现

import numpy as np

Ha = np.array([[1.078, 0.0618, 1.2], [0.9019, 1.8825, 1.2], [0, 0, 1]])
Ha

array([[1.078 , 0.0618, 1.2 ],
[0.9019, 1.8825, 1.2 ],
[0. , 0. , 1. ]])

p1 = np.array([[0.5], [-1.0], [1.0]])
p2 = np.array([[1.5], [-1.0], [1.0]])
q1 = np.array([[-0.5], [-1.5], [2]])
q2 = np.array([[-1.5], [-1.5], [2]])

实现：
$$p_1^{{}^{\prime }}=H_a{p}_1,p_2^{{}^{\prime }}=H_a{p}_2$$

$$q_1^{{}^{\prime }}=H_a{q}_1,q_2^{{}^{\prime }}=H_a{q}_2$$

p1_new = np.matmul(Ha, p1)
p2_new = np.matmul(Ha, p2)
q1_new = np.matmul(Ha, q1)
q2_new = np.matmul(Ha, q2)
print('p1_new: \n', p1_new, '\n p2_new: \n', p2_new, '\n q1_new: \n', q1_new, '\n q2_new: \n', q2_new)

实现：

$$D_1=[p_1-p_2{]}^T[p_1-p_2]$$

$$D_1^{{}^{\prime }}=[p_1^{{}^{\prime }}-p_2^{{}^{\prime }}{]}^T[p_1^{{}^{\prime }}-p_2^{{}^{\prime }}]$$

$$D_2=[q_1-q_2{]}^T[q_1-q_2]$$

$$D_2^{{}^{\prime }}=[q_1^{{}^{\prime }}-q_2^{{}^{\prime }}{]}^T[q_1^{{}^{\prime }}-q_2^{{}^{\prime }}]$$

D1 = np.matmul((p1 - p2).T, (p1-p2))
D2 = np.matmul((q1 - q2).T, (q1-q2))
D1_new = np.matmul((p1_new - p2_new).T, (p1_new-p2_new))
D2_new = np.matmul((q1_new - q2_new).T, (q1_new-q2_new))
print('D1: ', D1, '\n D2: ', D2, '\n D1_new: ', D1_new, '\n D2_new: ', D2_new)

最后计算：
$$\frac{D_1}{D_2}=\frac{D_1^{{}^{\prime }}}{D_2^{{}^{\prime }}}$$

D1/D2

array([[1.]])

D1_new/D2_new

array([[1.]])

结果一样，证明完成！