OpenCV:如何使用欧拉角确定摄像机方向

时间:2018-04-13 01:47:32

标签: opencv rotation euler-angles opencv-solvepnp

我有一个关于如何使用EulerAngle来确定相机方向的问题 首先,我使用solvePnP函数并获得两个输出“rvec”和“tvec”,然后我使用Rodrigues将rvec转换为旋转矩阵“R”。之后,我使用下面的函数计算了EulerAngle:

void getEulerAngles(cv::Mat matrix)

{

assert(isRotationMatrix(matrix));

float sy=sqrt(matrix.at<double>(0,0)*matrix.at<double>(0,0)+matrix.at<double>(1,0)*matrix.at<double>(1,0));

bool singular = sy<1e-6;
float theta_x=0.0,theta_y=0.0,theta_z=0.0;//theta_x means rotation around X-Axis
if(!singular)
{
    theta_x=atan2(matrix.at<double>(2,1),matrix.at<double>(2,2));
    theta_x= theta_x*180.0/3.1416 ;
    theta_y=atan2(-matrix.at<double>(2,0), sy);
    theta_y= theta_y*180.0/3.1416 ;
    theta_z=atan2(matrix.at<double>(1,0), matrix.at<double>(0,0));
    theta_z= theta_z*180.0/3.1416 ;
}
else
{
    theta_x=atan2(-matrix.at<double>(1,2), matrix.at<double>(1,1));
    theta_x= theta_x*180.0/3.1416 ;
    theta_y=atan2(-matrix.at<double>(2,0), sy);
    theta_y= theta_y*180.0/3.1416 ;
    theta_z=0;
    theta_z= theta_z*180.0/3.1416 ;
}

我知道不同的旋转顺序可以产生不同的结果。所以如果我想获得相机的方向,我应该选择什么样的旋转顺序?

1 个答案:

答案 0 :(得分:0)

我想我知道如何解决这个问题。方向顺序始终为z-y-x。这意味着您只需围绕Z轴旋转“tvec”,然后围绕y轴旋转,最后围绕x轴旋转。并记住使用负欧拉角。 有我的代码:

    void calCamPose(cv::Mat t)
// the order of rotation is z-y-x
{
    cv::Point3f tvec(t);
    float x1=cos(-theta_z)*tvec.x-sin(-theta_z)*tvec.y;
    float y1=sin(-theta_z)*tvec.x+cos(-theta_z)*tvec.y;//first rotation

    float outx=cos(-theta_y)*x1+sin(-theta_y)*tvec.z;
    float z2=cos(-theta_y)*tvec.z+sin(-theta_y)*x1;//second rotation

    float outy=cos(-theta_x)*y1-sin(-theta_x)*z2;
    float outz=cos(-theta_x)*z2+sin(-theta_x)*y1;//third rotation
    cv::Point3f cam_pose=(0,0,0);
    cam_pose.x=outx,cam_pose.y=outy,cam_pose.z=outz;
    Debug("Cam_Pose");
    Debug(cam_pose);
}