Transform camera positon from one ArUco marker to another

asked 2019-10-11 13:37:27 -0500

Szepy gravatar image

updated 2019-10-12 09:19:50 -0500

I'm creating a university project with OpenCV Python and ArUco markers, where I would like to get a (relatively) robust pose estimation for the movement of the camera. I plan on using this for indoor drone flight graphing. For this, I have to transform the camera pose to world coordinates defined by the first seen marker.

I know there must be a transformation matrix between the markers, but I can't seem to figure out, what it is. I am trying with the difference of respective rvecs.

The code for the function in Python:

def TransformBetweenMarkers(tvec_m, tvec_n, rvec_m, rvec_n):
    tvec_m = np.transpose(tvec_m) # tvec of 'm' marker
    tvec_n = np.transpose(tvec_n) # tvec of 'n' marker
    # vector from 'm' to 'n' marker in the camera's coordinate system
    dtvec = tvec_m - tvec_n

    # get the markers' rotation matrices respectively
    R_m = cv2.Rodrigues(rvec_m)[0]
    R_n = cv2.Rodrigues(rvec_n)[0]

    # camera pose in 'm' marker's coordinate system
    tvec_mm = np.matmul(-R_m.T, tvec_m)
    # camera pose in 'n' marker's coordinate system
    tvec_nn = np.matmul(-R_n.T, tvec_n)

    # translational difference between markers in 'm' marker's system,
    # basically the origin of 'n'
    dtvec_m = np.matmul(-R_m.T, dtvec)

    # this gets me the same as tvec_mm,
    # but this only works, if 'm' marker is seen
    # tvec_nm = dtvec_m + np.matmul(-R_m.T, tvec_n)

    # something with the rvec difference must give the transformation(???)
    drvec = rvec_m-rvec_n
    # transformed to 'm' marker
    drvec_m = np.transpose(np.matmul(R_m.T, np.transpose(drvec)))
    dR_m = cv2.Rodrigues(drvec_m)[0]

    # I want to transform tvec_nn with a single matrix,
    # so it would be interpreted in 'm' marker's system
    tvec_nm = dtvec_m + np.matmul(dR_m.T, tvec_nn)

    # objective: tvec_mm == tvec_nm

This is the best I could get, but there is still an error value of +-0.03 meters between the tvec_mm and tvec_nm translation values.

Is it possible to get any better with this? Is this even a legit transformation or just a huge coincidence, that it gives approximately the same values? Any ideas?

Thank you!

edit retag flag offensive close merge delete