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

Question

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

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

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) В соответствии с этим положение камеры должно быть рассчитано так:

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

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

Answer 1

Я думаю, что у меня есть это сейчас. Проблема заключалась в методе, описанном в шаге 4. Положение камеры не может быть рассчитано только на основе матрицы гомографии. Матрица внутренней камеры также необходима. Итак, правильная процедура следующая:

1) нарисуйте карту местности

2) откалибруйте камеру, используя изображение шахматной доски с cv2.findChessboardCorners это дает матрицу камеры и коэффициенты искажения

3) решить PnP с мировыми координатами (3D) и координатами изображения (2D). SolvePnP возвращает ориг. Объекта в системе координат камеры, учитывая 4 соответствующие точки и матрицу камеры.

4) Теперь мне нужно рассчитать положение камеры в мировых координатах. Матрица вращения: rotM = cv2.Rodrigues(rvec)[0]

5) Положение x,y,z камеры: cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)