基于控制点的相机内/外参数标定仿真

背景

基于标定板的相机内参标定方法只能在消除畸变方面有较大作用，对于fx、fy、u0、v0的解算十分不稳定。因此，考虑采用基于多组控制点三维坐标的标定方法对相机内参数进行标定。分别通过仿真和实测实验对编写的算法进行了验证，结果证明本方法有效，下面进行详细叙述。

坐标系

• 图像坐标系-$m$；
• 相机坐标系-$c$；
• 全站仪坐标系-$t$；

测量模型

基于视觉测量小孔成像的基础共线模型（参考《视觉测量原理与方法》）：图像点$P_c^{m}i$、相机光心$O_c$、物方测量点$P_c^{t}i$共线。数学描述如下：

$O_c-P_c^{m}i=\lambda (P_c^{t}i-O_c)$

上述公式均在相机坐标系$c$下表达，变量展开后，可得：

$O_c=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$ ，$P_c^{m}i=\left[\begin{array}{c}{X}_c^{m}i\\ Y_c^{m}i\\ Z_c^{m}i\end{array}\right]$ ，

其中，$Z_c^{m}i=-f$，$f$代表相机焦距，

$X_c^m=(u-u0)\ast cx$，$Y_c^m=(v-v0)\ast cy$，因此：

$P_c^{m}i=\left[\begin{array}{c}(u-u0)\ast cx\\ (v-v0)\ast cy\\ -f\end{array}\right]$

其中，$cx、cy$为像素大小。由于$P_c^{t}i$为在相机坐标系下的特征点坐标，在标定时只能通过外部测量装置（如全站仪等）测得其在自身坐标系下坐标，因此可写成：

$P_c^{t}i=R_t^c\ast P_t^{t}i+T_t^c$，

其中$P_t^{t}i$为全站仪坐标系下特征点的三维坐标，$R_t^c、T_t^c$为全站仪到相机坐标系下的旋转和平移矩阵，可以写成：

$\left[\begin{array}{cc}{R}_t^c& T_t^c\end{array}\right]=\left[\begin{array}{cccc}r11& r12& r13& tx\\ r21& r22& r23& ty\\ r31& r32& r33& tz\end{array}\right]$

带入可得：

$\left[\begin{array}{c}{X}_c^{t}i\\ Y_c^{t}i\\ Z_c^{t}i\end{array}\right]=\left[\begin{array}{cccc}r11& r12& r13& tx\\ r21& r22& r23& ty\\ r31& r32& r33& tz\end{array}\right]\ast \left[\begin{array}{c}{X}_t^{t}i\\ Y_t^{t}i\\ Z_t^{t}i\\ 1\end{array}\right]$，展开可得：

$\left[\begin{array}{c}{X}_c^{t}i\\ Y_c^{t}i\\ Z_c^{t}i\end{array}\right]=\left[\begin{array}{c}r11\ast X_t^{t}i+r12\ast Y_t^{t}i+r13\ast Z_t^{t}i+tx\\ r21\ast X_t^{t}i+r22\ast Y_t^{t}i+r23\ast Z_t^{t}i+ty\\ r31\ast X_t^{t}i+r32\ast Y_t^{t}i+r33\ast Z_t^{t}i+tz\end{array}\right]$，带入原始共线方程可得：

$\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]-\left[\begin{array}{c}(u-u0)\ast cx\\ (v-v0)\ast cy\\ -f\end{array}\right]=\lambda \left[\left[\begin{array}{c}r11\ast X_t^{t}i+r12\ast Y_t^{t}i+r13\ast Z_t^{t}i+tx\\ r21\ast X_t^{t}i+r22\ast Y_t^{t}i+r23\ast Z_t^{t}i+ty\\ r31\ast X_t^{t}i+r32\ast Y_t^{t}i+r33\ast Z_t^{t}i+tz\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\right]$
，展开可得到三组表达式：

①：$-(u-u0)\ast cx=\lambda (r11X_t^{t}i+r12Y_t^{t}i+r13\ast Z_t^{t}i+tx)$,

②：$-(v-v0)\ast cy=\lambda (r21X_t^{t}i+r22Y_t^{t}i+r23\ast Z_t^{t}i+ty)$,

③：$f=\lambda (r31X_t^{t}i+r32Y_t^{t}i+r33\ast Z_t^{t}i+tz)$,

①/③，②/③可得如下共线方程的精确表达式：

$-(u-u0)\ast cx/f-\frac{\lambda (r11X_t^{t}i+r12Y_t^{t}i+r13\ast Z_t^{t}i+tx)}{\lambda (r31X_t^{t}i+r32Y_t^{t}i+r33\ast Z_t^{t}i+tz)}=0$,

$-(v-v0)\ast cy/f-\frac{\lambda (r21X_t^{t}i+r22Y_t^{t}i+r23\ast Z_t^{t}i+ty)}{\lambda (r31X_t^{t}i+r32Y_t^{t}i+r33\ast Z_t^{t}i+tz)}=0$,

其中，$cx/f$、$cy/f$可写成$1/fx$、$1/fy$，则共线方程可以写为：

$-(u-u0)/fx-\frac{\lambda (r11X_t^{t}i+r12Y_t^{t}i+r13\ast Z_t^{t}i+tx)}{\lambda (r31X_t^{t}i+r32Y_t^{t}i+r33\ast Z_t^{t}i+tz)}=0$,

$-(v-v0)/fy-\frac{\lambda (r21X_t^{t}i+r22Y_t^{t}i+r23\ast Z_t^{t}i+ty)}{\lambda (r31X_t^{t}i+r32Y_t^{t}i+r33\ast Z_t^{t}i+tz)}=0$,

因此，假设相机镜头不存在畸变，标定内参数的过程即为求解$u0、v0、fx、fy$的过程。另外，$R_t^c、T_t^c$亦未知，其内部包含12个未知数，因此相机标定过程需要完成16个未知数的求解。综合考虑共线方程具有明显的非线性，采用基于最小二乘平差的求解方法，通过求解非线性方程组和旋转平移矩阵，完成内参数标定。由于旋转矩阵自身可列6个约束方程，欲求解18个未知数，还需要另外列12个方程。由于每个特征点对应2个共线方程，因此完成内参数标定所需要的最小空间特征点数为6个，当然此数量越多且空间分布越均匀广阔，求解精度就越高。

建模仿真

利用solidworks软件对此问题进行建模，整体模型截图如下：

任意选取空间48个位置作为全站仪空间三维坐标测量所得特征点，每16个点为一组，共处1个平面，组件间隔1m，以此为仿真条件。相机和图像坐标系遵循视觉测量理论中的小孔成像模型，如下图所示：

放大后且调整角度，显示相机坐标系和图像坐标系显示如下图所示：

利用SW中的测量功能，逐点量取像点和物点分别在图像坐标系和全站仪坐标系下的坐标，整理到表格中，如下表所示：



编写PYTHON程序，利用scipy包中的最小二乘求解函数，对相机内参和旋转平移矩阵进行联合求解，主程序代码如下：

'''
The internal parameters of the camera are calculated by the control point
by QQ 770896174
20200820
'''
import algorithm as al
import numpy as np
import pandas as pd
'''-----------------------------------The virtual point in sw-----------------------------------'''
df_news = pd.read_table('controlpts-sw.txt', header = None)
ulist = df_news.iloc[:, 0]
vlist = df_news.iloc[:, 1]
Xlist = df_news.iloc[:, 2]
Ylist = df_news.iloc[:, 3]
Zlist = df_news.iloc[:, 4]
print('ulist:', ulist)
# initial values
# from camera frame to total station frame
pto = np.array([3741.559378, -223.903379, 100])
ptx = np.array([3812.270056, -294.614057, 131.666437])
pty = np.array([3762.906204, -245.250205, 4.665714])
R1, Tbuf = al.calRTfrom3Pts(pto, ptx, pty)
R0 = np.linalg.inv(R1)
Tbuf = np.matrix([3741.559378, -223.903379, 100])+100#Direct measurement, The approximate origin of coordinate of camera, add error
T0 = -R0 * Tbuf.T
x0 = [R0[0,0], R0[0,1], R0[0,2], R0[1,0], R0[1,1], R0[1,2], R0[2,0], R0[2,1], R0[2,2], T0[0,0], T0[1,0], T0[2,0], 12.5/0.0053, 12.5/0.0053, 9.348871/0.0053, 8.744599/0.0053]
x = al.optimize_params(ulist, vlist, Xlist, Ylist, Zlist, x0)
print('求解结果', x)
print('初值X0:', x0)
R = np.matrix([[x[0], x[1], x[2]],
[x[3], x[4], x[5]],
[x[6], x[7], x[8]]])
T = np.matrix([x[9], x[10], x[11]]).T
Rct = np.linalg.inv(R)
Tct = -Rct * T
print('相机坐标系到全站仪的R、T', Rct, Tct)

程序中所用到的两个核心函数optimize_params()和calRTfrom3Pts()代码为：

def calRTfrom3Pts(pto, ptx, pty):
    '''
    * function * ：calculate the R T from A to B frames;
    * params * ：pto, ptx, pty2 - 3D feature point coordinates of A in B frame, pto is the origin of coordinate frame; 
    * author * ：Hz 2020-8-6
    '''
    #print(np.array(pto))
    lx = (ptx - pto) / np.linalg.norm((ptx - pto))
    ly = (pty - pto) / np.linalg.norm((pty - pto))
    lz = np.cross(lx, ly)
    R = np.matrix([lx, ly, lz]).transpose()
    T = np.matrix(pto).transpose()
    return R, T

def optimize_params(ulist, vlist, X, Y, Z, x0):
    '''
    * function * ：Calibrate the camera's internal and external parameters through feature points;
    * params * ：ulist, vlist, Xlist, Ylist, Zlist;
        X: X component of feature point;
        Y: Y component of feature point;
        Z: Z component of feature point;
        ulist:The u component of the characteristic point in the image coordinate system;
        vlist:The v component of the characteristic point in the image coordinate system; 
        x0: The initial value
    * author * ：Hz 2020-8-20
        output: R,T, u0, v0, fx, fy
    '''
    n = len(ulist)
    def func(x):
        fline = [] # Collinear constraint equation
        r11, r12, r13, r21, r22, r23, r31, r32, r33, tx, ty, tz, fx, fy, u0, v0 = x.tolist()
        #xpc = - (ulist - u0) * cx
        #ypc = - (vlist - v0) * cy
        for i in range(n):
            # Collinear constraint
            fline.append(-(-(ulist[i] - u0) /fx) - (r11*X[i] + r12*Y[i] + r13*Z[i] + tx)/(r31*X[i] + r32*Y[i] + r33*Z[i] + tz))
            fline.append(-(-(vlist[i] - v0) /fy) - (r21*X[i] + r22*Y[i] + r23*Z[i] + ty)/(r31*X[i] + r32*Y[i] + r33*Z[i] + tz))
        return np.asarray([
            #real
                           fline[0],fline[1],fline[2],fline[3],fline[4],fline[5],fline[6],fline[7],fline[8],fline[9],fline[10],fline[11],fline[12],
                           fline[13],fline[14],fline[15],fline[16],fline[17],fline[18],fline[19],fline[20],fline[21],fline[22],
                           fline[23],fline[24],fline[25],fline[26],fline[27],fline[28],fline[29],

                           fline[30],fline[31],fline[32],fline[33],fline[34],fline[35],fline[36],fline[37],fline[38],fline[39],
                           fline[40],fline[41],fline[42],fline[43],fline[44],fline[45],fline[46],fline[47],fline[48],fline[49],
                           fline[50],fline[51],fline[52],fline[53],fline[54],fline[55],fline[56],fline[57],fline[58],fline[59],
                           #sw
                           fline[60],fline[61],fline[62],fline[63],fline[64],fline[65],fline[66],fline[67],fline[68],fline[69],
                           fline[70],fline[71],fline[72],fline[73],fline[74],fline[75],fline[76],fline[77],fline[78],fline[79],
                           fline[80],fline[81],fline[82],fline[83],fline[84],fline[85],fline[86],fline[87],fline[88],fline[89],
                           fline[90],fline[91],fline[92],fline[93],fline[94],fline[95],
                           
                          #正交约束
                            r11 ** 2 + r21 ** 2 + r31 ** 2 - 1,
                            r12 ** 2 + r22 ** 2 + r32 ** 2 - 1,
                            r13 ** 2 + r23 ** 2 + r33 ** 2 - 1,
                            r11 * r12 + r21 * r22 + r31 * r32,
                            r11 * r13 + r21 * r23 + r31 * r33,
                            r12 * r13 + r22 * r23 + r32 * r33])

    root = least_squares(func, x0, method='lm')
    print('result:', root)
    return root.x

运行结果如下所示，16个参数成功求解，结果与数模一致。

有问题欢迎留言讨论，感谢关注！