Proper way to get rotation for each key frame? And convert it to Euler

[ProstoNekitos]

Greetings, i'm trying to gather rotation in Euler angles by collecting quaternions from publishCamPose method and than converting it to Euler.

void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override {
    Eigen::Quaterniond quat(frame->camToWorld.rotationMatrix());
    ...

    struct EulerAngles
    {
        double roll, pitch, yaw;
    };

    struct Quaternion
    {
        double w, x, y, z;
    };

    EulerAngles ToEulerAngles(Quaternion q)
    {
        EulerAngles angles;

        // roll (x-axis rotation)
        double sinr_cosp = +2.0 * (q.w * q.x + q.y * q.z);
        double cosr_cosp = +1.0 - 2.0 * (q.x * q.x + q.y * q.y);
        angles.roll = atan2(sinr_cosp, cosr_cosp);

        // pitch (y-axis rotation)
        double sinp = +2.0 * (q.w * q.y - q.z * q.x);
        if (fabs(sinp) >= 1)
            angles.pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
        else
            angles.pitch = asin(sinp);

        // yaw (z-axis rotation)
        double siny_cosp = +2.0 * (q.w * q.z + q.x * q.y);
        double cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z);
        angles.yaw = atan2(siny_cosp, cosy_cosp);

        return angles;
    }


double quatToAngle(double x, double y, double z, double w)
{
    return 2*asin(std::sqrt(std::pow(x, 2) +
                     std::pow(y, 2) +
                     std::pow(z, 2))) * 180 / M_PI;
}

But than plots shows some very strange angles:

It's not normal, cause azimuth from gnss is ok and i've never tried to do a barrel roll

Need some hints, what am i doing wrong? Might it be because of z axis is depth?

[studennis911988]

Yes, you are right, since the visual reference frame has z axis pointing forward, so you need some modification.

Eigen::Quaterniond quat(frame->camToWorld.rotationMatrix()); q_.coeffs()[0] = q_.coeffs()[0]; // w->w q_.coeffs()[1] = q_.coeffs()[3]; // x->z q_.coeffs()[2] = - q_.coeffs()[1]; // y-> -x q_.coeffs()[3] = - q_.coeffs()[2]; // z-> -y Or you can use rotation matrix to correct the orientation of quat.

Hope this help.

[rancheng]

export those quaternions into csv file, and convert these quaternions by the quaternion_matrix(q) function in transformations.py