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

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

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

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

3) solvePnP с координатами мира (3D) и координатами изображения (2D). РешениеPnP возвращает объект origo в систему координат камеры с учетом 4 соответствующих точек и матрицы камеры.

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

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