Вычислить положение камеры в OpenCV Python

Я хочу рассчитать положение моей камеры в мировых координатах. Это должно быть довольно легко, но я не получаю ожидаемых результатов. По-моему, я читал все по этой теме, но мой код не работает. Вот что я делаю:

У меня есть камера, смотрящая на область.

1) Я нарисовал карту области.

2) Я вычислил гомографию, сопоставив 4 точки изображения с 4 точками на моей карте, используя cv2.getPerspectiveTransform

3) H-гомография преобразует каждую координату мира в координату камеры; это работает нормально

4) Чтобы вычислить матрицу камеры, я последовал за этим:

translation = np.zeros((3,1)) 
translation[:,0] = homography[:,2]

rotation = np.zeros((3,3))
rotation[:,0] = homography[:,0]
rotation[:,1] = homography[:,1]
rotation[:,2] = np.cross(homography[0:3,0],homography[0:3,1])

cameraMatrix = np.zeros((3,4))
cameraMatrix[:,0:3] = rotation
cameraMatrix[:,3] = homography[:,2]

cameraMatrix = cameraMatrix/cameraMatrix[2][3] #normalize the matrix

5) Согласно this, положение камеры должно быть рассчитано следующим образом:

x,y,z = np.dot(-np.transpose(rotation),translation)

Координаты, которые я получаю, абсолютно ошибочны. Проблема должна быть где-то на шаге 4 или 5, я думаю. Что случилось с моим методом?

Ответ 1