宝慕林4294392
我用两种不同的方法解决了它。一种用于小旋转并求解 R 和 t(12 个参数),另一种方法可以使用 Euler 和 t(6 个参数)计算更大的旋转。我opt.least_squares()用不同的初始值调用了两次,并使用了具有更好重投影误差的方法。f.eul2rot 只是欧拉角和旋转矩阵之间的转换。def sphere_eq(p): xyz_points = xyz uv_points = uv #r11,r12,r13,r21,r22,r23,r31,r32,r33,tx,ty,tz = p if len(p) == 12: r11, r12, r13, r21, r22, r23, r31, r32, r33, tx, ty, tz = p R = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]]) else: gamma, beta, alpha,tx,ty,tz = p E = [gamma, beta, alpha] R = f.eul2rot(E) pi = np.pi eq_grad = () for i in range(len(xyz_points)): # Point with Orgin: LASER in Cartesian and Spherical coordinates xyz_laser = np.array([xyz_points[i,0],xyz_points[i,1],xyz_points[i,2]]) # Transformation - ROTATION MATRIX and Translation Vector t = np.array([[tx, ty, tz]]) # Point with Orgin: CAMERA in Cartesian and Spherical coordinates uv_camera = np.array(uv_points[i]) long_camera = ((uv_camera[0]) / w) * 2 * pi lat_camera = ((uv_camera[1]) / h) * pi xyz_camera = (R.dot(xyz_laser) + t)[0] r = np.linalg.norm(xyz_laser + t) x_eq = (xyz_camera[0] - (np.sin(lat_camera) * np.cos(long_camera) * r),) y_eq = (xyz_camera[1] - (np.sin(lat_camera) * np.sin(long_camera) * r),) z_eq = (xyz_camera[2] - (np.cos(lat_camera) * r),) eq_grad = eq_grad + x_eq + y_eq + z_eq return eq_gradx = np.zeros(12)x[0], x[4], x[8] = 1, 1, 1initial_guess = [x,np.zeros(6)]for p, x0 in enumerate(initial_guess): x = opt.least_squares(sphere_eq, x0, '3-point', method='trf') if len(x0) == 6: E = np.resize(x.x[:4], 3) R = f.eul2rot(E) t = np.resize(x.x[4:], (3, 1)) else: R = np.resize(x.x[:8], (3, 3)) E = f.rot2eul(R) t = np.resize(x.x[9:], (3, 1))