1 | initial version |
You can use solvePnP() to calculate euler angles , but the out put of solvePnP() is not directly roll, yaw & pitch, but need to calculate using rotation matrix.
I also came through similar problem, and found the function decomposeProjectionMatrix which gives euler angles as it's output.
In C++ the implementation should be something like below, assumes you already get the rotation Mat from solvePnP() like,
solvePnP(..,...,rotCamerMatrix,transMatrix,false);
Rodrigues(rotCamerMatrix,rotCamerMatrix1);
Now using the function,
void getEulerAngles(Mat &rotCamerMatrix,Vec3d &eulerAngles){
Mat cameraMatrix,rotMatrix,transVect,rotMatrixX,rotMatrixY,rotMatrixZ;
double* _r = rotCamerMatrix.ptr<double>();
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);
}
Call it like,
Vec3d eulerAngles;
getEulerAngles(rotCamerMatrix1,eulerAngles);
where,
yaw = eulerAngles[1];
pitch = eulerAngles[0];
roll = eulerAngles[2];