Averaging quaternions

Question

To compensate for shaky hands when holding a mobile device I'm averaging the rotation of the phone over the last n (in this case 16) frames, and setting the camera's rotation to this average. This works smoothly and allows the user to look around in 360° by moving their phone around without getting sick of vibrations.

using System.Collections.Generic;
using UnityEngine;
using UnityEngine.XR;

public class OrientationController : MonoBehaviour
{
    private List<Quaternion> lastQuaternions = new List<Quaternion>();
    private readonly int maxRotationCount = 16;

    void Start()
    {
        //Load the cardboard after we've started up to prevent the cardboard overlay from showing
        XRSettings.LoadDeviceByName("cardboard");
        //Disable XR to go into "magic window" mode
        XRSettings.enabled = false;
    }

    void Update()
    {
        UpdateCameraRotation();
    }

    /// <summary>
    /// Update the main camera's rotation based on the data taken from the XRNode.Head
    /// </summary>
    private void UpdateCameraRotation()
    {
        lastQuaternions.Add(InputTracking.GetLocalRotation(XRNode.Head));
        if (lastQuaternions.Count > maxRotationCount)
        {
            lastQuaternions.RemoveAt(0);
            transform.localRotation = SmoothRotation();
        }
    }

    /// <summary>
    /// Get the average rotation over the last 16 frames
    /// </summary>
    /// <returns>The new rotation quaternion</returns>
    private Quaternion SmoothRotation()
    {
        Quaternion quatA = lastQuaternions[0];
        Quaternion quatB = lastQuaternions[1];
        Quaternion quatC = lastQuaternions[2];
        Quaternion quatD = lastQuaternions[3];
        Quaternion quatE = lastQuaternions[4];
        Quaternion quatF = lastQuaternions[5];
        Quaternion quatG = lastQuaternions[6];
        Quaternion quatH = lastQuaternions[7];
        Quaternion quatI = lastQuaternions[8];
        Quaternion quatJ = lastQuaternions[9];
        Quaternion quatK = lastQuaternions[10];
        Quaternion quatL = lastQuaternions[11];
        Quaternion quatM = lastQuaternions[12];
        Quaternion quatN = lastQuaternions[13];
        Quaternion quatO = lastQuaternions[14];
        Quaternion quatP = lastQuaternions[15];
        Quaternion quatAB = Quaternion.Lerp(quatA, quatB, 0.5f);
        Quaternion quatCD = Quaternion.Lerp(quatC, quatD, 0.5f);
        Quaternion quatEF = Quaternion.Lerp(quatE, quatF, 0.5f);
        Quaternion quatGH = Quaternion.Lerp(quatG, quatH, 0.5f);
        Quaternion quatIJ = Quaternion.Lerp(quatI, quatJ, 0.5f);
        Quaternion quatKL = Quaternion.Lerp(quatK, quatL, 0.5f);
        Quaternion quatMN = Quaternion.Lerp(quatM, quatN, 0.5f);
        Quaternion quatOP = Quaternion.Lerp(quatO, quatP, 0.5f);
        Quaternion quatABCD = Quaternion.Lerp(quatAB, quatCD, 0.5f);
        Quaternion quatEFGH = Quaternion.Lerp(quatEF, quatGH, 0.5f);
        Quaternion quatIJKL = Quaternion.Lerp(quatIJ, quatKL, 0.5f);
        Quaternion quatMNOP = Quaternion.Lerp(quatMN, quatOP, 0.5f);
        Quaternion quatABCDEFGH = Quaternion.Lerp(quatABCD, quatEFGH, 0.5f);
        Quaternion quatIJKLMNOP = Quaternion.Lerp(quatIJKL, quatMNOP, 0.5f);
        //Quaternion quatABCDEFGHIJKLMNOP = Quaternion.Lerp(quatABCDEFGH, quatIJKLMNOP, 0.5f);//We don't currently want this extreme level of smoothness

        return quatIJKLMNOP;
    }
}

However, despite it working as intended my current approach doesn't feel optimal, as it relies on creating a lot of quaternions each frame, and lerping between them. It is also not scaleable at all, as I'd need to make hardcoded changes every time I want to change the intensity of the smoothing.

Using Unity 2019.1.8f1 running C# .Net 4.x scripting runtime, and IL2CPP scripting backend.

To recreate in Unity, you also need to set the build target to Android. Enable "Virtual reality supported" under project settings > player > XR Settings. enable the virtual reality SDKs "None" and "Cardboard" (In that order).

I'm not experiencing any performance issues with this implementation, but I am curious to know if there are approaches that don't require me to create so many quaternions, or at least make the number of quaternions I can average dynamic.

Answer 1

Below you can find some of my reflections.

1. Method InputTracking.GetLocalRotation(XRNode.Head) is obsolete as DOCS says.
2. There is a lot calculations per one frame indeed, but Quaternions are structs and you store only 16 of them on heap. As you mentioned yourself, you don't see performance issues. So it's up to you/manager if it's worth more time spending on this.
3. If you want to make it more scalable then you should move it to external class and inject your rate (the last parameter of Lerp method)
4. I suggest small refactoring. Please consider refactor of SmoothRotation method to use loop internally, it shouldn't be hard task. Then you'll be able to easily extend frame count that should be considered during calculation.
5. Set maxRotationCount as const, as it won't change.
6. Initialize list with size new List(maxRotationCount), you'll avoid copying data while list resizing.

7. Little pseudo code for point 4 below. You can easily change interface to have two parameters, test for edge cases etc.

   class SmoothnessCalculator
   {
       private readonly List<Quaternion> quaternions;
   
       public Calculator(int frameCountToStore)
       {
           this.quaternions = new List<Quaternion>(frameCountToStore);
       }
   
       public void Add(Quaternion quaternion)
       {
           if(this.quaternions.Count == this.quaternions.Capacity)
           {
               this.quaternions.RemoveAt(0);
           }
   
           this.quaternions.Add(quaternion);
       }
   
       public Quaternion SmoothRotation(double rate)
       {
           Quaternion result = null;
   
           for(int i = 0; i < this.quaternions.Count - 1; i++)
           {
               var current = this.quaternions[i];
               var next = this.quaternions[i + 1];
   
               result = Quaternion.Larp(current, next, rate);
           }
   
           return result;
       }
   } 
   

Answer 2

To build upon Karol Miszczyk's answer, I'll take on the averaging function of multiple quaternions, and make it scaleable.

The first option is to use a recursive function. It's scaleable, but you need to ensure the quaternions list has a count that is a power of 2.

/// <summary>
/// Get the average value of a list of quaternion
/// </summary>
/// <param name="quaternions">The list of quaternions to average. Count must be a power of 2 and at least 2</param>
/// <returns>The average quaternion</returns>

public static Quaternion QuaternionAverageRecursive(List<Quaternion> quaternions)
{
    if (quaternions.Count == 2)
    {
        return Quaternion.Lerp(quaternions[0], quaternions[1], 0.5f);
    }

    var quats1 = quaternions.GetRange(0, quaternions.Count / 2);
    var quats2 = quaternions.GetRange(quaternions.Count / 2, quaternions.Count / 2);

    return Quaternion.Lerp(QuaternionAverageRecursive(quats1), QuaternionAverageRecursive(quats2), 0.5f);
}

It should give the exact same result as your method, but solves none of the potential performance issue.

Another option is to average each quaternion with a partial average in a loop, similar to what Karol Miszczyk proposed, although his version seems to have bugs. Doing so naively would give more weight to the quaternions at the end of the list, which strays away from your original solution, but could be suitable to your case:

/// <summary>
/// Get the average value of a list of quaternion using a naive accumulating algorithm
/// </summary>
/// <param name="quaternions">The list of quaternions to average.</param>
/// <returns></returns>
public static Quaternion QuaternionAverageNaive(List<Quaternion> quaternions)
{
    Quaternion result = quaternions[0];

    for (int i = 1; i < quaternions.Count; i++)
    {
        result = Quaternion.Lerp(result, quaternions[i], 0.5f);
    }

    return result;
}

It is suitable for any non-empty List<Quaternion> and should improve performance, but the results differ from the ones returned by your current method.

Using the third parameter of Quaternion.Lerp to weight the values differently allows to get results close to your current solution:

/// <summary>
/// Get the average value of a list of quaternion using a weighted accumulating algorithm
/// </summary>
/// <param name="quaternions">The list of quaternions to average.</param>
/// <returns></returns>
public static Quaternion QuaternionAverageWeighted(List<Quaternion> quaternions)
{
    Quaternion result = quaternions[0];

    for (int i = 1; i < quaternions.Count; i++)
    {
        result = Quaternion.Lerp(result, quaternions[i], 1f / (i + 1f));
    }

    return result;
}

It should have little cost on performance compared to the previous solution, and is still completely scalable. However, while this method would be exact when averaging numbers, the results differ slightly from your method. I suppose this is due to the quirkiness of quaternions, or may be caused by floating point errors, I'm not sure. It is stable, though (averaging a list of identical quaternions outputs the same quaternion), an can be applied to your case.

Finally, answers to this question on StackOverflow seem to indicate that getting the exact average implies solving a 4n * 4n matrix for eigenvalues (n beein the number of quaternions), which is definitely doable and scalable, but probably sub-optimal performance-wise.

Answer 3

Building on the previous two answers, I like to suggest a different approach to smoothing the series of quaternions.

Right now, you are weighting the past 16 frames equally, which is requiring you to keep all 16 rotations and use a bunch of calls to lerp them all together to get the average.

A different, simpler to implement, approach is to use an exponentially weighted moving average as your smoothing function. That would look like:

public static Quaternion SmoothRotation(Quaternion nextFrame)
{
  const float alpha = 0.118; // 2/(N+1), N is number of frames in equivalent simple moving average) 
  lastFrame = Quaternion.Lerp(lastFrame, nextFrame, alpha);
  return lastFrame;
}
private static Quaternion lastFrame = Quaternion.Identity;

The results won't be exactly like your equally weighted moving average, but it should still smooth acceptably. The higher alpha is, the less smoothing will be done. The lower alpha is, the longer it takes to reach a steady state after a fast transition.