Как трансформировать позу камеры между маркерами ArUco? - PullRequest
Новая
       57

Как трансформировать позу камеры между маркерами ArUco?

0 голосов
/ 12 октября 2019

Я создаю университетский проект с маркерами 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.

Можно ли с этим что-то улучшить? Это даже законное преобразование или просто огромное совпадение, что оно дает примерно одинаковые значения? Есть идеи?

Спасибо!

...