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?

So you need a function which gives you the average of multiple rotations? — TobiasW, Apr 22 '16 at 06:49
Maybe, sorry i'm very confused at the moment. My source gives me the "rotation vector that describes the 'pose' of an object". Then it continuously gives it to me but varies a little bit (due to noise), so I need a function that gives me the average of multiple rotation vectors. (Each rotation vector is a sample in time) — Pochi, Apr 22 '16 at 07:06

score 2 · Accepted Answer · edited Apr 13 '17 at 12:18

2

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)

edited Apr 13 '17 at 12:18

Community

1

answered Apr 22 '16 at 07:15

TobiasW

987
5
20
37

But I don't have quaternions, my rotation vector has 3 dimensions and is used after with the Rodrigues formula to create a rotation matrix. Or am I missing something? – Pochi Apr 22 '16 at 07:17
Actually, computing the weighted average of n quaternions is not going to amount to that pseudocode and it cannot be done analytically. It requires a "gradient descent" iterative process, computing the exp and log maps to switch back and forth between the unit hypersphere and the tangent plane where you actually estimate the average. This is a reference for that: https://hal.inria.fr/hal-00789164v2/document. – teodron Apr 22 '16 at 07:18
1

@Chiquis you can convert a rotation vector to a quaternion. I assume that your rotation vector is v = angle * unitDirection; therefore, compute the angle as the norm of v and recover the normalized unitDirection. You should be able to convert that to quaternion form. – teodron Apr 22 '16 at 07:19
@teodron Do you mean I should first change my rotation vectors to quaternions and then compute the weighted average by using the formulas shown on the paper you posted? (Or can i convert to quaternion and then us the code listed here?) – Pochi Apr 22 '16 at 07:29
I'm not sure, what exactly is wrong with the code I posted, since it's doing the job in my case so I guess give it a try and convert to quaternion... (I don't want to say, that something is wrong what teodron posted, he seems to have a lot of knowledge on this topic ;) ) – TobiasW Apr 22 '16 at 07:33
@TobiasW: mathematically, the results is not exactly a correct average (unless, I think, the axis of rotation is the same). Nevertheless, it works well enough if the quaternions are close to each other. Depending on the obtained results and input data, you and the OP might not have anything to worry about. The OP still needs to convert to quats, then back to rotation vectors. – teodron Apr 22 '16 at 07:55

Smoothing a rotation vector

1 Answers1