python - Opencv coordinate transformation: Rotate a rotated coordinate system by one of its new (rotated) axes - Stack Overflow

admin管理员组

文章数量:1122846

For my thesis I am doing a computer vision project. Part of this is a lot of coordinate transformations which i struggle with at the moment. I just cannot rotate my coordinate system by an arbitrary axis. I start with 3D coordinates of an object. Given that I configured the camera and know the camera matrix and distort coefficients I can project that point back to a 2D image.

Now I want to obtain the euler angles of a coordinate system so that the Z-Axis of the coordinate system points directly towards the optical center (the camera).

For completeness sake here are the helper functions from my code. For my main problem please skip towards the next Textblock.

def coordinate_system_to_image(image, custom_rotation, custom_translation):
    origin_3d = np.array([(0.0, 0.0, 0.0)])
    start_point_coord, jacobian = cv2.projectPoints(origin_3d,custom_rotation,custom_translation,mtx,dist,)

    point0 = (int(start_point_coord[0][0][0]), int(start_point_coord[0][0][1]))
    pointx_3d = np.add(origin_3d, np.array([(2.5, 0.0, 0.0)]))
    end_point_x, jacobian = cv2.projectPoints(pointx_3d,custom_rotation,custom_translation,mtx,dist,)
    pointx = (int(end_point_x[0][0][0]), int(end_point_x[0][0][1]))

    pointy_3d = np.add(origin_3d, np.array([(0.0, 2.5, 0.0)]))
    end_point_y, jacobian = cv2.projectPoints(pointy_3d,custom_rotation,custom_translation,mtx,dist,)
    pointy = (int(end_point_y[0][0][0]), int(end_point_y[0][0][1]))

    pointz_3d = np.add(origin_3d, np.array([(0, 0.0, 2.5)]))
    end_point_z, jacobian = cv2.projectPoints(pointz_3d,custom_rotation,custom_translation,mtx,dist,)
    pointz = (int(end_point_z[0][0][0]), int(end_point_z[0][0][1]))

    cv2.line(image, point0, pointx, (255, 0, 0), 5)
    cv2.line(image, point0, pointy, (0, 255, 0), 5)
    cv2.line(image, point0, pointz, (0, 0, 255), 5)

    cv2.circle(image, (int(start_point_coord[0][0][0]), int(start_point_coord[0][0][1])), 10, (0, 0, 255), -1)

    return image

def coords_rotation_matrix_from_axis_and_angle(axis,angle):
    # 
    ux = axis[0]
    uy = axis[1]
    uz = axis[2]

    length = ux**2 + uy**2 + uz**2
    if(length != 1.0):
        print("Length of axis != 1")
        return False

    Mx11 = ux*ux*(1-np.cos(angle)) + np.cos(angle)
    Mx12 = ux*uy*(1-np.cos(angle)) - uz*np.sin(angle)
    Mx13 = ux*uz*(1-np.cos(angle)) + uy*np.sin(angle)
    Mx21 = ux*uy*(1-np.cos(angle)) + uz*np.sin(angle)
    Mx22 = uy*uy*(1-np.cos(angle)) + np.cos(angle)
    Mx23 = uy*uz*(1-np.cos(angle)) - ux*np.sin(angle)
    Mx31 = ux*uz*(1-np.cos(angle)) - uy*np.sin(angle)
    Mx32 = uy*uz*(1-np.cos(angle)) + ux*np.sin(angle)
    Mx33 = uz*uz*(1-np.cos(angle)) + np.cos(angle)
    Mx = np.array([[Mx11, Mx12, Mx13], [Mx21, Mx22, Mx23], [Mx31, Mx32, Mx33]])
    return Mx

def coords_rotation_matrix_to_euler(rotation_matrix):
    r11 = rotation_matrix[0][0]
    r21 = rotation_matrix[1][0]
    r31 = rotation_matrix[2][0]
    r32 = rotation_matrix[2][1]
    r33 = rotation_matrix[2][2]

    degy_reverse1 = -1*np.arcsin(r31)
    degy_reverse2 = np.pi - degy_reverse1

    degx_reverse1 = np.arctan2(r32/np.cos(degy_reverse1), r33/np.cos(degy_reverse1))
    degx_reverse2 = np.arctan2(r32/np.cos(degy_reverse2), r33/np.cos(degy_reverse2))

    degz_reverse1 = np.arctan2(r21/np.cos(degy_reverse1), r11/np.cos(degy_reverse1))
    degz_reverse2 = np.arctan2(r21/np.cos(degy_reverse2), r11/np.cos(degy_reverse2))
    print(f"Extracted Eulers 2: X:({degx_reverse1},{degx_reverse2}) Y:({degy_reverse1},{degy_reverse2}) Z:({degz_reverse1},{degz_reverse2})")
    return degx_reverse1, degy_reverse1, degz_reverse1

The first part of the system works well. I can set 3D coordinates, determine the x- and y-angles and print an according coordinate system that has its z-axis towards the camera as expected (z-axis "disappears" in the plot).

demo = np.zeros((2988, 5312, 3))
demo = np.uint8(demo)

pos3dx_wchess = 5.0
pos3dy_wchess = -3
pos3dz_wchess = 30
degx_camera3d = np.arctan(pos3dy_wchess/pos3dz_wchess)
degy_camera3d = -1*np.arctan(pos3dx_wchess/pos3dz_wchess)
degz_camera3d = 0

connector_pointing_to_camera_orientation = np.array([[-1*degx_camera3d, -1*degy_camera3d, -1*degz_camera3d]])
print(connector_pointing_to_camera_orientation)
custom_translation = np.array([[pos3dx_wchess, pos3dy_wchess, pos3dz_wchess]])

demo = coordinate_system_to_image(demo, custom_rotation=connector_pointing_to_camera_orientation, custom_translation=custom_translation)

M_rot = coords_euler_to_rotation_matrix(connector_pointing_to_camera_orientation[0][0],connector_pointing_to_camera_orientation[0][1],connector_pointing_to_camera_orientation[0][2])
print("Test retreive angles:",coords_rotation_matrix_to_euler(M_rot))

[[0.09966865 0.16514868 0. ]] Test retreive angles: (0.09966865249116204, 0.1651486774146268, 0.0)

The test print also shows, that the angle conversion from the rotation matrix works.

Now I want to rotate that coordinate system around its new z-axis. This z-axis of course does no longer align with the camera z-axis because it was rotated by the previous rotation.

So I first obtain the new rotation axis, then create the according rotation matrix for a rotation of 45 degrees, combine the two rotation matrices, get the euler angles from the combined rotation matrix and plot the new rotated coordinate system.

world_point_z_axis = np.array([0,0,1])
second_axis = np.matmul(M_rot, world_point_z_axis)  # equals M_rot @ world_point_z_axis

second_matrix = coords_rotation_matrix_from_axis_and_angle(second_axis,45*np.pi/180)

combined = second_matrix @ M_rot #np.matmul(second_matrix, M_rot)

new_eulerx, new_eulery, new_eulerz = coords_rotation_matrix_to_euler(combined)
new_rotation = np.array([new_eulerx, new_eulery, new_eulerz])

print(img_shape)

demo2 = coordinate_system_to_image(demo2, custom_rotation=new_rotation, custom_translation=custom_translation)

This does not work for some reason. In the new plot the X and Y axes look somewhat alright but it seems that the rotation was not executed around the rotated z-axis from the first rotation because now this axis is visible again. It seems more like the rotation was done around the cameras z-axis rather than the rotated one.

Is there a mistake in my pipeline? Why is the second rotation not executed around the seperately produced axis?

Best regards, Felix

本文标签：