Convert Euler angle between camera & robot coordinate system

My problem is simple, but yet confusing as I personally have no experience in angles and angles conversion yet.

Basically, I need to locate the position of an object attached with single AruCo marker then send the 3d coordinate and pose of that object (the marker) to the robot. Note that the robot model I use is an industrial one manufactured by ABB, and the 3d coordinate I sent already been converted to Robot Coordinate System.

Put aside the problem of coordinate, I solved it using Stereo Cameras. However, I found the pose problem to be so difficult, especially when convert the pose of AruCo marker w.r.t camera to the robot coordinate system. The images below represent the two-coordinate system, one for camera and one for the robot.

Coordinate System

The angle I collected from AruCo Marker were converted to Euler Angles, the methods were applied from OpenCV library here:

def PoseCalculate(rvec, tvec, marker):
    rmat = cv2.Rodrigues(rvec)[0]
    P = np.concatenate((rmat, np.reshape(tvec, (rmat.shape[0], 1))), axis=1)
    euler = -cv2.decomposeProjectionMatrix(P)[6]
    eul = euler_angles_radians
    yaw = eul[1, 0] 
    pitch = eul[0, 0]
    roll = eul[2, 0]
    return (pitch, yaw, roll)

The result are three angles that represent pose of the marker. Pitch represents the rotation when the marker rotate around X axis (camera), Yaw for the Y axis (camera) and Roll for the Z axis (camera as well.)
So, how I can convert these three angles to the robot coordinate system?

Sample Angles Collected

Thanks for reading this long question and wish all of you be healthy in new year 2021!


rmat should be the 3x3 rotation part of the 4x4 pose matrix for your marker (relative to/in the camera coordinate frame). if you need that pose to be in a different frame, multiply the corresponding transformation matrices onto it.

is that what you’re asking? that’s the easiest way I know to represent poses. I don’t know how else, apart from specific angles of specific joints, your robot would want this to be represented as.