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

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

я знаю это cv::solvePnP могли бы помочь мне, и после прочтения этого и этого я понимаю, что я выводить решения executePnP 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 представляют собой матрицу 3x1, 3 элемента вектора
  • cv2.Rodrigues(rvec) - это матрица 3x3
  • cv2.Rodrigues(rvec)[0] - матрица 3x1, 3 элемента вектора
  • cameraPosition - это матричное умножение 3x1 * 1x3, которое представляет собой матрицу.. 3x3. как я могу использовать это в OpenGL с простым glTranslatef а также glRotate звонки?
Источник

2 ответа

Решение

Если с "мировыми координатами" вы имеете в виду "объектные координаты", вы должны получить обратное преобразование результата, заданного алгоритмом 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 - в главном порядке столбцов.

Источник

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

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

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

Источник
Другие вопросы по тегам