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

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

Я знаю, что cv::solvePnP может мне помочь, и после прочтения этого и этого я понимаю, что я выходы solvePnP 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 stuffs (я использую С++), но это не имеет для меня большого смысла:

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

Ответ 1

Если с "мировыми координатами" вы подразумеваете "координаты объекта", вам нужно получить обратное преобразование результата, заданного алгоритмом 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(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
// fill the last row of T (NOTE: depending on your types, use float or double)
double *p = T.ptr<double>(3);
p[0] = p[1] = p[2] = 0; p[3] = 1;

// 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 градусов. Формула этой матрицы вращения находится в wikipedia.

// 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 в основном порядке столбца.

Ответ 2

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

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

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