pythonopencvoptimizationleast-squaresspherical-coordinate

computing rotation and translation matrix with 3D and 2D Point correspondences


I have a set of 3D points and the correspondend point in 2D from a diffrent position.
The 2D points are on a 360° panorama. So i can convert them to polar -> (r,theta , phi ) with no information about r.

But r is just the distance of the transformed 3D Point:

[R|t]*xyz = xyz'
r = sqrt(xyz')

Then with the 3D point also in spherical coordinates, i can now search for R and t with this linear equation system:

x' = sin(theta) * cos(phi) * r
y' = sin(theta) * cos(phi) * r
z' = sin(theta) * cos(phi) * r

I get good results for tests with t=[0,0,0.5] and without any rotation. But if there is a rotation the results are bad.

Is this the correct approach for my problem?

How can I use solvepnp() without a camera Matrix (it is a panorama without distortion)?

I am using opt.least_squares to calculate R and t.


Solution

  • I solved it with two diffrent methods.

    One is for small rotations and solves for R and t (12 parameter), the other method can compute even big rotations with Euler and t (6 parameter).

    I am calling the opt.least_squares() two times with diffrent initial values and use the method with an better reprojection error.

    The f.eul2rot is just a conversion between euler angles and the rotation matrix.

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