使用 3D 和 2D 点对应计算旋转和平移矩阵

Question

我有一组 3D 点和来自不同位置的 2D 对应点。2D 点位于 360° 全景图上。所以我可以将它们转换为极坐标 -> (r,theta, phi) 而没有关于 r 的信息。但是 r 只是转换后的 3D 点的距离：[R|t]*xyz = xyz'r = sqrt(xyz')然后 3D 点也在球坐标中，我现在可以用这个线性方程系统搜索 R 和 t：x' = sin(theta) * cos(phi) * ry' = sin(theta) * cos(phi) * rz' = sin(theta) * cos(phi) * r我在 t=[0,0,0.5] 且没有任何旋转的情况下得到了很好的测试结果。但如果有轮换，结果就很糟糕。这是解决我的问题的正确方法吗？如何在没有相机矩阵的情况下使用 solvepnp() （它是没有失真的全景图）？我正在使用 opt.least_squares 来计算 R 和 t。

Answer 1

我用两种不同的方法解决了它。

一种用于小旋转并求解 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_grad

x = np.zeros(12)

x[0], x[4], x[8] = 1, 1, 1

initial_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))