Поворот камеры SolvePnp - PullRequest
Новая
       20

Поворот камеры SolvePnp

0 голосов
/ 29 мая 2018

Может ли кто-нибудь помочь мне с математикой при расчете вращения Y моей камеры?Я использую SolvePnP, чтобы получить rvec.Из этого rvec я хочу узнать, насколько моя камера вращается вокруг своей оси Y. 

PointF[] ImagePointsCam = GetImagePointsCam(imgThres);
MCvPoint3D32f[] ObjectPointsCam = GetObjectPointsCam();
float f = 1.0f / (float)(Math.Tan(CameraFOV / 360.0 * 3.1415));
Matrix<float> CamMat = new Matrix<float>(3, 3);
CamMat.Data[0, 0] = (float)(f * 0.5 * ResolutionWidth);
CamMat.Data[0, 1] = 0;
CamMat.Data[0, 2] = (float)(ResolutionWidth / 2);
CamMat.Data[1, 0] = 0;
CamMat.Data[1, 1] = (float)(f * 0.5 * ResolutionHeight);
CamMat.Data[1, 2] = (float)(ResolutionHeight / 2);
CamMat.Data[2, 0] = 0;
CamMat.Data[2, 1] = 0;
CamMat.Data[2, 2] = 1;
Mat DistMat = new Mat();
Mat rvec = new Mat();
Mat tvec = new Mat();
CvInvoke.SolvePnP(ObjectPointsCam, ImagePointsCam, CamMat, DistMat, rvec, tvec, false, SolvePnpMethod.Iterative);
Mat R = new Mat();
CvInvoke.Rodrigues(rvec, R);

Я не уверен, как продолжить, чтобы получить одно значение, представляющее вращение Yкамера.

РЕДАКТИРОВАТЬ

Мне удалось заставить его работать в C # с DllImport и ответом @ Tahera.T.Теперь я получаю сообщение об ошибке, которое, я думаю, связано с переменными, которые я предоставляю для функции.Может ли кто-нибудь посмотреть, правильно ли я это делаю? 

[DllImport("opencv_world320.dll")]

private static extern void cvDecomposeProjectionMatrix(Mat projMatr, out Mat calibMatr, 
out Mat rotMatr, out Mat posVect, out Mat rotMatrX, out Mat rotMatrY, out Mat rotMatrZ,
out MCvPoint3D64f eulerAngles);

public MCvPoint3D64f GetEulerAngles(Mat R)
{
    Matrix<float> _r = new Matrix<float>(3,4);
    Matrix<float> _R = new Matrix<float>(R.Rows, R.Cols);
    R.CopyTo(_R);
    _r.Data[0, 0] = _R.Data[0, 0];
    _r.Data[0, 1] = _R.Data[0, 1];
    _r.Data[0, 2] = _R.Data[0, 2];
    _r.Data[0, 3] = 0;
    _r.Data[1, 0] = _R.Data[1, 0];
    _r.Data[1, 1] = _R.Data[1, 1];
    _r.Data[1, 2] = _R.Data[1, 2];
    _r.Data[1, 3] = 0;
    _r.Data[2, 0] = _R.Data[2, 0];
    _r.Data[2, 1] = _R.Data[2, 1];
    _r.Data[2, 2] = _R.Data[2, 2];
    _r.Data[2, 3] = 0;
    Mat projMatr = _r.Mat;

    cvDecomposeProjectionMatrix(projMatr, out Mat calibMatr, out Mat rotMatr, out Mat posVect, 
    out Mat rotMatrX, out Mat rotMatrY, out Mat rotMatrZ, out MCvPoint3D64f eulerAngles);

    return eulerAngles;
}

Ошибка, которую я получаю сейчас: 

System.Runtime.InteropServices.SEHException: 'External component has thrown an exception.'

С кодом ошибки: 

-2147467259

Я вызываю функцию с неправильными переменными?

1 Ответ

0 голосов
/ 31 мая 2018

Solvepnp не дает фактических значений вращения, он использовался для получения реальной точки мира, поэтому, чтобы получить реальные углы поворота, вам нужно будет сделать еще несколько шагов: Сначала используйте solvepnp, чтобы получить вектор вращения.Во-вторых, используйте родриг, чтобы преобразовать его в матрицу вращения.Теперь используйте декомпозицию ProjectionMatrix для вычисления угла Эйлера.Эти углы Эйлера дают вам вращение вдоль оси.Вы можете попробовать это: 

void getEulerAngles(Mat &rotCamerMatrix,Vec3d &eulerAngles){

Mat cameraMatrix,rotMatrix,transVect,rotMatrixX,rotMatrixY,rotMatrixZ;
double* _r = rotCamerMatrix.ptr();
double projMatrix[12] = {_r[0],_r[1],_r[2],0,
_r[3],_r[4],_r[5],0,
_r[6],_r[7],_r[8],0};

decomposeProjectionMatrix( Mat(3,4,CV_64FC1,projMatrix),
cameraMatrix,
rotMatrix,
transVect,
rotMatrixX,
rotMatrixY,
rotMatrixZ,
eulerAngles);
}

Вызовите функцию следующим образом: 

Vec3d eulerAngles;
getEulerAngles(rotCamerMatrix1,eulerAngles);

Где ваши углы поворота будут: 

yaw = eulerAngles[1];
pitch = eulerAngles[0];
roll = eulerAngles[2];

Где шаг - это ваш поворот вдольОсь Y.

...