/// <summary>
/// Update (and filter) the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public override void Update(IJoint joint)
{
_trackingState = joint.TrackingState;
if (_trackingState == TrackingState.NotTracked)
{
_position = new CameraSpacePoint();
return;
}
if (_init)
{
_init = false;
_filteredPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
_positionVariance = new Vector3(1000f);
_velocityVariance = new Vector3(1000f);
}
else
{
var oldPosition = _filteredPosition;
var oldVelocity = _velocity;
// Predict
_filteredPosition = _filteredPosition + _velocity;
_velocity = _velocity + _velocityDelta;
_positionVariance = _positionVariance + _measurementUncertainty;
_velocityVariance = _velocityVariance + _measurementUncertainty;
// Update
var reportedPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
// ... but first filter for jitter ...
var positionDelta = reportedPosition - oldPosition;
var differenceLength = Math.Abs((float)positionDelta.Length());
var jitterRadiusMod = joint.TrackingState == TrackingState.Tracked ? _jitterRadius : _jitterRadius * 2;
if (differenceLength <= jitterRadiusMod)
{
reportedPosition = (reportedPosition * (differenceLength / jitterRadiusMod)) +
(oldPosition * (1.0f - (differenceLength / jitterRadiusMod)));
}
_filteredPosition = (_positionVariance * reportedPosition + _measurementUncertainty * _filteredPosition) / (_measurementUncertainty + _positionVariance);
_velocity = (_velocityVariance * (reportedPosition - oldPosition) + _measurementUncertainty * _velocity) / (_measurementUncertainty + _velocityVariance);
_velocityDelta = _velocity - oldVelocity;
_positionVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _positionVariance));
_velocityVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _velocityVariance));
}
_position.X = _filteredPosition.X;
_position.Y = _filteredPosition.Y;
_position.Z = _filteredPosition.Z;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
}
/// <summary>
/// Update (and filter) the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public override void Update(IJoint joint)
{
_trackingState = joint.TrackingState;
if (_trackingState == TrackingState.NotTracked)
{
_position = new CameraSpacePoint();
Reset();
return;
}
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
var jitterRadius = _jitterRadius;
var maxDeviationRadius = _maxDeviationRadius;
if (joint.TrackingState != TrackingState.Tracked)
{
jitterRadius *= 2.0f;
maxDeviationRadius *= 2.0f;
}
Vector3 filteredPosition,
positionDelta,
trend;
float diffVal;
var reportedPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
var prevFilteredPosition = _history.FilteredPosition;
var prevTrend = _history.Trend;
var prevRawPosition = _history.ReportedPosition;
// If joint is invalid, reset the filter
if (reportedPosition.X == 0 && reportedPosition.Y == 0 && reportedPosition.Z == 0)
{
Reset();
}
// Initial start values
if (this._history.FrameCount == 0)
{
filteredPosition = reportedPosition;
trend = new Vector3();
}
else if (this._history.FrameCount == 1)
{
filteredPosition = (reportedPosition + prevRawPosition) * 0.5f;
positionDelta = filteredPosition - prevFilteredPosition;
trend = (positionDelta * _correction) + (prevTrend * (1.0f - _correction));
}
else
{
// First apply jitter filter
positionDelta = reportedPosition - prevFilteredPosition;
diffVal = Math.Abs(positionDelta.Length());
if (diffVal <= jitterRadius)
{
filteredPosition = (reportedPosition * (diffVal / jitterRadius)) +
(prevFilteredPosition * (1.0f - (diffVal / jitterRadius)));
}
else
{
filteredPosition = reportedPosition;
}
// Now the double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - _smoothing)) +
((prevFilteredPosition + prevTrend) * _smoothing);
positionDelta = filteredPosition - prevFilteredPosition;
trend = (positionDelta * _correction) + (prevTrend * (1.0f - _correction));
}
// Predict into the future to reduce latency
var predictedPosition = filteredPosition + (trend * _prediction);
// Check that we are not too far away from raw data
positionDelta = predictedPosition - reportedPosition;
diffVal = Math.Abs(positionDelta.Length());
if (diffVal > maxDeviationRadius)
{
predictedPosition = (predictedPosition * (maxDeviationRadius / diffVal)) +
(reportedPosition * (1.0f - (maxDeviationRadius / diffVal)));
}
// Save the data from this frame
this._history.ReportedPosition = reportedPosition;
this._history.FilteredPosition = filteredPosition;
this._history.Trend = trend;
this._history.FrameCount++;
// Set the filtered data back into the joint
_position.X = predictedPosition.X;
_position.Y = predictedPosition.Y;
_position.Z = predictedPosition.Z;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
}
/// <summary>
/// Create a new <c>CustomJoint</c> object based on the supplied
/// <c>JointType</c> value.
/// </summary>
public CustomJoint(JointType type)
{
_jointType = type;
_position = new CameraSpacePoint();
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = TrackingState.NotTracked;
}
/// <summary>
/// Update the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public virtual void Update(IJoint joint)
{
if (this.JointType != joint.JointType)
throw new Exception("Cannot Update with Joint of a different Type");
_position = joint.Position;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = joint.TrackingState;
}