VectorRollingStatistics.cs

Go to the documentation of this file.

// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License. See LICENSE in the project root for license information.
using UnityEngine;

namespace HoloToolkit.Unity
{
    public class VectorRollingStatistics
    {
        public float CurrentStandardDeviation;

        public float StandardDeviationDeltaAfterLatestSample;

        public float StandardDeviationsAwayOfLatestSample;

        public Vector3 Average;

        public float ActualSampleCount;

        private int currentSampleIndex;

        private Vector3[] samples;

        private Vector3 cumulativeFrame;

        private Vector3 cumulativeFrameSquared;

        private int cumulativeFrameSamples;

        private int maxSamples;

        public void Init(int sampleCount)
        {
            maxSamples = sampleCount;
            samples = new Vector3[sampleCount];
            Reset();
        }

        public void Reset()
        {
            currentSampleIndex = 0;
            ActualSampleCount = 0;
            cumulativeFrame = Vector3.zero;
            cumulativeFrameSquared = Vector3.zero;
            cumulativeFrameSamples = 0;
            CurrentStandardDeviation = 0.0f;
            StandardDeviationDeltaAfterLatestSample = 0.0f;
            StandardDeviationsAwayOfLatestSample = 0.0f;
            Average = Vector3.zero;
            if (samples != null)
            {
                for (int index = 0; index < samples.Length; index++)
                {
                    samples[index] = Vector3.zero;
                }
            }
        }

        public void AddSample(Vector3 value)
        {
            if (maxSamples == 0)
            {
                return;
            }

            // remove the old sample from our accumulation
            Vector3 oldSample = samples[currentSampleIndex];
            cumulativeFrame -= oldSample;
            cumulativeFrameSquared -= (oldSample.Mul(oldSample));

            // Add the new sample to the accumulation
            samples[currentSampleIndex] = value;
            cumulativeFrame += value;
            cumulativeFrameSquared += value.Mul(value);

            // Keep track of how many samples we have
            cumulativeFrameSamples++;
            ActualSampleCount = Mathf.Min(maxSamples, cumulativeFrameSamples);

            // see how many standard deviations the current sample is from the previous average
            Vector3 deltaFromAverage = (Average - value);
            float oldStandardDeviation = CurrentStandardDeviation;
            StandardDeviationsAwayOfLatestSample = oldStandardDeviation == 0 ? 0 : (deltaFromAverage / oldStandardDeviation).magnitude;

            // And calculate new averages and standard deviations
            // (note that calculating a standard deviation of a Vector3 might not 
            // be done properly, but the logic is working for the gaze stabilization scenario)
            Average = cumulativeFrame / ActualSampleCount;
            float newStandardDev = Mathf.Sqrt((cumulativeFrameSquared / ActualSampleCount - Average.Mul(Average)).magnitude);
            StandardDeviationDeltaAfterLatestSample = oldStandardDeviation - newStandardDev;
            CurrentStandardDeviation = newStandardDev;

            // update the next list position
            currentSampleIndex = (currentSampleIndex + 1) % maxSamples;
        }
    }
}