python - 用3D和2D点对应关系计算旋转和平移矩阵

Question

我从不同的位置有一组3D点和2D的对应点。
2D点在360°全景上。所以我可以将它们转换为Polar->(r，theta，phi)而没有关于r的信息。
但是r只是变换后的3D点的距离:
[R | t] * xyz = xyz'
r = sqrt(xyz')
然后，在3D点也处于球坐标的情况下，我现在可以使用此线性方程组搜索R和t:
x'= sin(θ)* cos(phi)* r
y'= sin(θ)* cos(phi)* r
z'= sin(θ)* 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))