private void ProcessHandState(SmoothedBody<KalmanSmoother> aTracked, Point aHandLocation)
{
HandState trackBodyHandState = (TrackingLeftHand ? aTracked.HandLeftState : aTracked.HandRightState);
//save the current state in the counter
_handStateCounter.Add(trackBodyHandState);
//check if the hand just closed
if (_lastConfirmedHandState == HandState.Open && trackBodyHandState == HandState.Closed)
{
//see if the hand is been closed for long enough
if (_handStateCounter.Count >= this.MinimumClosedStatesAfterOpen)
{
//if so, send out event message
DidDetectHandStateChange(_lastConfirmedHandState, trackBodyHandState, aHandLocation);
//set last confirmed state
_lastConfirmedHandState = HandState.Closed;
}
}
//check if the hand is been open for long enough
else if (_lastConfirmedHandState == HandState.Closed && trackBodyHandState == HandState.Open)
{
//see if there is enough data for a hand open message
if (_handStateCounter.Count >= this.MinimumOpenStatesAfterClose)
{
//if so, send out event message
DidDetectHandStateChange(_lastConfirmedHandState, trackBodyHandState, aHandLocation);
//set last confirmed state
_lastConfirmedHandState = HandState.Open;
}
}
else if (_lastConfirmedHandState == HandState.NotTracked)
{
}
}
public void KinectManagerDidGetUpdatedBodyData(KinectManager aManager, SmoothedBody<KalmanSmoother>[] aBodies)
{
SmoothedBody<KalmanSmoother> tracked = aBodies.Where(b => b.TrackingId == this._kinectManager.CurrentlyTrackingId).FirstOrDefault();
if (tracked != null && tracked.TrackingId != 0)
{
Dictionary<JointType, Point> jointPoints = this._kinectManager.HandManager.ConvertJointsToDepthSpace(tracked);
//check if we are already tracking a hand
bool beganTracking = false;
if (this._lastConfirmedHandState == HandState.NotTracked || !HandIsInCorrectPosition(jointPoints, this.TrackingLeftHand))
{
if (this.ChooseHand(jointPoints))
{
beganTracking = true;
_lastConfirmedHandState = HandState.Open;
}
}
//Process hand loc
Point currentHandLoc = ProcessHandLocation(jointPoints);
//if began tracking, send out event
if (beganTracking)
{
HandStateChangeEvent beganTrackingEvent = new HandStateChangeEvent(HandStateChangeType.BeganTracking, currentHandLoc);
this.NotifyHandStateChangeListenersOfEvent(beganTrackingEvent);
}
//Process Hand State
ProcessHandState(tracked, currentHandLoc);
}
else
{
_lastConfirmedHandState = HandState.NotTracked;
_handStateCounter.Reset();
_scaledHandLocationFilter.Set(new Point(this.HandCoordRangeX / 2, this.HandCoordRangeY / 2));
}
}
private Dictionary<JointType, Point> ConvertJointsToDepthSpace(SmoothedBody<KalmanSmoother> aBody)
{
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in aBody.Joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = aBody.Joints[jointType].Position;
if (position.Z < 0)
{
position.Z = INFERRED_Z_POSITION_CLAMP;
}
DepthSpacePoint depthSpacePoint = this._coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X / _depthFrameWidth, depthSpacePoint.Y / _depthFrameHeight);
}
return jointPoints;
}
private void InitBody(float aJitterRadius, float aMeasurementUncertainty)
{
log.Info("InitBody");
KalmanSmoothingParameters smoothingParam = new KalmanSmoothingParameters();
smoothingParam.JitterRadius = aJitterRadius;
smoothingParam.MeasurementUncertainty = aMeasurementUncertainty;
_smoothedBodies = new SmoothedBody<KalmanSmoother>[this.KinectSensor.BodyFrameSource.BodyCount];
for (int i = 0; i < this.KinectSensor.BodyFrameSource.BodyCount; i++)
{
_smoothedBodies[i] = new SmoothedBody<KalmanSmoother>(smoothingParam);
}
// get the coordinate mapper
this.coordinateMapper = this.KinectSensor.CoordinateMapper;
// get total number of bodies from BodyFrameSource
this.bodies = new Body[this.KinectSensor.BodyFrameSource.BodyCount];
// sets total number of possible tracked bodies
this.bodyCount = this.KinectSensor.BodyFrameSource.BodyCount;
// open the reader for the body frames
this.bodyFrameReader = this.KinectSensor.BodyFrameSource.OpenReader();
// wire handler for frame arrival
this.bodyFrameReader.FrameArrived += this.Reader_BodyFrameArrived;
}
private void DrawClippedEdges(SmoothedBody<ExponentialSmoother> body, DrawingContext drawingContext)
#endif
{
FrameEdges clippedEdges = body.ClippedEdges;
if (clippedEdges.HasFlag(FrameEdges.Bottom))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, _displayHeight - KinectBodyHelper.ClipBoundsThickness, _displayWidth,
KinectBodyHelper.ClipBoundsThickness));
}
if (clippedEdges.HasFlag(FrameEdges.Top))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, _displayWidth, KinectBodyHelper.ClipBoundsThickness));
}
if (clippedEdges.HasFlag(FrameEdges.Left))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, KinectBodyHelper.ClipBoundsThickness, _displayHeight));
}
if (clippedEdges.HasFlag(FrameEdges.Right))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(_displayWidth - KinectBodyHelper.ClipBoundsThickness, 0,
KinectBodyHelper.ClipBoundsThickness, _displayHeight));
}
}
/// Draws indicators to show which edges are clipping body data
private void DrawClippedEdges(SmoothedBody<KalmanSmoother> body, DrawingContext drawingContext)
{
if (this.showClipping)
{
FrameEdges clippedEdges = body.ClippedEdges;
if (clippedEdges.HasFlag(FrameEdges.Bottom))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, this.displayHeight - ClipBoundsThickness, this.displayWidth, ClipBoundsThickness));
}
if (clippedEdges.HasFlag(FrameEdges.Top))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, this.displayWidth, ClipBoundsThickness));
}
if (clippedEdges.HasFlag(FrameEdges.Left))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, ClipBoundsThickness, this.displayHeight));
}
if (clippedEdges.HasFlag(FrameEdges.Right))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(this.displayWidth - ClipBoundsThickness, 0, ClipBoundsThickness, this.displayHeight));
}
}
}
private void DrawBodies(SmoothedBody<KalmanSmoother>[] aBodies)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
foreach (SmoothedBody<KalmanSmoother> body in aBodies)
{
if (body.TrackingId == this.kinectManager.CurrentlyTrackingId)
penIndex = 0;
else
penIndex = 1;
Pen drawPen = this.bodyColors[penIndex];
if (body.IsTracked)
{
this.DrawClippedEdges(body, dc);
var joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
ColorSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X * this._scale, depthSpacePoint.Y * this._scale);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
if (body != null && KinectManager.Default.CurrentlyTrackingBody != null && body.TrackingId == KinectManager.Default.CurrentlyTrackingBody.TrackingId)
{
if (KinectManager.Default.HandManager.TrackingLeftHand)
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
else
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
}
//draw hand rect
if (penIndex == 0 && this.ShouldDrawHandRect)
{
Rect handRect = this.kinectManager.HandManager.HandRect;
handRect.X *= this.displayWidth;
handRect.Y *= this.displayHeight;
handRect.Width *= this.displayWidth;
handRect.Height *= this.displayHeight;
dc.DrawRectangle(this.trackedJointBrush, drawPen, handRect);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
public void KinectManagerDidGetUpdatedBodyData(KinectManager aManager, SmoothedBody<KalmanSmoother>[] aBodies)
{
//update on UI thread
Dispatcher.InvokeAsync((Action)delegate()
{
DrawBodies(aBodies);
});
}
