Transform camera positon from one ArUco marker to another
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!