Transformation Matrix From EstimatePoseSingleMarkers

907 views Asked by At

I try to get transformation matrix from my camera to the mid point of an aruco marker and I use cv2.aruco.estimatePoseSingleMarkers function.

In the description it says :

The returned transformation is the one that transforms points from each marker coordinate system to the camera coordinate system.

I give corners of the marker detection result to the pose estimation function and convert the rotation vector with cv2.Rodriguez to rotation matrix. Already the resulting translation vector is different from the measured translation from the camera coordinates to the marker coordinates which it should be the same in my knowledge.

Regardless of the inference, I initialize a 4x4 transformation matrix and put the rotation matrix and the translation vector from pose estimation to the 4x4 matrix which I assume it gives the transformation from the marker coordinate frame to the camera coordinate frame but when I correct the transformation with another point from the environment it shows the transformation matrix is not correct.

I am sure about the MARKER_SIZE.

    marker2camera_r_vec, marker2camera_t_vec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, MARKER_SIZE, camera.mtx, camera.dist)
    marker2camera_r_mat = np.array(cv2.Rodrigues(marker2camera_r_vec)[0])
    transformation_matrix = np.zeros([4, 4])
    transformation_matrix[0:3, 0:3] = rotation_matrix
    transformation_matrix[0:3, 3] = translation_vector
    transformation_matrix[3, 3] = 1

Example results:

marker2camera_r_mat =
[[-0.96533802 -0.03093691 -0.25916292], 
[ 0.07337548  0.92073721 -0.38322189], 
[ 0.25047664 -0.38895487 -0.88655263]]

marker2camera_t_vec = 
[ 1.45883855  6.98282269 77.73744481]

transformation_matrix =
[[-9.65338018e-01 -3.09369077e-02 -2.59162919e-01  1.45883855e+00], 
[ 7.33754824e-02  9.20737214e-01 -3.83221895e-01  6.98282269e+00], 
[ 2.50476645e-01 -3.88954869e-01 -8.86552627e-01  7.77374448e+01], 
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00]]

Actual point coordinates of the marker from the camera coordinates is =
[-0.086 0.12 0.83]

Can anyone tell me what I do wrong and explain the steps to obtain the transformation matrix from the camera coordinates to the marker coordinates via cv2.aruco.estimatePoseSingleMarkers ?

0

There are 0 answers