public KinectDriver(CameraSpacePoint up, CameraSpacePoint right, CameraSpacePoint down, CameraSpacePoint left, BodyFrameReader reader)
{
// Keep the calibration data
Up = up;
Right = right;
Down = down;
Left = left;
HorizontalThreshold = KinectHelper.ComputeHorizontalThreshold(up, right, down, left);
VerticalThreshold = KinectHelper.ComputeVerticalThreshold(up, right, down, left);
StopTracking = new ManualResetEvent(false);
// Initialize the virtual keyboard
Keyboard = new InputSimulator();
// Find the connected Kinect
KinectSensor kinect = KinectSensor.GetDefault();
// Create a space to store skeleton data
Bodies = new Body[kinect.BodyFrameSource.BodyCount];
// Setup the kinect to read the skeletons
reader.FrameArrived += ParseFrame;
}
static void Main(string[] args)
{
// Ready the Kinect
KinectSensor sensor = KinectSensor.GetDefault();
sensor.Open();
// Start Calibration
Console.WriteLine("Stand on the left and right arrows, facing forward.\n");
// Calibrate Left/Right
KinectCalibrator calibrator = new KinectCalibrator();
Debug.WriteLine("Left/Right: {0}", DateTime.Now);
CameraSpacePoint leftArrow = calibrator.LeftFoot;
CameraSpacePoint rightArrow = calibrator.RightFoot;
DepthSpacePoint leftDepth = sensor.CoordinateMapper.MapCameraPointToDepthSpace(leftArrow);
DepthSpacePoint rightDepth = sensor.CoordinateMapper.MapCameraPointToDepthSpace(rightArrow);
Console.WriteLine("Left Arrow: X:{0}, Y:{1}, Z:{2}", leftArrow.X, leftArrow.Y, leftArrow.Z);
Console.WriteLine("Right Arrow: X:{0}, Y:{1}, Z:{2}", rightArrow.X, rightArrow.Y, rightArrow.Z);
//CameraSpacePoint center = calibrator.Center;
//Console.WriteLine("Center: X:{0}, Y:{1}, Z:{2}", center.X, center.Y, center.Z);
Console.WriteLine("\nLeft + Right Calibrated!\n");
Console.WriteLine("Turn 90 Degrees CW and stand on the up and down arrows.");
// Give them some time to figure out what CW stands for
Thread.Sleep(2500);
// Calibrate Up/Down
calibrator = new KinectCalibrator();
Debug.WriteLine("Up/Down: {0}", DateTime.Now);
// TODO: Figure out which direction they turned. Right now it assumes they turned 90 degrees clockwise
// NOTE: It does ask them to turn 90 degrees CW, but it shouldn't have to
// NOTE: It works a lot better when you're facing the kinect
CameraSpacePoint upArrow = calibrator.LeftFoot;
CameraSpacePoint downArrow = calibrator.RightFoot;
DepthSpacePoint upDepth = sensor.CoordinateMapper.MapCameraPointToDepthSpace(upArrow);
DepthSpacePoint downDepth = sensor.CoordinateMapper.MapCameraPointToDepthSpace(downArrow);
Console.WriteLine("Up Arrow: X:{0}, Y:{1}, Z:{2}", upArrow.X, upArrow.Y, upArrow.Z);
Console.WriteLine("Right Foot: X:{0}, Y:{1}, Z:{2}", downArrow.X, downArrow.Y, downArrow.Z);
//center = calibrator.Center;
//Console.WriteLine("Center: X:{0}, Y:{1}, Z:{2}", center.X, center.Y, center.Z);
Console.WriteLine("\nUp + Down Calibrated!\n");
// Compute where the center should be
CameraSpacePoint center = KinectHelper.Average2CameraSpacePoints(upArrow, rightArrow);
center = KinectHelper.Average2CameraSpacePoints(center, downArrow, 3);
center = KinectHelper.Average2CameraSpacePoints(center, leftArrow, 4);
DepthSpacePoint centerDepth = sensor.CoordinateMapper.MapCameraPointToDepthSpace(center);
//KinectDepthDriver depthDriver = new KinectDepthDriver(upDepth, rightDepth, downDepth, leftDepth, centerDepth);
//depthDriver.Calibrate();
//depthDriver.Start();
KinectDriver driver = new KinectDriver(upArrow, rightArrow, downArrow, leftArrow, calibrator.BodyFrameReader);
driver.Start();
}
private void ParseFrame(object sender, BodyFrameArrivedEventArgs bodyFrameArrivedEventArgs)
{
BodyFrame frame = bodyFrameArrivedEventArgs.FrameReference.AcquireFrame();
using (frame)
{
// Check if frame is usable
if (frame != null)
{
frame.GetAndRefreshBodyData(Bodies);
foreach (Body body in Bodies)
{
// Check if the Kinect found a skeleton
if (body.IsTracked)
{
CameraSpacePoint center = body.Joints[JointType.SpineBase].Position;
CameraSpacePoint leftFoot = body.Joints[JointType.FootLeft].Position;
CameraSpacePoint rightFoot = body.Joints[JointType.FootRight].Position;
Debug.WriteLine("{3} Left Foot: {0}, {1}, {2}", leftFoot.X, leftFoot.Y, leftFoot.Z, body.TrackingId);
Debug.WriteLine("{3} Right Foot: {0}, {1}, {2}", rightFoot.X, rightFoot.Y, rightFoot.Z, body.TrackingId);
Debug.WriteLine("{3} Center: {0}, {1}, {2}", center.X, center.Y, center.Z, body.TrackingId);
//KinectHelper.GetArrowsPressed(Up, Right, Down, Left, leftFoot, rightFoot, HorizontalThreshold, VerticalThreshold);
KinectHelper.Arrows arrows = KinectHelper.GetArrowsPressed(Up, Right, Down, Left, leftFoot, rightFoot, 0.075, 0.025);
if (arrows.HasFlag(KinectHelper.Arrows.Up) && !LastArrows.HasFlag(KinectHelper.Arrows.Up))
{
InputManager.Keyboard.KeyDown(Keys.Up);
}
else if (!arrows.HasFlag(KinectHelper.Arrows.Up) && LastArrows.HasFlag(KinectHelper.Arrows.Up))
{
InputManager.Keyboard.KeyUp(Keys.Up);
}
if (arrows.HasFlag(KinectHelper.Arrows.Right) && !LastArrows.HasFlag(KinectHelper.Arrows.Right))
{
InputManager.Keyboard.KeyDown(Keys.Right);
}
else if (!arrows.HasFlag(KinectHelper.Arrows.Right) && LastArrows.HasFlag(KinectHelper.Arrows.Right))
{
InputManager.Keyboard.KeyUp(Keys.Right);
}
if (arrows.HasFlag(KinectHelper.Arrows.Down) && !LastArrows.HasFlag(KinectHelper.Arrows.Down))
{
InputManager.Keyboard.KeyDown(Keys.Down);
}
else if (!arrows.HasFlag(KinectHelper.Arrows.Down) && LastArrows.HasFlag(KinectHelper.Arrows.Down))
{
InputManager.Keyboard.KeyUp(Keys.Down);
}
if (arrows.HasFlag(KinectHelper.Arrows.Left) && !LastArrows.HasFlag(KinectHelper.Arrows.Left))
{
InputManager.Keyboard.KeyDown(Keys.Left);
}
else if (!arrows.HasFlag(KinectHelper.Arrows.Left) && LastArrows.HasFlag(KinectHelper.Arrows.Left))
{
InputManager.Keyboard.KeyUp(Keys.Left);
}
LastArrows = arrows;
}
}
}
}
}
private void ParseFrame(object sender, BodyFrameArrivedEventArgs bodyFrameArrivedEventArgs)
{
BodyFrame frame = bodyFrameArrivedEventArgs.FrameReference.AcquireFrame();
using (frame)
{
// Check if frame is usable
if (frame != null)
{
frame.GetAndRefreshBodyData(Bodies);
foreach (Body body in Bodies)
{
// Check if the Kinect found a skeleton
if (body.IsTracked)
{
if (TrackingID == 0)
{
// Verify there is only one person in front of the Kinect to start
foreach (Body body2 in Bodies)
{
if (body2 == body)
{
// Same body
continue;
}
if (body2.IsTracked)
{
// Multiple users in the first frame, don't start calibrating until one moves out
break;
}
}
// Start tracking this body!
TrackingID = body.TrackingId;
}
else if (TrackingID != body.TrackingId)
{
// Either lost tracking, or there are two people in front of the Kinect
bool verifiedStillVisible = false;
foreach (Body body2 in Bodies)
{
if (body2 == body)
{
// Same body
continue;
}
if (body2.IsTracked && body2.TrackingId == TrackingID)
{
// There's multiple people
verifiedStillVisible = true;
break;
}
}
if (!verifiedStillVisible)
{
// Restart calibration
FrameNo = 0;
TrackingID = 0;
break;
}
}
if (LeftFootPoints.Count < NumberOfFramesToAverage)
{
// Add the frame without doing any calculations
body.Joints[JointType.FootLeft].
LeftFootPoints.Enqueue(body.Joints[JointType.FootLeft].Position);
RightFootPoints.Enqueue(body.Joints[JointType.FootRight].Position);
}
else if (LeftFootPoints.Count == NumberOfFramesToAverage)
{
// See if the last 5 seconds are somewhat the same
// Average the left foot
CameraSpacePoint firstPoint = LeftFootPoints.Peek();
CameraSpacePoint averagePoint = firstPoint;
uint i = 2;
foreach (CameraSpacePoint leftFootPoint in LeftFootPoints)
{
averagePoint = KinectHelper.Average2CameraSpacePoints(averagePoint, leftFootPoint, i);
i++;
}
float error = KinectHelper.MaxDifference(averagePoint, firstPoint);
if (error > MaximumError)
{
Debug.WriteLine("Left Foot Error: {0}", error);
Debug.WriteLine("Left Foot Average: {0}, {1}, {2}", averagePoint.X, averagePoint.Y, averagePoint.Z);
Debug.WriteLine("Left Foot Last: {0}, {1}, {2}", firstPoint.X, firstPoint.Y, firstPoint.Z);
// Too much error detected, remove half of the frames
for (int j = 0; j < NumberOfFramesToAverage / 2; j++)
{
LeftFootPoints.Dequeue();
RightFootPoints.Dequeue();
}
break;
}
LeftFoot = averagePoint;
// Check the right foot
firstPoint = RightFootPoints.Peek();
averagePoint = firstPoint;
i = 2;
foreach (CameraSpacePoint rightFootPoint in RightFootPoints)
{
averagePoint = KinectHelper.Average2CameraSpacePoints(averagePoint, rightFootPoint, i);
i++;
}
error = KinectHelper.MaxDifference(averagePoint, firstPoint);
if (error > MaximumError)
{
Debug.WriteLine("Right Foot Error: {0}", error);
Debug.WriteLine("Right Foot Average: {0}, {1}, {2}", averagePoint.X, averagePoint.Y, averagePoint.Z);
Debug.WriteLine("Right Foot Last: {0}, {1}, {2}", firstPoint.X, firstPoint.Y, firstPoint.Z);
// Too much error detected, remove half of the frames
for (int j = 0; j < NumberOfFramesToAverage / 2; j++)
{
LeftFootPoints.Dequeue();
RightFootPoints.Dequeue();
}
break;
}
RightFoot = averagePoint;
Center = body.Joints[JointType.SpineBase].Position;
// Calibration is within error!
CalibrationFinished.Set();
}
}
}
}
}
}
