Skip to content

Commit fc627cf

Browse files
daraxdarax
authored
Merge pull request #286 from darax/master
simplified GazeStabilizer and made it less sticky
2 parents 2ea7c44 + b0069cd commit fc627cf

File tree

3 files changed

+200
-170
lines changed

3 files changed

+200
-170
lines changed

‎Assets/HoloToolkit/Input/Scripts/GazeStabilizer.cs‎

Lines changed: 45 additions & 170 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,5 @@
11
// Copyright (c) Microsoft Corporation. All rights reserved.
22
// Licensed under the MIT License. See LICENSE in the project root for license information.
3-
4-
using System.Collections.Generic;
5-
using System.Linq;
63
using UnityEngine;
74

85
namespace HoloToolkit.Unity
@@ -14,57 +11,43 @@ namespace HoloToolkit.Unity
1411
public class GazeStabilizer : MonoBehaviour
1512
{
1613
[Tooltip("Number of samples that you want to iterate on.")]
17-
[Range(1, 120)]
14+
[Range(40, 120)]
1815
public int StoredStabilitySamples = 60;
1916

20-
[Tooltip("Position based distance away from gravity well.")]
21-
public float PositionDropOffRadius = 0.02f;
22-
23-
[Tooltip("Direction based distance away from gravity well.")]
24-
public float DirectionDropOffRadius = 0.1f;
25-
26-
[Tooltip("Position lerp interpolation factor.")]
27-
[Range(0.25f, 0.85f)]
28-
public float PositionStrength = 0.66f;
29-
30-
[Tooltip("Direction lerp interpolation factor.")]
31-
[Range(0.25f, 0.85f)]
32-
public float DirectionStrength = 0.83f;
33-
34-
[Tooltip("Stability average weight multiplier factor.")]
35-
public float StabilityAverageDistanceWeight = 2.0f;
36-
37-
[Tooltip("Stability variance weight multiplier factor.")]
38-
public float StabilityVarianceWeight = 1.0f;
39-
4017
// Access the below public properties from the client class to consume stable values.
4118
public Vector3 StableHeadPosition { get; private set; }
4219
public Quaternion StableHeadRotation { get; private set; }
4320
public Ray StableHeadRay { get; private set; }
4421

45-
public struct GazeSample
46-
{
47-
public Vector3 Position;
48-
public Vector3 Direction;
49-
public float Timestamp;
50-
};
22+
/// <summary>
23+
/// These classes do the work of calculating standard deviation and averages for the gaze
24+
/// position and direction.
25+
/// </summary>
26+
private VectorRollingStatistics positionRollingStats = new VectorRollingStatistics();
27+
private VectorRollingStatistics directionRollingStats = new VectorRollingStatistics();
5128

52-
private LinkedList<GazeSample> stabilitySamples = new LinkedList<GazeSample>();
29+
/// <summary>
30+
/// These are the tunable parameters.
31+
/// </summary>
32+
// If the standard deviation is above these values we reset and stop stabalizing
33+
private const float positionStandardDeviationReset = 0.2f;
34+
private const float directionStandardDeviationReset = 0.1f;
5335

54-
private Vector3 gazePosition;
55-
private Vector3 gazeDirection;
36+
// We must have at least this many samples with a standard deviation below the above constants to stabalize
37+
private const int minimumSamplesRequiredToStabalize = 30;
5638

57-
// Most recent calculated instability values.
58-
private float gazePositionInstability;
59-
private float gazeDirectionInstability;
39+
// When not stabalizing this is the 'lerp' applied to the position and direction of the gaze to smooth it over time.
40+
private const float unstabalizedLerpFactor = 0.3f;
6041

61-
private bool gravityPointExists = false;
62-
private Vector3 gravityWellPosition;
63-
private Vector3 gravityWellDirection;
42+
// When stabalizing we will use the standard deviation of the position and direction to create the lerp value.
43+
// By default this value will be low and the cursor will be too sluggish, so we 'boost' it by this value.
44+
private const float stabalizedLerpBoost = 10.0f;
6445

65-
// Transforms instability value into a modified drop off distance, modify with caution.
66-
private const float positionDestabilizationFactor = 0.02f;
67-
private const float directionDestabilizationFactor = 0.3f;
46+
private void Awake()
47+
{
48+
directionRollingStats.Init(StoredStabilitySamples);
49+
positionRollingStats.Init(StoredStabilitySamples);
50+
}
6851

6952
/// <summary>
7053
/// Updates the StableHeadPosition and StableHeadRotation based on GazeSample values.
@@ -74,138 +57,30 @@ public struct GazeSample
7457
/// <param name="rotation">Rotation value from a RaycastHit rotation.</param>
7558
public void UpdateHeadStability(Vector3 position, Quaternion rotation)
7659
{
77-
gazePosition = position;
78-
gazeDirection = rotation * Vector3.forward;
79-
80-
AddGazeSample(gazePosition, gazeDirection);
81-
82-
UpdateInstability(out gazePositionInstability, out gazeDirectionInstability);
83-
84-
// If we don't have a gravity point, just use the gaze position.
85-
if (!gravityPointExists)
86-
{
87-
gravityWellPosition = gazePosition;
88-
gravityWellDirection = gazeDirection;
89-
gravityPointExists = true;
90-
}
91-
92-
UpdateGravityWellPositionDirection();
93-
}
94-
95-
private void AddGazeSample(Vector3 positionSample, Vector3 directionSample)
96-
{
97-
// Record and save sample data.
98-
GazeSample newStabilitySample;
99-
newStabilitySample.Position = positionSample;
100-
newStabilitySample.Direction = directionSample;
101-
newStabilitySample.Timestamp = Time.time;
102-
103-
if (stabilitySamples != null)
104-
{
105-
// Remove from front items if we exceed stored samples.
106-
while (stabilitySamples.Count >= StoredStabilitySamples)
107-
{
108-
stabilitySamples.RemoveFirst();
109-
}
110-
111-
stabilitySamples.AddLast(newStabilitySample);
60+
Vector3 gazePosition = position;
61+
Vector3 gazeDirection = rotation * Vector3.forward;
62+
63+
positionRollingStats.AddSample(gazePosition);
64+
directionRollingStats.AddSample(gazeDirection);
65+
66+
float lerpPower = unstabalizedLerpFactor;
67+
if (positionRollingStats.ActualSampleCount > minimumSamplesRequiredToStabalize && // we have enough samples and...
68+
(positionRollingStats.CurrentStandardDeviation > positionStandardDeviationReset || // the standard deviation of positions is high or...
69+
directionRollingStats.CurrentStandardDeviation > directionStandardDeviationReset)) // the standard deviation of directions is high
70+
{
71+
// We've detected that the user's gaze is no longer fixed, so stop stabalizing so that gaze is responsive.
72+
//Debug.LogFormat("Reset {0} {1} {2} {3}", positionRollingStats.standardDeviation, positionRollingStats.standardDeviationsAway, directionRollignStats.standardDeviation, directionRollignStats.standardDeviationsAway);
73+
positionRollingStats.Reset();
74+
directionRollingStats.Reset();
11275
}
113-
}
114-
115-
private void UpdateInstability(out float positionInstability, out float directionInstability)
116-
{
117-
positionInstability = 0.0f;
118-
directionInstability = 0.0f;
119-
120-
// If we have zero or one sample, there is no instability to report.
121-
if (stabilitySamples.Count < 2)
122-
{
123-
return;
124-
}
125-
126-
GazeSample mostRecentSample = stabilitySamples.Last.Value;
127-
128-
float positionDeltaMin = float.MaxValue;
129-
float positionDeltaMax = float.MinValue;
130-
float positionDeltaMean = 0.0f;
131-
132-
float directionDeltaMin = float.MaxValue;
133-
float directionDeltaMax = float.MinValue;
134-
float directionDeltaMean = 0.0f;
135-
136-
float positionDelta = 0.0f;
137-
float directionDelta = 0.0f;
138-
139-
foreach (GazeSample sample in stabilitySamples)
140-
{
141-
if (sample.Timestamp == mostRecentSample.Timestamp)
142-
{
143-
continue;
144-
}
145-
146-
// Calculate difference between current sample and most recent sample.
147-
positionDelta = Vector3.Magnitude(sample.Position - mostRecentSample.Position);
148-
directionDelta = Vector3.Angle(sample.Direction, mostRecentSample.Direction) * Mathf.Deg2Rad;
149-
150-
// Update maximum, minimum and mean differences from most recent sample.
151-
positionDeltaMin = Mathf.Min(positionDelta, positionDeltaMin);
152-
positionDeltaMax = Mathf.Max(positionDelta, positionDeltaMax);
153-
154-
directionDeltaMin = Mathf.Min(directionDelta, directionDeltaMin);
155-
directionDeltaMax = Mathf.Max(directionDelta, directionDeltaMax);
156-
157-
positionDeltaMean += positionDelta;
158-
directionDeltaMean += directionDelta;
159-
}
160-
161-
positionDeltaMean = positionDeltaMean / (stabilitySamples.Count - 1);
162-
directionDeltaMean = directionDeltaMean / (stabilitySamples.Count - 1);
163-
164-
// Calculate stability value for Gaze position and direction. Note that stability values will be significantly different for position and
165-
// direction since the position value is based on values in meters while the direction stability is based on data in radians.
166-
positionInstability = StabilityVarianceWeight * (positionDeltaMax - positionDeltaMin) + StabilityAverageDistanceWeight * positionDeltaMean;
167-
directionInstability = StabilityVarianceWeight * (directionDeltaMax - directionDeltaMin) + StabilityAverageDistanceWeight * directionDeltaMean;
168-
}
169-
170-
private void UpdateGravityWellPositionDirection()
171-
{
172-
float stabilityModifiedPositionDropOffDistance;
173-
float stabilityModifiedDirectionDropOffDistance;
174-
float normalizedGazeToGravityWellPosition;
175-
float normalizedGazeToGravityWellDirection;
176-
177-
// Modify effective size of well based on gaze stability.
178-
stabilityModifiedPositionDropOffDistance = Mathf.Max(0.0f, PositionDropOffRadius - (gazePositionInstability * positionDestabilizationFactor));
179-
stabilityModifiedDirectionDropOffDistance = Mathf.Max(0.0f, DirectionDropOffRadius - (gazeDirectionInstability * directionDestabilizationFactor));
180-
181-
// Determine how far away from the well the gaze is, if that distance is zero push the normalized value above 1.0 to
182-
// force a gravity well position update.
183-
normalizedGazeToGravityWellPosition = 2.0f;
184-
if (stabilityModifiedPositionDropOffDistance > 0.0f)
185-
{
186-
normalizedGazeToGravityWellPosition = Vector3.Magnitude(gravityWellPosition - gazePosition) / stabilityModifiedPositionDropOffDistance;
187-
}
188-
189-
normalizedGazeToGravityWellDirection = 2.0f;
190-
if (stabilityModifiedDirectionDropOffDistance > 0.0f)
191-
{
192-
normalizedGazeToGravityWellDirection = Mathf.Acos(Vector3.Dot(gravityWellDirection, gazeDirection)) / stabilityModifiedDirectionDropOffDistance;
193-
}
194-
195-
// Move gravity well with Gaze if necessary.
196-
if (normalizedGazeToGravityWellPosition > 1.0f)
197-
{
198-
gravityWellPosition = gazePosition - Vector3.Normalize(gazePosition - gravityWellPosition) * stabilityModifiedPositionDropOffDistance;
199-
}
200-
201-
if (normalizedGazeToGravityWellDirection > 1.0f)
76+
else if (positionRollingStats.ActualSampleCount > minimumSamplesRequiredToStabalize)
20277
{
203-
gravityWellDirection = Vector3.Normalize(gazeDirection - Vector3.Normalize(gazeDirection - gravityWellDirection) * stabilityModifiedDirectionDropOffDistance);
78+
// We've detected that the user's gaze is fairly fixed, so start stabalizing. The more fixed the gaze the less the cursor will move.
79+
lerpPower = stabalizedLerpBoost * (positionRollingStats.CurrentStandardDeviation + directionRollingStats.CurrentStandardDeviation);
20480
}
20581

206-
// Adjust direction and position towards gravity well based on configurable strengths.
207-
StableHeadPosition = Vector3.Lerp(gazePosition, gravityWellPosition, PositionStrength);
208-
StableHeadRotation = Quaternion.LookRotation(Vector3.Lerp(gazeDirection, gravityWellDirection, DirectionStrength));
82+
StableHeadPosition = Vector3.Lerp(StableHeadPosition, gazePosition, lerpPower);
83+
StableHeadRotation = Quaternion.LookRotation(Vector3.Lerp(StableHeadRotation * Vector3.forward, gazeDirection, lerpPower));
20984
StableHeadRay = new Ray(StableHeadPosition, StableHeadRotation * Vector3.forward);
21085
}
21186
}

‎Assets/HoloToolkit/Utilities/Scripts/RollingStatistics.cs‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
// Copyright (c) Microsoft Corporation. All rights reserved.
2+
// Licensed under the MIT License. See LICENSE in the project root for license information.
3+
using UnityEngine;
4+
5+
namespace HoloToolkit.Unity
6+
{
7+
public class VectorRollingStatistics
8+
{
9+
/// <summary>
10+
/// Current standard deviation of the positions of the vectors
11+
/// </summary>
12+
public float CurrentStandardDeviation;
13+
14+
/// <summary>
15+
/// Difference to standardDeviation when the latest sample was added.
16+
/// </summary>
17+
public float StandardDeviationDeltaAfterLatestSample;
18+
19+
/// <summary>
20+
/// How many standard deviations the latest sample was away.
21+
/// </summary>
22+
public float StandardDeviationsAwayOfLatestSample;
23+
24+
/// <summary>
25+
/// The average position.
26+
/// </summary>
27+
public Vector3 Average;
28+
29+
/// <summary>
30+
/// The number of samples in the current set (may be 0 - maxSamples)
31+
/// </summary>
32+
public float ActualSampleCount;
33+
34+
/// <summary>
35+
/// Keeps track of the index into the sample list for
36+
/// the rolling average.
37+
/// </summary>
38+
private int currentSampleIndex;
39+
40+
/// <summary>
41+
/// An array of samples for calculating standard deviation
42+
/// </summary>
43+
private Vector3[] samples;
44+
45+
/// <summary>
46+
/// The sum of all of the samples.
47+
/// </summary>
48+
private Vector3 cumulativeFrame;
49+
50+
/// <summary>
51+
/// The sum of all of the samples squared
52+
/// </summary>
53+
private Vector3 cumulativeFrameSquared;
54+
55+
/// <summary>
56+
/// The total number of samples taken.
57+
/// </summary>
58+
private int cumulativeFrameSamples;
59+
60+
/// <summary>
61+
/// The maximum number of samples to include in
62+
/// the average and standard deviation calculations.
63+
/// </summary>
64+
private int maxSamples;
65+
66+
/// <summary>
67+
/// Initialize the rolling stats.
68+
/// </summary>
69+
/// <param name="sampleCount"></param>
70+
public void Init(int sampleCount)
71+
{
72+
maxSamples = sampleCount;
73+
samples = new Vector3[sampleCount];
74+
Reset();
75+
}
76+
77+
/// <summary>
78+
/// Resets the stats to zero.
79+
/// </summary>
80+
public void Reset()
81+
{
82+
currentSampleIndex = 0;
83+
ActualSampleCount = 0;
84+
cumulativeFrame = Vector3.zero;
85+
cumulativeFrameSquared = Vector3.zero;
86+
cumulativeFrameSamples = 0;
87+
CurrentStandardDeviation = 0.0f;
88+
StandardDeviationDeltaAfterLatestSample = 0.0f;
89+
StandardDeviationsAwayOfLatestSample = 0.0f;
90+
Average = Vector3.zero;
91+
if (samples != null)
92+
{
93+
for (int index = 0; index < samples.Length; index++)
94+
{
95+
samples[index] = Vector3.zero;
96+
}
97+
}
98+
}
99+
100+
/// <summary>
101+
/// Adds a new sample to the sample list and updates the stats.
102+
/// </summary>
103+
/// <param name="value">The new sample to add</param>
104+
public void AddSample(Vector3 value)
105+
{
106+
if (maxSamples == 0)
107+
{
108+
return;
109+
}
110+
111+
// remove the old sample from our accumulation
112+
Vector3 oldSample = samples[currentSampleIndex];
113+
cumulativeFrame -= oldSample;
114+
cumulativeFrameSquared -= (oldSample.Mul(oldSample));
115+
116+
// Add the new sample to the accumulation
117+
samples[currentSampleIndex] = value;
118+
cumulativeFrame += value;
119+
cumulativeFrameSquared += value.Mul(value);
120+
121+
// Keep track of how many samples we have
122+
cumulativeFrameSamples++;
123+
ActualSampleCount = Mathf.Min(maxSamples, cumulativeFrameSamples);
124+
125+
// see how many standard deviations the current sample is from the previous average
126+
Vector3 deltaFromAverage = (Average - value);
127+
float oldStandardDeviation = CurrentStandardDeviation;
128+
StandardDeviationsAwayOfLatestSample = oldStandardDeviation == 0 ? 0 : (deltaFromAverage / oldStandardDeviation).magnitude;
129+
130+
// And calculate new averages and standard deviations
131+
// (note that calculating a standard deviation of a Vector3 might not
132+
// be done properly, but the logic is working for the gaze stabalization scenario)
133+
Average = cumulativeFrame / ActualSampleCount;
134+
float newStandardDev = Mathf.Sqrt((cumulativeFrameSquared / ActualSampleCount - Average.Mul(Average)).magnitude);
135+
StandardDeviationDeltaAfterLatestSample = oldStandardDeviation - newStandardDev;
136+
CurrentStandardDeviation = newStandardDev;
137+
138+
//
139+
// update the next list position
140+
currentSampleIndex = (currentSampleIndex + 1) % maxSamples;
141+
}
142+
}
143+
}

0 commit comments

Comments
 (0)