Get angles of head pose

[patriciastar]

Hi again, seems like I'm super stupid, but I don't know how to get the angles of the head pose from the rotation matrix. OpenCV-SolvePnP and then Rodriges give me a 4x4 matrix (rotation + translation) - if I do unterstand this correctly. I thought I can use the roll-angle = atan2(r{32}, r{33}) (or atan2(r{21},r{22}) when starting to count from 0) and also pitch and yaw to get the angles... But somehow the values I get do not correspont to the headpose shown in my images. For example there seems to be no big difference between a picture with a frontal head-pose and one with a 45° head-pose (only looking at the angle around the vertical axis...) Hope someone can help

Thanks Patricia

[jormansa]

Hi, I have the same problem. I tried to use decomposeProjectionMatrix from opencv to get the Euler angles, but the angles returned dont look good. Any suggestions? Thanks.

[severin-lemaignan]

Hello! and sorry for not responding earlier.

You can check the code of tools/estimate_head_direction.cpp in gazr (the 'active' for of attention_tracker). It does exactly that.

https://github.com/severin-lemaignan/gazr/blob/master/tools/estimate_head_direction.cpp#L125

Note that I rely on one header of the ROS tf library for the conversion (see LinearMath/).

[jormansa]

Thanks for the response. However, I have implemented the function you said and I still have wrong angles, I dont know where is the mistake. I attach the code of the function and the main call. Any help would be apreciate. Thanks a lot!

//**********************************************************************
inline void getEulerYPR(double& yaw, double& pitch, double& roll, const cv::Matx33d& rotMat, unsigned int solution_number = 1)
{
    double m00 = rotMat(0, 0);
    double m01 = rotMat(0, 1);
    double m02 = rotMat(0, 2);
    double m10 = rotMat(1, 0);
    double m11 = rotMat(1, 1);
    double m12 = rotMat(1, 2);
    double m20 = rotMat(2, 0);
    double m21 = rotMat(2, 1);
    double m22 = rotMat(2, 2);

    struct Euler
    {
        double yaw;
        double pitch;
        double roll;
    };

    Euler euler_out;
    Euler euler_out2; //second solution
                      //get the pointer to the raw data

                      // Check that pitch is not at a singularity
                      // Check that pitch is not at a singularity
    if (abs(m20) >= 1)
    {
        euler_out.yaw = 0;
        euler_out2.yaw = 0;

        // From difference of angles formula
        if (m20 < 0)  //gimbal locked down
        {
            double delta = atan2(m01, m02);
            euler_out.pitch = CV_PI / double(2.0);
            euler_out2.pitch = CV_PI / double(2.0);
            euler_out.roll = delta;
            euler_out2.roll = delta;
        }
        else // gimbal locked up
        {
            double delta = atan2(-m01, -m02);
            euler_out.pitch = -CV_PI / double(2.0);
            euler_out2.pitch = -CV_PI / double(2.0);
            euler_out.roll = delta;
            euler_out2.roll = delta;
        }
    }
    else
    {
        euler_out.pitch = -asin(m20);
        euler_out2.pitch = CV_PI - euler_out.pitch;

        euler_out.roll = atan2(m21 / cos(euler_out.pitch), m22 / cos(euler_out.pitch));
        euler_out2.roll = atan2(m21 / cos(euler_out2.pitch), m22 / cos(euler_out2.pitch));

        euler_out.yaw = atan2(m10 / cos(euler_out.pitch), m00 / cos(euler_out.pitch));
        euler_out2.yaw = atan2(m10 / cos(euler_out2.pitch), m00 / cos(euler_out2.pitch));
    }

    if (solution_number == 1)
    {
        yaw = euler_out.yaw;
        pitch = euler_out.pitch;
        roll = euler_out.roll;
    }
    else
    {
        yaw = euler_out2.yaw;
        pitch = euler_out2.pitch;
        roll = euler_out2.roll;
    }
}
//**********************************************************************
// MAIN CODE
Matx33d rotation;
Rodrigues(rvec, rotation);

// Get Euler angles from rotation matrix: http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf
double raw_yaw = 0, raw_pitch = 0, raw_roll = 0;
getEulerYPR(raw_yaw, raw_pitch, raw_roll, rotation, 1);

// Fix angles:
raw_roll = raw_roll - CV_PI / 2;
raw_yaw = raw_yaw + CV_PI / 2;

double roll = raw_pitch;
double yaw = raw_yaw;
double pitch = -raw_roll;

std::cout << "YAW = " << todeg(yaw) << std::endl;
std::cout << "PITCH = " << todeg(pitch) << std::endl;
std::cout << "ROLL = " << todeg(roll) << std::endl;
//**********************************************************************

[severin-lemaignan]

Did you try to compile and run tools/estimate_head_direction.cpp to check if this work for you?

[jormansa]

My mistake, I forgot to invert the matrix, sorry. Now I get the correct yaw, pitch, roll according to the left-hand rule. Many thanks!! Do you know where can I check the maths behind these formulas? I guess the equations are from http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf. However, the other operations are quite strange for me (invert the matrix, substract PI/2, permute roll and pitch angles, etc). Thanks.

[severin-lemaignan]

Matrix inversion, adding/removing Pi/2, permute roll and pitch are very likely due to the conversions between different coordinate systems that are used. For instance, the optical axis of a camera is traditionally the Z axis, which would typically lead to inverted angles when you assume a Z pointing up for the head.