Smoothing a rotation vector

Question

I am currently using OpenCV to track the face of a person.

With this I extract the feature points and calculate the objects heads pose by applying the "solvePnP" function.

This function gives me a translation vector and a rotation vector.

I can later apply this to a camera in Unity so that it rotates around the object simulating the face moving around.

This works fairly well, and the head moves as the user moves his head however it is very jaggy.

I figured that smoothing the translation vector would be as easy as calculating the average of the current and previous points.

But I am having a hard time smoothing the rotation vector.

The data comes as a vector (here are some consecutive examples):

[2.918782807448489; 0.1896564461354077; -0.2327480775908206]

[2.922645604179207; 0.1834802203843292; -0.1842765118011375]

[-3.339417618845302; -0.2022563569884805; 0.1950620807331237]

[2.884993823241512; 0.1935712993426144; -0.2426539399793146]

[-3.341933025273495; -0.2062648239738171; 0.2609849753763552]

Which I would like to "smoothen". The opencv documents state the following about this vector:

Output rotation vector (see "Rodrigues") that, together with tvec, brings points from the model coordinate system to the camera coordinate system.

I've tried adding the previous plus the current rotation vector and then normalizing it and even though it seems a lot more stable it flickers between being upside down and not.

http://answers.unity3d.com/questions/437111/calculate-vector-exactly-between-2-vectors.html

Does anyone know what am I doing wrong?

Answer 1

So I guess you could save like the last 10 rotations in a List and get the average of them with this function. The function return could be set as your final rotation. This should smoothen your rotation.

private Quaternion calcAvg(List<GameObject> rotationlist)
{
    if (rotationlist.Count == 0)
        throw new ArgumentException();

    float x, y, z, w;
    foreach (Quaternion q in rotationlist)
    {
        x += q.x; y += q.y; z += q.z; w += q.w;
    }
    float k = 1.0f / Mathf.Sqrt(x * x + y * y + z * z + w * w);
    return new Quaternion(x * k, y * k, z * k, w * k);
}

Further discussion on this topic can be found here, which is also the source of the code I posted above : Calculate average of arbitrary amount of quaternions (recursion)