设备安装精度标定

今天遇到一个问题，需要标定一下设备的安装精度。

---

设备安装精度标定

• 设备在安装过程中，有时候需要标定一下安装精度，如何判断摄像头是否水平旋转，是否倾斜等

• 设备安装好后，拍摄已知坐标的标定物，选取标志点，构建相对世界坐标系，得到机械坐标

• 摄像头拍摄相对点，得到像素坐标

• 机械坐标$(x_1,y_1)$到像素坐标$(x_2,y_2)$仿射变换如下

$$\{\begin{array}{l}{x}_2=a_{11}{x}_1+a_{12}{y}_1+a_{13}\\ y_2=a_{21}{x}_1+a_{22}{y}_1+a_{23}\end{array}$$

• 最小二乘计算仿射变换关系

$$\left[\begin{array}{cccccc}{x}_1& y_1& 1& 0& 0& 0\\ 0& 0& 0& x_1& y_1& 0\end{array}\right]\left[\begin{array}{c}{a}_{11}\\ a_{12}\\ a_{13}\\ a_{21}\\ a_{22}\\ a_{23}\end{array}\right]=\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]$$

• 仿射变换矩阵分解

$$\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& T_x\\ 0& 1& T_y\\ 0& 0& 1\end{array}\right].\left[\begin{array}{ccc}cos(\varphi )& -sin(\varphi )& 0\\ sin(\varphi )& cos(\varphi )& 0\\ 0& 0& 1\end{array}\right].\left[\begin{array}{ccc}1& -sin(\theta )& 0\\ 0& cos(\theta )& 0\\ 0& 0& 1\end{array}\right].\left[\begin{array}{ccc}{S}_x& 0& 0\\ 0& S_y& 0\\ 0& 0& 1\end{array}\right]$$

• 解方程，求得转换参数

$$\begin{gathered} T_x=a_{13} \\ {T}_y=a_{23} \\ {S}_x=\sqrt{(}{a}_{11}^2+a_{21}^2) \\ \varphi =arctan(\frac{a_{21}}{a_{11}}) \\ A=\frac{a_{22}tan\varphi +a_{12}}{-(sin\varphi tan\varphi +cos\varphi )} \\ B=\frac{a_{22}-a_{12}tan\varphi }{cos\varphi +sin\varphi tan\varphi } \\ {S}_y=\sqrt{(}{A}^2+B^2) \\ \theta =arctan\frac{A}{B} \end{gathered}$$

设备标定——Halcon

• halcon中使用就是两个函数解决问题
  • 获取仿射变换矩阵：vector_to_hom_mat2d
  • 解析放射变换矩阵：hom_mat2d_to_affine_par
  • l_x1, l_y1为机械坐标，l_x2, l_y2为像素坐标
  • HomMat2D (输入参数):仿射变换矩阵
  • Sx：x方向的缩放因子
  • Sy：y方向的缩放因子
  • Phi ：旋转角度
  • Theta：斜切角度
  • Tx ：沿x方向平移的距离
  • Ty ：沿y方向平移的距离

vector_to_hom_mat2d(l_x1, l_y1, l_x2, l_y2, HomMat2D)
hom_mat2d_to_affine_par(HomMat2D, Sx, Sy, Phi, Theta, Tx, Ty)

• 分解仿射变换，缩放、轴切、旋转、平移

  hom_mat2d_identity (HomMat2DIdentity)
  hom_mat2d_scale (HomMat2DIdentity, Sx, Sy, 0, 0, HomMat2DScale) * 缩放
  hom_mat2d_slant (HomMat2DScale, Theta, 'y', 0, 0, HomMat2DSlant) * 轴切
  hom_mat2d_rotate (HomMat2DSlant, Phi, 0, 0, HomMat2DRotate) * 旋转
  hom_mat2d_translate (HomMat2DRotate, Tx, Ty, HomMat2D) * 平移

设备标定——OpenCV

• 最小二乘求得放射变换矩阵

def affine(l_x1, l_y1, l_x2, l_y2):
    def cal_dist(A, B):
        return (A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2

    def trans(para, coor):
        X = (para[0] * coor[0] + para[1] * coor[1] + para[4])
        Y = (para[2] * coor[0] + para[3] * coor[1] + para[5])
        return [X, Y]
    if len(l_x1) != len(l_y1) or len(l_y1) != len(l_x2) or len(l_x2) != len(l_y2):
        return None
    num_points = len(l_x1)
    Bval, Lval = [], []
    for i in range(num_points):
        Bval.append([l_x1[i], l_y1[i], 0, 0, 1, 0])
        Bval.append([0, 0, l_x1[i], l_y1[i], 0, 1])
        Lval.extend([[l_x2[i]], [l_y2[i]]])
    Bmat = np.matrix(Bval, dtype=np.float32) 
    Lmat = np.matrix(Lval, dtype=np.float32)
    gH = (Bmat.T * Bmat).I * (Bmat.T * Lmat)
    Hmat = gH.reshape([2, 3])
    # a11 a12 a21 a22 b1 b2
    affine_para = np.reshape(Hmat.tolist(), (1,6))[0]
    sigma = 0
    for i in range(num_points):
        sigma += cal_dist(trans(affine_para, [l_x1[i], l_y1[i]]), [l_x2[i], l_y2[i]])
    sigma = np.sqrt(sigma / num_points)
    return affine_para, sigma

• 分解仿射变换矩阵，得到转换参数

def affine_to_para(para):
    # a11, a12, a21, a22, b1, b2
    a11, a12, a21, a22, b1, b2 = para
    Tx, Ty = b1, b2
    Sx = np.sqrt(a11**2 + a21**2)
    Phi = np.arctan(a21 / a11) # 注意存在360转换
    sin_phi = a21 / Sx
    cos_phi = a11 / Sx
    Bval = np.array([[-cos_phi, -sin_phi], [-sin_phi, cos_phi]])
    Lval = np.array([[a12], [a22]])
    Bmat = np.matrix(Bval, dtype=np.float32)
    Lmat = np.matrix(Lval, dtype=np.float32)
    gH = (Bmat.T * Bmat).I * (Bmat.T * Lmat)
    Sy = np.sqrt(gH.tolist()[0][0] **2 + gH.tolist()[1][0] ** 2)
    Theta = np.arctan(gH.tolist()[0][0] / gH.tolist()[1][0] )
    return Sx, Sy, Phi, Theta, Tx, Ty