OpenCV: как использовать угол Эйлера для определения ориентации камеры

У меня возник вопрос о том, как использовать 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 ;
}

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


opencv rotation euler-angles opencv-solvepnp
person V.Lee    schedule 13.04.2018    source источник

Ответы (1)


arrow_upward
0
arrow_downward

Кажется, я знаю, как решить эту проблему. Порядок ориентации всегда z-y-x. Это означает, что вам просто нужно повернуть ваш "tvec" вокруг оси Z, затем вокруг оси 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);
}
person V.Lee    schedule 16.04.2018