Converting angular velocity to quaternion in OpenCV

Question

I need the angular velocity expressed as a quaternion for updating the quaternion every frame with the following expression in OpenCV:

q(k)=q(k-1)*qwt;

My angular velocity is

Mat w;  //1x3

I would like to obtain a quaternion form of the angles

Mat qwt;   //1x4

I couldn't find information about this, any ideas?

Answer 1

If I understand properly you want to pass from this Axis Angle form to a quaternion.

As shown in the link, first you need to calculate the module of the angular velocity (multiplied by delta(t) between frames), and then apply the formulas.

A sample function for this would be

// w is equal to angular_velocity*time_between_frames
void quatFromAngularVelocity(Mat& qwt, const Mat& w)
{
    const float x = w.at<float>(0);
    const float y = w.at<float>(1);
    const float z = w.at<float>(2);
    const float angle = sqrt(x*x + y*y + z*z);  // module of angular velocity

    if (angle > 0.0) // the formulas from the link
    {
        qwt.at<float>(0) = x*sin(angle/2.0f)/angle;
        qwt.at<float>(1) = y*sin(angle/2.0f)/angle;
        qwt.at<float>(2) = z*sin(angle/2.0f)/angle;
        qwt.at<float>(3) = cos(angle/2.0f);
    } else    // to avoid illegal expressions
    {
        qwt.at<float>(0) = qwt.at<float>(0)=qwt.at<float>(0)=0.0f;
        qwt.at<float>(3) = 1.0f;
    }
}