Я создаю университетский проект с маркерами OpenCV Python и ArUco, где я хотел бы получить (относительно) надежную оценку позы для движения камеры. Я планирую использовать это для построения графиков полета дронов. Для этого мне нужно преобразовать позу камеры в мировые координаты, определенные первым видимым маркером.
Я знаю, что между маркерами должна быть матрица преобразования, но я не могу понять, что этоявляется. Я пытаюсь с разницей соответствующих rvecs.
Код для функции в 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
dtvec = tvec_m - tvec_n # vector from 'm' to 'n' marker in the camera's coordinate system
# get the markers' rotation matrices respectively
R_m = cv2.Rodrigues(rvec_m)[0]
R_n = cv2.Rodrigues(rvec_n)[0]
tvec_mm = np.matmul(-R_m.T, tvec_m) # camera pose in 'm' marker's coordinate system
tvec_nn = np.matmul(-R_n.T, tvec_n) # camera pose in 'n' marker's coordinate system
# 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
drvec_m = np.transpose(np.matmul(R_m.T, np.transpose(drvec))) # transformed to 'm' marker
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
Это лучшее, что я мог получить, но все еще есть значение ошибки +-0,03 метра между значениями перевода tvec_mm
и tvec_nm
.
Можно ли с этим что-то улучшить? Это даже законное преобразование или просто огромное совпадение, что оно дает примерно одинаковые значения? Есть идеи?
Спасибо!