今天遇到一个问题,需要标定一下设备的安装精度。
设备安装精度标定
-
设备在安装过程中,有时候需要标定一下安装精度,如何判断摄像头是否水平旋转,是否倾斜等
-
设备安装好后,拍摄已知坐标的标定物,选取标志点,构建相对世界坐标系,得到机械坐标
-
摄像头拍摄相对点,得到像素坐标
-
机械坐标 ( x 1 , y 1 ) (x_1,y_1) (x1,y1)到像素坐标 ( x 2 , y 2 ) (x_2,y_2) (x2,y2)仿射变换如下
{ x 2 = a 11 x 1 + a 12 y 1 + a 13 y 2 = a 21 x 1 + a 22 y 1 + a 23 \begin{cases} 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{cases} {x2=a11x1+a12y1+a13y2=a21x1+a22y1+a23
- 最小二乘计算仿射变换关系
[ x 1 y 1 1 0 0 0 0 0 0 x 1 y 1 0 ] [ a 11 a 12 a 13 a 21 a 22 a 23 ] = [ x 2 y 2 ] \begin{bmatrix} x_1 & y_1 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & x_1 & y_1 & 0 \end{bmatrix}\begin{bmatrix} a_{11} \\ a_{12} \\ a_{13} \\ a_{21} \\ a_{22} \\ a_{23} \end{bmatrix} =\begin{bmatrix} x_2 \\ y_2 \end{bmatrix} [x10y10100x10y100]⎣⎢⎢⎢⎢⎢⎢⎡a11a12a13a21a22a23⎦⎥⎥⎥⎥⎥⎥⎤=[x2y2]
- 仿射变换矩阵分解
[ a 11 a 12 a 13 a 21 a 22 a 23 0 0 1 ] = [ 1 0 T x 0 1 T y 0 0 1 ] . [ c o s ( ϕ ) − s i n ( ϕ ) 0 s i n ( ϕ ) c o s ( ϕ ) 0 0 0 1 ] . [ 1 − s i n ( θ ) 0 0 c o s ( θ ) 0 0 0 1 ] . [ S x 0 0 0 S y 0 0 0 1 ] \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ 0 & 0 & 1 \end{bmatrix}=\begin{bmatrix} 1 & 0 & T_x \\ 0 & 1 & T_y \\ 0 & 0 & 1 \end{bmatrix} . \begin{bmatrix} cos(\phi) & -sin(\phi) & 0 \\ sin(\phi) & cos(\phi) & 0 \\ 0 & 0 & 1 \end{bmatrix} . \begin{bmatrix} 1 & -sin(\theta) & 0 \\ 0 & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix} . \begin{bmatrix} S_x & 0 & 0 \\ 0 & S_y & 0 \\ 0 & 0 & 1 \end{bmatrix} ⎣⎡a11a210a12a220a13a231⎦⎤=⎣⎡100010TxTy1⎦⎤.⎣⎡cos(ϕ)sin(ϕ)0−sin(ϕ)cos(ϕ)0001⎦⎤.⎣⎡100−sin(θ)cos(θ)0001⎦⎤.⎣⎡Sx000Sy0001⎦⎤
- 解方程,求得转换参数
T x = a 13 T y = a 23 S x = ( a 11 2 + a 21 2 ) ϕ = a r c t a n ( a 21 a 11 ) A = a 22 t a n ϕ + a 12 − ( s i n ϕ t a n ϕ + c o s ϕ ) B = a 22 − a 12 t a n ϕ c o s ϕ + s i n ϕ t a n ϕ S y = ( A 2 + B 2 ) θ = a r c t a n A B T_x = a_{13}\\ T_y = a_{23}\\ S_x = \sqrt(a_{11}^2 + a_{21}^2)\\ \phi=arctan(\frac{a_{21}}{a_{11}})\\ A = \frac{a_{22}tan\phi + a_{12}}{-(sin\phi tan\phi + cos\phi)} \\ B = \frac{a_{22}-a_{12}tan\phi}{cos\phi + sin\phi tan\phi} \\ S_y = \sqrt(A^2 + B^2) \\ \theta = arctan\frac{A}{B} Tx=a13Ty=a23Sx=(a112+a212)ϕ=arctan(a11a21)A=−(sinϕtanϕ+cosϕ)a22tanϕ+a12B=cosϕ+sinϕtanϕa22−a12tanϕSy=(A2+B2)θ=arctanBA
设备标定——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