예제 #1
0
        /// <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();
        }
예제 #2
0
 /// <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;
 }
예제 #3
0
        /// <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();
        }
예제 #4
0
        /// <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;
        }