基于数据集拟合求解非线性方程组的相机位姿标定问题求助

我正在求解以下方程组：

0 = A P + T - X

其中X、P、T为3D向量，A为3x3方向余弦矩阵（DCM）。我拥有X及其对应P的数据集（代码中定义P = [x_p, y_p, 1]，T = [x_t, y_t, z_t]，X = [x, y, z]），需要找到旋转矩阵对应的欧拉角（psi、theta、phi）与T的最优值，以此拟合数据点。

该需求源于相机位姿标定场景：已知标定板特征的世界坐标系坐标X与图像坐标系坐标P，采用针孔相机模型求解相机的位置与姿态。

我已尝试使用SciPy的最小二乘回归方法求解，但未得到可行结果，不确定是否正确传递数据，也不确定是否找到能最小化所有数据点残差的变量值。

求解代码 #

import numpy as np
from scipy.optimize import least_squares

def calibrate(coords, img_path, pixel_size = 3.5/1280):

    p_x = coords[:,0]; p_y = coords[:,1]
    x   = coords[:,2]; y   = coords[:,3]; z = coords[:,4]
   
    img_shape =  ( 1024, 1280)
            
    
    def equations(vars):
        x_t, y_t, z_t, phi, theta, psi, f = vars

        x_p = ( p_x - img_shape[1]/2)*pixel_size/f
        y_p = (-p_y + img_shape[0]/2)*pixel_size/f

        zc = np.sqrt((x_t - x)**2 + (y_t - y)**2 + (z_t - z)**2)

        f1 = (( m.cos(psi) * m.cos(theta)) * x_p + 
              (-m.sin(psi) * m.cos(phi) + m.cos(psi) * m.sin(theta) * m.cos(phi)) * y_p + 
              ( m.sin(psi) * m.sin(phi) + m.cos(psi) * m.sin(theta) * m.cos(phi))) * zc + x_t - x
        
        f2 = (( m.sin(psi) * m.sin(theta)) * x_p + 
              ( m.cos(psi) * m.cos(phi) + m.sin(psi) * m.sin(theta) * m.sin(phi)) * y_p + 
              (-m.cos(psi) * m.sin(phi) + m.sin(psi) * m.sin(theta) * m.cos(phi))) * zc + y_t - y
        
        f3 = ((-m.sin(theta)) * x_p + 
              ( m.cos(theta) * m.sin(phi)) * y_p + 
              ( m.cos(theta) * m.cos(phi))) * zc + z_t - z

        return np.hstack([f1, f2, f3]) 
    
    result = least_squares(
                            fun = equations, x0  = (0, 0, 0, 0, 0, 0, 5),
                            max_nfev= 100, loss='soft_l1', 
                            bounds= ([-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, 2.5], [np.inf,              
np.inf, np.inf, np.inf, np.inf, np.inf, 30])
                        )

    values = result.x
    values[3:-2] = (values[3:-2]*180/np.pi)%360

    print(result.message)
    print('x_t, y_t, z_t = ', values[0:3])
    print('phi, theta, psi = ', values[3:-1])
    print('f = ', values[-1])
    print('Nb of iterations = ', result.nfev)
    return values

使用数据集 #

coords = np.array([[ 234,  347,   30,   25,    8],[ 981,  336,  -40,   25,    8],[1011,  607,  -40,    0,   -8],[ 249,  619,   30,    0,   -8],[ 240,  455,   30,   15,    0],[ 943,  443,  -35,   15,    0],[ 688,  612,  -10,    0,   -8],[ 675,  448,  -10,   15,    0],[ 665,  340,  -10,   25,    8],[ 234,  347,   30,   25,    8],[ 981,  336,  -40,   25,    8],[1011,  607,  -40,    0,   -8],[ 249,  619,   30,    0,   -8],[ 240, 455,   30,   15,    0],[ 943,  443,  -35,   15,    0],[ 688,  612,  -10,    0,   -8],[ 675,  448, -10,   15,    0],[ 665,  340,  -10,   25,    8]])

内容的提问来源于stack exchange，提问作者Theo