[VP] 简易相机标定装置实现—计算相机标定矩阵K - 多视觉几何

---

一、算法原理

用相机拍摄一张带有三个正方形框的照片，确定其 4*3=12 个点，如下图所示：

设定单应矩阵 $\mathbf{H}$ 为：

$$H=\left[\begin{array}{ccc}{H}_{11}& H_{12}& H_{13}\\ H_{21}& H_{22}& H_{23}\\ H_{31}& H_{32}& H_{33}\end{array}\right]\text{\hspace{1em}(1)}$$

$\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]$是角点 $(0,0{)}^T,(0,1{)}^T,(1,0{)}^T,(1,1{)}^T$,

$\left[\begin{array}{c}{x}_i^{{}^{\prime }}\\ y_i^{{}^{\prime }}\\ 1\end{array}\right]$是图中正方形的四个角点

$$\left[\begin{array}{c}{x}_i^{{}^{\prime }}\\ y_i^{{}^{\prime }}\\ 1\end{array}\right]=H\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]\text{\hspace{1em}(2)}$$

根据公式 $(1)(2)$, 我们可以计算 $x^{{}^{\prime }}和y^{{}^{\prime }}$, 此处 $H_{33}=1$,因为变换使用的是齐次点, aH和 H 一样的, 所以我们可以忽略缩放. 然后得到下列式子：

$$x_i^{{}^{\prime }}=\frac{H_{11}{x}_i+H_{12}{y}_i+H_{13}}{H_{31}{x}_i+H_{32}{y}_i+1}\text{\hspace{1em}(3)}$$

$$y_i^{{}^{\prime }}=\frac{H_{21}{x}_i+H_{22}{y}_i+H_{23}}{H_{31}{x}_i+H_{32}{y}_i+1}\text{\hspace{1em}(3)}$$

分别计算完三个正方形的单应矩阵 H 后，由于图像的圆点是 $h_1\pm i{h}_2$, 如果 $h_1\pm i{h}_2$ 在 $\omega$ 上， 那么 $$(h_1\pm i{h}_2)\omega (h_1\pm i{h}_2)=0$$

那么实部和虚部分别给出：
$$h_1^T\omega h_2=0\text{\hspace{1em}(4)}$$
$$h_1^T\omega h_1=h_2^T\omega h_2\text{\hspace{1em}(4)}$$

之前已经得到了 $H_1,H_{2}and{H}_3$, 使用他们和公式 $(4)$, 就可以计算出 $\omega$ i.e. Matrix C.

最后根据下式得到标定矩阵 K：
$$\omega =(k{k}^T{)}^{-1}=K^{-T}{K}^{-1}$$

二、代码实现

import cv2
from sympy import *
from PIL import Image
import numpy as np
import matplotlib.pyplot as plt

0、显示图像和正方形的点

img = cv2.imread('Ex-4.PNG')
plt.plot(77, 73, 'o', color='r')
plt.plot(244, 37, 'o', color='r')
plt.plot(247, 165, 'o', color='r')
plt.plot(111, 209, 'o', color='r')

plt.plot(300, 40, 'o', color='g')
plt.plot(454, 98, 'o', color='g')
plt.plot(424, 231, 'o', color='g')
plt.plot(300, 172, 'o', color='g')

plt.plot(247, 196, 'o', color='b')
plt.plot(395, 235, 'o', color='b')
plt.plot(347, 363, 'o', color='b')
plt.plot(174, 305, 'o', color='b')

plt.imshow(img)

1、计算单应矩阵 H

根据之前写的公式(1)(2)(3)，计算出 H

# Compute homography  𝐇
def getH(P1, P2, P3, P4, P01, P02, P03, P04):
    H11 = Symbol('H11')
    H12 = Symbol('H12')
    H13 = Symbol('H13')
    H21 = Symbol('H21')
    H22 = Symbol('H22')
    H23 = Symbol('H23')
    H31 = Symbol('H31')
    H32 = Symbol('H32')
    
    def formula(originalPoint, squarePoint):
        f1 = (H11*originalPoint[0] + H12*originalPoint[1] + H13) / (H31*originalPoint[0] + H32*originalPoint[1] + 1) - squarePoint[0]
        f2 = (H21*originalPoint[0] + H22*originalPoint[1] + H23) / (H31*originalPoint[0] + H32*originalPoint[1] + 1) - squarePoint[1]
        return f1, f2
    
    f1, f2 = formula(P01, P1)
    f3, f4 = formula(P02, P2)
    f5, f6 = formula(P03, P3)
    f7, f8 = formula(P04, P4)
    
    result = []
    answer = solve([f1, f2, f3, f4, f5, f6, f7, f8], [H11, H12, H13, H21, H22, H23, H31, H32])
    
    for i, j in answer.items():
        result.append(j)
    H = Matrix([[result[0], result[1], result[2]], 
                [result[3], result[4], result[5]], 
                [result[6], result[7], 1]])
    return H

1.1 确定正方形的角点

P01 = [0, 0]
P02 = [0, 1]
P03 = [1, 1]
P04 = [1, 0]

P1 = [77, 73]
P2 = [244, 37]
P3 = [247, 165]
P4 = [111, 209]

P5 = [300, 40]
P6 = [454, 98]
P7 = [424, 231]
P8 = [300, 172]

P9 = [247, 196]
P10 = [395, 235]
P11 = [347, 363]
P12 = [174, 305]

1.2 计算单应矩阵 H

H1 = getH(P1, P2, P3, P4, P01, P02, P03, P04)
H2 = getH(P5, P6, P7, P8, P01, P02, P03, P04)
H3 = getH(P9, P10, P11, P12, P01, P02, P03, P04)

simplify(H1)

simplify(H2)

simplify(H3)

2、计算图像正方形的成像圆点

# COmpute omega 
def getOmega(H1, H2, H3):
    a = Symbol('a')
    b = Symbol('b')
    c = Symbol('c')
    d = Symbol('d')
    e = Symbol('e')
    f = Symbol('f')

    C = Matrix([[a, b/2, d/2], 
                [b/2, c, e/2], 
                [d/2, e/2, f]])
    
    # h1, h2 of H1 Matrix 
    h11 = H1.col(0)
    h12 = H1.col(1)

    # h1, h2 of H2 Matrix 
    h21 = H2.col(0)
    h22 = H2.col(1)

    # h1, h2 of H3 Matrix 
    h31 = H3.col(0)
    h32 = H3.col(1)
    
    f1 = h11.T * C * h12
    f2 = h11.T * C * h11 - h12.T * C * h12
    f3 = h21.T * C * h22
    f4 = h21.T * C * h21 - h22.T * C * h22
    f5 = h31.T * C * h32
    f6 = h31.T * C * h31 - h32.T * C * h32
    
    result = []
    answer = solve([f1, f2, f3, f4, f5], [a, b, c, d, e])
    
    for i, j in answer.items():
        result.append(N(j, 5) / f)
        
    return result

abcde = getOmega(H1, H2, H3)

C = Matrix([[abcde[0], abcde[1]/2, abcde[3]/2], 
           [abcde[1]/2, abcde[2], abcde[4]/2], 
           [abcde[3]/2, abcde[4]/2, 1]])
C

3、使用Cholesky因子计算标定矩阵 K

we finally get calibration matrix K, according to this following formula:

$$\omega =(k{k}^T{)}^{-1}=K^{-T}{K}^{-1}$$

K = C.cholesky(false).inv().T
K = K / K[8]
K.xreplace({n : round(n, 1) for n in K.atoms(Number)}) # Constrained decimal precision

和书上的结果对比下，数据相似但是不相同，这是由于图像缩放和选点位置误差造成的。