Оценка позы камеры (OpenCV PnP)

Я пытаюсь получить глобальную оценку позы по изображению четырех опорных точек с известными глобальными позициями, используя мою веб-камеру.

Я проверил много вопросов об обмене стека и несколько статей, и я не могу найти правильное решение. Номера позиций, которые я получаю, повторяются, но никоим образом не линейно пропорциональны движению камеры. К вашему сведению, я использую C++ OpenCV 2.1.

По этой ссылке изображены мои системы координат и данные испытаний, использованные ниже.

% Input to solvePnP():
imagePoints =     [ 481, 831; % [x, y] format
                    520, 504;
                   1114, 828;
                   1106, 507]
objectPoints = [0.11, 1.15, 0; % [x, y, z] format
                0.11, 1.37, 0; 
                0.40, 1.15, 0;
                0.40, 1.37, 0]

% camera intrinsics for Logitech C910
cameraMat = [1913.71011, 0.00000,    1311.03556;
             0.00000,    1909.60756, 953.81594;
             0.00000,    0.00000,    1.00000]
distCoeffs = [0, 0, 0, 0, 0]

% output of solvePnP():
tVec = [-0.3515;
         0.8928; 
         0.1997]

rVec = [2.5279;
       -0.09793;
        0.2050]
% using Rodrigues to convert back to rotation matrix:

rMat = [0.9853, -0.1159,  0.1248;
       -0.0242, -0.8206, -0.5708;
        0.1686,  0.5594, -0.8114]

Пока кто-нибудь может увидеть что-то не так с этими числами? Я был бы признателен, если бы кто-нибудь зарегистрировал их, например, в MatLAB (код выше подходит для m-file).

С этого момента я не уверен, как получить глобальную позу от rMat и tVec. Из того, что я прочитал в этом вопросе, получить позу от rMat и tVec просто:

position = transpose(rMat) * tVec   % matrix multiplication

Однако из других источников я подозреваю, что это не так просто.

Что мне нужно сделать, чтобы получить положение камеры в координатах реального мира? Поскольку я не уверен, является ли это проблемой реализации (хотя, скорее всего, проблемой теории), я бы хотел, чтобы кто-то, кто успешно использовал функцию solvePnP в OpenCV, ответил на этот вопрос, хотя любые идеи тоже приветствуются!

Большое спасибо за уделенное время.

Источник

3 ответа

Решение

Я решил это некоторое время назад, извинения за задержку года.

В Python OpenCV 2.1, который я использовал, и в более новой версии 3.0.0-dev я проверил, что, чтобы получить позу камеры в глобальном кадре, вы должны:

_, rVec, tVec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
Rt = cv2.Rodrigues(rvec)
R = Rt.transpose()
pos = -R * tVec

Теперь pos - это позиция камеры, выраженная в глобальном кадре (тот же кадр, в котором выражены точки объекта). R - это DCM-матрица ориентации, которая является хорошей формой для сохранения ориентации. Если вам требуются углы Эйлера, то вы можете преобразовать углы DCM в углы Эйлера, используя последовательность вращения XYZ, используя:

roll = atan2(-R[2][1], R[2][2])
pitch = asin(R[2][0])
yaw = atan2(-R[1][0], R[0][0])
Источник

Если вы имеете в виду с глобальной позой матрицу позы камеры 4x4, которую можно использовать в OpenGL, я делаю это так

CvMat* ToOpenGLCos( const CvMat* tVec, const CvMat* rVec )
{
    //** flip COS 180 degree around x-axis **//

    // Rodrigues to rotation matrix
    CvMat* extRotAsMatrix = cvCreateMat(3,3,CV_32FC1);
    cvRodrigues2(rVec,extRotAsMatrix);

    // Simply merge rotation matrix and translation vector to 4x4 matrix 
    CvMat* world2CameraTransformation = CreateTransformationMatrixH(tVec,
    extRotAsMatrix );

    // Create correction rotation matrix (180 deg x-axis)
    CvMat* correctionMatrix = cvCreateMat(4,4,CV_32FC1);
    /* 1.00000   0.00000   0.00000   0.00000
       0.00000  -1.00000  -0.00000   0.00000
       0.00000   0.00000  -1.00000   0.00000
       0.00000   0.00000   0.00000   1.00000 */
    cvmSet(correctionMatrix,0,0,1.0); cvmSet(correctionMatrix,0,1,0.0);
    ... 

    // Flip it
    CvMat* world2CameraTransformationOpenGL = cvCreateMat(4,4,CV_32FC1);
    cvMatMul(correctionMatrix,world2CameraTransformation,   world2CameraTransformationOpenGL);

    CvMat* camera2WorldTransformationOpenGL = cvCreateMat(4,4,CV_32FC1);
    cvInv(world2CameraTransformationOpenGL,camera2WorldTransformationOpenGL,
    CV_LU );

    cvReleaseMat( &world2CameraTransformationOpenGL );
    ...

    return camera2WorldTransformationOpenGL;
}

Я думаю, что переключение системы координат необходимо, потому что OpenCV и OpenGL/VTK/ и т.д. использовать разные системы координат, как показано на рисунке. Системы координат OpenGL и OpenCV

Ну, это работает так, но у кого-то может быть лучшее объяснение.

Источник

Положение камеры будет {- transpose( r) * t } . Вот и все.

И вы все сделали правильно, за исключением того, что cv::solvePnp дает (4 x 1) вектор для перевода, если я правильно помню, вам придется делить x, y, z с координатой w.

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