Положение камеры в мировой координате из cv :: solvePnP

Question

Положение камеры в мировой координате из cv :: solvePnP

У меня есть откалиброванная камера (внутренняя матрица и коэффициенты искажения), и я хочу знать положение камеры, зная некоторые 3d-точки и соответствующие им точки на изображении (2d-точки).

я знаю это cv::solvePnP мог бы помочь и после прочтения этот а также этот Я так понимаю, что мне выходы решить PnP rvec а также tvec это поворот и перемещение объекта в системе координат камеры.

Поэтому мне нужно выяснить поворот / перемещение камеры в мировой системе координат.

Из приведенных выше ссылок кажется, что код прост в Python:

found,rvec,tvec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_coefs)
rotM = cv2.Rodrigues(rvec)[0]
cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)

Я не знаю Python / Numpy вещи (я использую C ++), но это не имеет большого смысла для меня:

Вывод rvec, tvec из solvePnP — это матрица 3×1, 3 элемента вектора
cv2.Rodrigues (rvec) — это матрица 3×3
cv2.Rodrigues (rvec) [0] — матрица 3×1, 3 элемента вектора
cameraPosition — это матричное умножение 3×1 * 1×3, которое представляет собой матрицу .. 3×3. как я могу использовать это в OpenGL с простым glTranslatef а также glRotate звонки?

44

c++computer-vision opencv opengl pose-estimation

Решение

Другие решения

Если вы хотите превратить его в стандартную матрицу поз 4×4, указав положение вашей камеры. Используйте rotM в качестве верхнего левого квадрата 3×3, tvec в качестве 3-х элементов справа и 0,0,0,1 в качестве нижнего ряда

pose = [rotation   tvec(0)
matrix     tvec(1)
here       tvec(2)
0  , 0, 0,  1]

затем инвертируйте его (чтобы получить позу камеры вместо позы мира)

3

Источник

Accepted Answer

Если с «мировыми координатами» вы имеете в виду «объектные координаты», вы должны получить обратное преобразование результата, заданного алгоритмом pnp.

Существует хитрость для инвертирования матриц преобразования, которая позволяет сохранить операцию инверсии, которая обычно является дорогостоящей, и объясняет код на Python. Учитывая преобразование [R|t]у нас есть это inv([R|t]) = [R'|-R'*t], где R' это транспонирование R, Итак, вы можете кодировать (не проверено):

cv::Mat rvec, tvec;
solvePnP(..., rvec, tvec, ...);
// rvec is 3x1, tvec is 3x1

cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3

R = R.t();  // rotation of inverse
tvec = -R * tvec; // translation of inverse

cv::Mat T = cv::Mat::eye(4, 4, R.type()); // T is 4x4
T( cv::Range(0,3), cv::Range(0,3) ) = R * 1; // copies R into T
T( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; // copies tvec into T

// T is a 4x4 matrix with the pose of the camera in the object frame

Обновить: Позже, чтобы использовать T с OpenGL вы должны иметь в виду, что оси рамы камеры отличаются между OpenCV и OpenGL.

OpenCV использует ссылку, обычно используемую в компьютерном зрении: X указывает вправо, Y вниз, Z вперед (как в это изображение). Рамка камеры в OpenGL: X указывает вправо, Y вверх, Z назад (как в левой части это изображение). Итак, вам нужно применить вращение вокруг оси X на 180 градусов. Формула этой матрицы вращения находится в википедия.

// T is your 4x4 matrix in the OpenCV frame
cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X
cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame

Эти преобразования всегда сбивают с толку, и я могу ошибаться на каком-то этапе, так что возьмите это с крошкой соли.

Наконец, примите во внимание, что матрицы в OpenCV хранятся в порядке старших строк в памяти, а матрицы OpenGL — в главном порядке столбцов.

48