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

Question

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

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

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

Answer 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 

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

OpenCV использует ссылку, обычно используемую в компьютерном зрении: X указывает направо, Y вниз, Z спереди (как в this image). Рамка камеры в OpenGL: X указывает вправо, Y вверх, Z на спину (как в this image). Таким образом, вам необходимо применить поворот вокруг оси 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 в порядке, соответствующем столбцу.

Answer 2

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

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

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