public static DepthPoint operator *(DepthPoint firstPoint, double constant)
{
DepthPoint multPoint = new DepthPoint((int)(firstPoint.x * constant),
(int)(firstPoint.y * constant),
(long)(firstPoint.depth * constant));
return multPoint;
}
/// <summary>
///
/// </summary>
public ReferenceFrame()
{
if (armLengths == null)
{
armLengths = new double[MINDATAPOINTSREQ];
}
if (torsoPositions == null)
{
torsoPositions = new DepthPoint[MINDATAPOINTSREQ];
for (int i = 0; i < MINDATAPOINTSREQ; i++)
{
torsoPositions[i] = new DepthPoint();
}
}
this.torsoPosition = new DepthPoint();
leftWristPoints = new Queue<DepthPoint>();
rightWristPoints = new Queue<DepthPoint>();
leftShoulderPoints = new Queue<DepthPoint>();
rightShoulderPoints = new Queue<DepthPoint>();
totalArmPoints = 0;
totalTorsoPoints = 0;
shoulderCenterPoints = new Queue<DepthPoint>();
spinePoints = new Queue<DepthPoint>();
hipCenterPoints = new Queue<DepthPoint>();
shoulderCenterPointsSupplemental = new Queue<DepthPoint>();
spinePointsSupplemental = new Queue<DepthPoint>();
hipCenterPointsSupplemental = new Queue<DepthPoint>();
}
private const double TORSO_LEARNING_ALPHA = 0.1; // torso learning alpha for after we have MAX_POINTS_IN_READING
/// <summary>
///
/// </summary>
public ReferenceFrame()
{
if (armLengths == null)
{
armLengths = new double[MINDATAPOINTSREQ];
}
if (torsoPositions == null)
{
torsoPositions = new DepthPoint[MINDATAPOINTSREQ];
for (int i = 0; i < MINDATAPOINTSREQ; i++)
{
torsoPositions[i] = new DepthPoint();
}
}
this.torsoPosition = new DepthPoint();
leftWristPoints = new Queue <DepthPoint>();
rightWristPoints = new Queue <DepthPoint>();
leftShoulderPoints = new Queue <DepthPoint>();
rightShoulderPoints = new Queue <DepthPoint>();
totalArmPoints = 0;
totalTorsoPoints = 0;
shoulderCenterPoints = new Queue <DepthPoint>();
spinePoints = new Queue <DepthPoint>();
hipCenterPoints = new Queue <DepthPoint>();
shoulderCenterPointsSupplemental = new Queue <DepthPoint>();
spinePointsSupplemental = new Queue <DepthPoint>();
hipCenterPointsSupplemental = new Queue <DepthPoint>();
}
public static DepthPoint operator *(DepthPoint firstPoint, double constant)
{
DepthPoint multPoint = new DepthPoint((int)(firstPoint.x * constant),
(int)(firstPoint.y * constant),
(long)(firstPoint.depth * constant));
return(multPoint);
}
/// <summary>
///
/// </summary>
/// <param name="leftWristJoint"></param>
/// <param name="leftShoulderJoint"></param>
/// <param name="rightWristJoint"></param>
/// <param name="rightShoulderJoint"></param>
public void computeArmLengthPixels(DepthPoint leftWristPoint, DepthPoint leftShoulderPoint, DepthPoint rightWristPoint, DepthPoint rightShoulderPoint)
{
totalArmPoints++;
if (totalArmPoints > MIN_POINTS_BEFORE_READING && leftWristPoints.Count < MAX_POINTS_IN_READING)
{
leftWristPoints.Enqueue(leftWristPoint);
leftShoulderPoints.Enqueue(leftShoulderPoint);
rightWristPoints.Enqueue(rightWristPoint);
rightShoulderPoints.Enqueue(rightShoulderPoint);
}
int leftArmX = leftWristPoint.x - leftShoulderPoint.x;
int leftArmY = leftWristPoint.y - leftShoulderPoint.y;
//int leftArmZ = leftWristPoint.Position.Z - leftShoulderPoint.Position.Z;
int rightArmX = rightWristPoint.x - rightShoulderPoint.x;
int rightArmY = rightWristPoint.y - rightShoulderPoint.y;
//int rightArmZ = rightWristPoint.Position.Z - rightShoulderPoint.Position.Z;
double leftArm = Math.Sqrt(leftArmX * leftArmX + leftArmY * leftArmY /*+ leftArmZ * leftArmZ*/);
double rightArm = Math.Sqrt(rightArmX * rightArmX + rightArmY * rightArmY /*+ rightArmZ * rightArmZ*/);
this.armLengthPixels = (int)((leftArm + rightArm) / 2.0);
/*
* The length of both arms changes depending on the location of the person due to noise in data
* and also inaccuracy of depth cammera at too far and too close postions.
* so we collect at least 50 data points of the four joint we need and calculate the avregae
*
*/
/*
* if (armCounter < MINDATAPOINTSREQ)
* {
* armLengths[armCounter] = (leftArm + rightArm) / 2;
* armCounter++;
* }
* else
* {
* for (int i = 0; i < armCounter; i++)
* {
* this.armLength += armLengths[i];
* }
* //this.armLength /= (armCounter);
* this.armLength = (leftArm + rightArm) / 2;
* }*/
}
public DepthPoint getNearestPoint(DepthPoint other)
{
DepthPoint nearestPoint = null;
foreach (DepthPoint pt in points)
{
if (nearestPoint == null)
{
nearestPoint = pt;
}
else
{
if (distance(pt, other) < distance(nearestPoint, other))
{
nearestPoint = pt;
}
}
}
return(nearestPoint);
}
public static double EuclideanDistance2d(DepthPoint p1, DepthPoint p2)
{
return(Math.Sqrt((p1.x - p2.x) * (p1.x - p2.x) +
(p1.y - p2.y) * (p1.y - p2.y)));
}
/// <summary>
///
/// </summary>
/// <param name="shoulderCenter"></param>
/// <param name="spine"></param>
/// <param name="hipCenter"></param>
internal void computerTorsoDepth(DepthPoint shoulderCenter, DepthPoint spine, DepthPoint hipCenter)
{
totalTorsoPoints++;
/*
* if (shoulderCenterPoints.Count == MAX_POINTS_IN_READING)
* {
* shoulderCenterPointsSupplemental.Enqueue(shoulderCenter);
* hipCenterPointsSupplemental.Enqueue(hipCenter);
* spinePointsSupplemental.Enqueue(spine);
* }
*/
while (shoulderCenterPoints.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPoints.Dequeue();
hipCenterPoints.Dequeue();
spinePoints.Dequeue();
}
while (shoulderCenterPointsSupplemental.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPointsSupplemental.Dequeue();
hipCenterPointsSupplemental.Dequeue();
spinePointsSupplemental.Dequeue();
}
if (totalTorsoPoints > MIN_POINTS_BEFORE_READING && shoulderCenterPoints.Count < MAX_POINTS_IN_READING)
{
shoulderCenterPoints.Enqueue(shoulderCenter);
hipCenterPoints.Enqueue(hipCenter);
spinePoints.Enqueue(spine);
}
else
{
shoulderCenterPointsSupplemental.Enqueue(shoulderCenter);
hipCenterPointsSupplemental.Enqueue(hipCenter);
spinePointsSupplemental.Enqueue(spine);
}
// Alpha average between the supplemental points and our stored average torso pos
if (shoulderCenterPointsSupplemental.Count > 30 && _avgTorsoPosition != null)
{
List <double> shoulderCenterX = new List <double>();
List <double> shoulderCenterY = new List <double>();
List <double> shoulderCenterZ = new List <double>();
List <double> hipCenterX = new List <double>();
List <double> hipCenterY = new List <double>();
List <double> hipCenterZ = new List <double>();
List <double> spineX = new List <double>();
List <double> spineY = new List <double>();
List <double> spineZ = new List <double>();
for (int i = 0; i < shoulderCenterPoints.Count; i++)
{
shoulderCenterX.Add(shoulderCenterPoints.ElementAt(i).x);
shoulderCenterY.Add(shoulderCenterPoints.ElementAt(i).y);
shoulderCenterZ.Add(shoulderCenterPoints.ElementAt(i).depth);
hipCenterX.Add(hipCenterPoints.ElementAt(i).x);
hipCenterY.Add(hipCenterPoints.ElementAt(i).y);
hipCenterZ.Add(hipCenterPoints.ElementAt(i).depth);
spineX.Add(spinePoints.ElementAt(i).x);
spineY.Add(spinePoints.ElementAt(i).y);
spineZ.Add(spinePoints.ElementAt(i).depth);
}
shoulderCenterX.Sort();
shoulderCenterY.Sort();
shoulderCenterZ.Sort();
hipCenterX.Sort();
hipCenterY.Sort();
hipCenterZ.Sort();
spineX.Sort();
spineY.Sort();
spineZ.Sort();
int numToRemove = shoulderCenterPoints.Count / 4;
for (int i = 0; i < numToRemove; i++)
{
shoulderCenterX.RemoveAt(0);
shoulderCenterY.RemoveAt(0);
shoulderCenterZ.RemoveAt(0);
hipCenterX.RemoveAt(0);
hipCenterY.RemoveAt(0);
hipCenterZ.RemoveAt(0);
spineX.RemoveAt(0);
spineY.RemoveAt(0);
spineZ.RemoveAt(0);
shoulderCenterX.RemoveAt(shoulderCenterX.Count - 1);
shoulderCenterY.RemoveAt(shoulderCenterY.Count - 1);
shoulderCenterZ.RemoveAt(shoulderCenterZ.Count - 1);
hipCenterX.RemoveAt(hipCenterX.Count - 1);
hipCenterY.RemoveAt(hipCenterZ.Count - 1);
hipCenterZ.RemoveAt(hipCenterZ.Count - 1);
spineX.RemoveAt(spineX.Count - 1);
spineY.RemoveAt(spineY.Count - 1);
spineZ.RemoveAt(spineZ.Count - 1);
}
double shoulderCenterSumX = shoulderCenterX.Sum();
double shoulderCenterSumY = shoulderCenterY.Sum();
double shoulderCenterSumZ = shoulderCenterZ.Sum();
double hipCenterSumX = hipCenterX.Sum();
double hipCenterSumY = hipCenterY.Sum();
double hipCenterSumZ = hipCenterZ.Sum();
double spineSumX = spineX.Sum();
double spineSumY = spineX.Sum();
double spineSumZ = spineX.Sum();
double avgShoulderX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgShoulderY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgShoulderZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgSpineX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgSpineY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgSpineZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgHipX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgHipY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgHipZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgTorsoX = (avgShoulderX + avgSpineX + avgHipX) / 3.0;
double avgTorsoY = (avgShoulderY + avgSpineY + avgHipY) / 3.0;
double avgTorsoZ = (avgShoulderZ + avgSpineZ + avgHipZ) / 3.0;
_avgTorsoPosition = new DepthPoint((int)(_avgTorsoPosition.x * (1 - TORSO_LEARNING_ALPHA) + avgTorsoX * TORSO_LEARNING_ALPHA),
(int)(_avgTorsoPosition.y * (1 - TORSO_LEARNING_ALPHA) + avgTorsoY * TORSO_LEARNING_ALPHA),
(long)(_avgTorsoPosition.depth * (1 - TORSO_LEARNING_ALPHA) + avgTorsoZ * TORSO_LEARNING_ALPHA));
}
if (shoulderCenterPointsSupplemental.Count == MAX_POINTS_IN_READING)
{
// Determine if we should switch to the supplemental queue because the user has moved
double torsoSumX = 0.0;
double torsoSumY = 0.0;
double torsoSumZ = 0.0;
double torsoSDX = 0.0;
double torsoSDY = 0.0;
double torsoSDZ = 0.0;
for (int i = 0; i < shoulderCenterPointsSupplemental.Count; i++)
{
torsoSumX += (shoulderCenterPointsSupplemental.ElementAt(i).x +
hipCenterPointsSupplemental.ElementAt(i).x +
spinePointsSupplemental.ElementAt(i).x) / 3.0;
torsoSumY += (shoulderCenterPointsSupplemental.ElementAt(i).y +
hipCenterPointsSupplemental.ElementAt(i).y +
spinePointsSupplemental.ElementAt(i).y) / 3.0;
torsoSumZ += (shoulderCenterPointsSupplemental.ElementAt(i).depth +
hipCenterPointsSupplemental.ElementAt(i).depth +
spinePointsSupplemental.ElementAt(i).depth) / 3.0;
}
double avgTorsoX = torsoSumX / shoulderCenterPointsSupplemental.Count;
double avgTorsoY = torsoSumY / shoulderCenterPointsSupplemental.Count;
double avgTorsoZ = torsoSumZ / shoulderCenterPointsSupplemental.Count;
for (int i = 0; i < shoulderCenterPointsSupplemental.Count; i++)
{
torsoSDX += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).x +
hipCenterPointsSupplemental.ElementAt(i).x +
spinePointsSupplemental.ElementAt(i).x) / 3.0) - avgTorsoX, 2);
torsoSDY += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).y +
hipCenterPointsSupplemental.ElementAt(i).y +
spinePointsSupplemental.ElementAt(i).y) / 3.0) - avgTorsoY, 2);
torsoSDZ += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).depth +
hipCenterPointsSupplemental.ElementAt(i).depth +
spinePointsSupplemental.ElementAt(i).depth) / 3.0) - avgTorsoZ, 2);
}
torsoSDX /= (shoulderCenterPointsSupplemental.Count - 1);
torsoSDY /= (shoulderCenterPointsSupplemental.Count - 1);
torsoSDZ /= (shoulderCenterPointsSupplemental.Count - 1);
DepthPoint current = AvgTorsoPosition;
double xZScore = Math.Abs((current.x - avgTorsoX) / torsoSDX);
double yZScore = Math.Abs((current.y - avgTorsoY) / torsoSDY);
double zZScore = Math.Abs((current.depth - avgTorsoZ) / torsoSDZ);
if (xZScore + yZScore + zZScore > Z_SCORE_SUM_BOUND)
{
//Console.WriteLine("Adjusting torso center, z score: " + (xZScore + yZScore + zZScore));
shoulderCenterPoints = shoulderCenterPointsSupplemental;
spinePoints = spinePointsSupplemental;
hipCenterPoints = hipCenterPointsSupplemental;
shoulderCenterPointsSupplemental.Clear();
spinePointsSupplemental.Clear();
hipCenterPointsSupplemental.Clear();
}
else
{
while (shoulderCenterPointsSupplemental.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPointsSupplemental.Dequeue();
spinePointsSupplemental.Dequeue();
hipCenterPointsSupplemental.Dequeue();
}
}
}
//long shoulderCenterZ = shoulderCenter.depth;
//long spineZ = spine.depth;
//long hipCenterZ = hipCenter.depth;
this.torsoPosition = new DepthPoint((shoulderCenter.x + spine.x + hipCenter.x) / 3,
(shoulderCenter.y + spine.y + hipCenter.y) / 3,
(long)((shoulderCenter.depth + spine.depth + hipCenter.depth) / 3));
/*
* this.torsoPosition.x = (shoulderCenter.x + spine.x + hipCenter.x) / 3;
* this.torsoPosition.y = (shoulderCenter.y + spine.y + hipCenter.y) / 3;
* this.torsoPosition.depth = (shoulderCenter.depth + spine.Position.Z + hipCenter.Position.Z) / 3;
* */
/*
* //System.Console.WriteLine("shoulder z is: " + shoulderCenterZ + " spine z is: " + spineZ + " hipCenter z is: " + hipCenterZ);
* if (torsoCounter < MINDATAPOINTSREQ)
* {
* torsoPositions[torsoCounter].X = (shoulderCenter.Position.X + spine.Position.X + hipCenter.Position.X) / 3;
* torsoPositions[torsoCounter].Y = (shoulderCenter.Position.Y + spine.Position.Y + hipCenter.Position.Y) / 3;
* torsoPositions[torsoCounter].Z = (shoulderCenter.Position.Z + spine.Position.Z + hipCenter.Position.Z) / 3;
* torsoCounter++;
* }
* else
* {
* this.torsoPosition = new Point3d();
* for (int i = 0; i < torsoCounter; i++)
* {
* this.torsoPosition =
* this.torsoPosition + torsoPositions[i];
* }
* this.torsoPosition.X /= (torsoCounter);
* this.torsoPosition.Y /= (torsoCounter);
* this.torsoPosition.Z /= (torsoCounter);
* //System.Console.WriteLine("The depth of torso is: " + this.torsoPosition);
* }
*/
}
public bool crossesPlane(DepthPoint point)
{
// Ignore Y axis data
// Return whether the length of the projected vector into xz-space is greater than the cutoff
return Math.Sqrt(Math.Pow(point.x - center.x, 2) + Math.Pow(point.depth - center.depth, 2)) > depth;
}
public bool crossesPlane(DepthPoint point)
{
//Console.WriteLine("Difference: " + (point.depth - (center.depth - depth)));
return point.depth < (center.depth - depth);
}
public void addPoint(DepthPoint point)
{
points.Add(point);
}
public bool crossesPlane(DepthPoint point)
{
// Return whether the length of the vector is greater than the cutoff
return Math.Sqrt(Math.Pow(point.x - center.x, 2) + Math.Pow(point.depth - center.depth, 2)) > depth;
}
public Vec2d(DepthPoint p1, DepthPoint p2)
{
this.p1 = new Point2d(p1.x, p1.y);
this.p2 = new Point2d(p2.x, p2.y);
}
// Simple euclidean distance
private double distance(DepthPoint x, DepthPoint y)
{
return Math.Sqrt(Math.Pow((x.x - y.x), 2) +
Math.Pow((x.y - y.y), 2) +
Math.Pow((x.depth - y.depth), 2));
}
public bool crossesPlane(DepthPoint point)
{
// Return whether the length of the vector is greater than the cutoff
return(Math.Sqrt(Math.Pow(point.x - center.x, 2) + Math.Pow(point.depth - center.depth, 2)) > depth);
}
public Vec2d(DepthPoint p1, DepthPoint p2)
{
this.p1 = new Point2d(p1.x, p1.y);
this.p2 = new Point2d(p2.x, p2.y);
}
public SphericalPlane(DepthPoint center, double depth)
{
this.depth = depth;
this.center = center;
}
public bool crossesPlane(DepthPoint point)
{
// Ignore Y axis data
// Return whether the length of the projected vector into xz-space is greater than the cutoff
return(Math.Sqrt(Math.Pow(point.x - center.x, 2) + Math.Pow(point.depth - center.depth, 2)) > depth);
}
public bool crossesPlane(DepthPoint point)
{
//Console.WriteLine("Difference: " + (point.depth - (center.depth - depth)));
return(point.depth < (center.depth - depth));
}
public void setCenter(DepthPoint center)
{
this.center = center;
}
private int manhattanDistance2d(DepthPoint x, DepthPoint y)
{
return(Math.Abs(x.x - y.x) + Math.Abs(x.y - y.y));
}
public void removePoint(DepthPoint point)
{
points.Remove(point);
}
// Simple euclidean distance
private double distance(DepthPoint x, DepthPoint y)
{
return(Math.Sqrt(Math.Pow((x.x - y.x), 2) +
Math.Pow((x.y - y.y), 2) +
Math.Pow((x.depth - y.depth), 2)));
}
private int manhattanDistance2d(DepthPoint x, DepthPoint y)
{
return Math.Abs(x.x - y.x) + Math.Abs(x.y - y.y);
}
public void addPoint(DepthPoint point)
{
points.Add(point);
}
public DepthPoint getNearestPoint(DepthPoint other)
{
DepthPoint nearestPoint = null;
foreach (DepthPoint pt in points)
{
if (nearestPoint == null)
{
nearestPoint = pt;
}
else
{
if (distance(pt, other) < distance(nearestPoint, other))
{
nearestPoint = pt;
}
}
}
return nearestPoint;
}
public void removePoint(DepthPoint point)
{
points.Remove(point);
}
private double depth; // distance to torso
#endregion Fields
#region Constructors
public CylindricalPlane(DepthPoint center, double depth)
{
this.depth = depth;
this.center = center;
}
public static double EuclideanDistance2d(DepthPoint p1, DepthPoint p2)
{
return Math.Sqrt((p1.x - p2.x) * (p1.x - p2.x) +
(p1.y - p2.y) * (p1.y - p2.y));
}
private double depth; // distance to torso
#endregion Fields
#region Constructors
public FlatPlane(DepthPoint center, double depth)
{
this.depth = depth * 1000; // Assuming we get depth in meters from the skeleton tracker
this.center = center;
}
private void drawHands(DrawingContext drawingContext, ThreeDAuth.DepthPoint hand, bool drawHand, DepthImageFrame depthFrame)
{
//showDepthView(depthFrame, drawingContext);
//return;
if (Util.EuclideanDistance2d(hand, lastPoint) < 3.0)
{
hand = lastPoint;
}
else
{
lastPoint = hand;
}
//Start Siavash
double resetButtonTimePercentage = 0.0;
double doneButtonTimePercentage = 0.0;
if (userImage != null)
{
drawingContext.DrawImage(userImage, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
// Draw the reset button regardless
//Pen buttonPen = new Pen(Brushes.Black, 0.1);
//Brush resetButtonBrush = new SolidColorBrush(Color.FromRgb());
//drawingContext
if (resetButtonTimer.IsRunning)
{
resetButtonTimePercentage = (double) resetButtonTimer.ElapsedMilliseconds / (double) BUTTON_HOLD_CUTOFF;
}
if (resetButtonTimePercentage > 1.0)
{
drawingContext.DrawEllipse(Brushes.Green, null, new System.Windows.Point(0, 0), 50, 50);
resetButtonTimePercentage = 1.0;
}
else
{
drawingContext.DrawEllipse(Brushes.White, null, new System.Windows.Point(0, 0), 50, 50);
}
DrawWedge(drawingContext, 0, 0, SweepDirection.Counterclockwise, resetButtonTimePercentage);
drawingContext.DrawText(
new FormattedText("Reset",
System.Globalization.CultureInfo.CurrentCulture,
FlowDirection.LeftToRight,
new Typeface("Arial"),
14.0,
Brushes.Black),
new System.Windows.Point(3, 12.5));
// Draw the done button if we're learning
if (gLearner.isRecording)
{
if (doneButtonTimer.IsRunning)
{
doneButtonTimePercentage = (double)doneButtonTimer.ElapsedMilliseconds / (double)BUTTON_HOLD_CUTOFF;
}
if (doneButtonTimePercentage > 1.0) doneButtonTimePercentage = 1.0;
drawingContext.DrawEllipse(Brushes.White, null, new System.Windows.Point(depthFrame.Width, 0), 50, 50);
DrawWedge(drawingContext, depthFrame.Width, 0, SweepDirection.Clockwise, doneButtonTimePercentage);
drawingContext.DrawText(
new FormattedText("Done",
System.Globalization.CultureInfo.CurrentCulture,
FlowDirection.LeftToRight,
new Typeface("Arial"),
14.0,
Brushes.Black),
new System.Windows.Point(depthFrame.Width - 35.0, 12.5));
}
}
//End Siavash
drawHand = true;
if (drawHand)
{
if (myFrame.AvgTorsoPosition != null)
//if (myFrame.torsoPosition != null)
{
// Anton's code
// when a new frame is available, we check if the wrists are crossing the plane and we draw an appropriately colored
// rectangle over them to give the user feedback
double planeDepth = myFrame.armLength * .8;
int planeDepthPixels = (int)((planeDepth / myFrame.armLength) * myFrame.AvgArmLengthPixels);
//ThreeDAuth.FlatPlane myPlane = new ThreeDAuth.FlatPlane(myFrame.torsoPosition, planeDepth);
ThreeDAuth.FlatPlane myPlane = new ThreeDAuth.FlatPlane(myFrame.AvgTorsoPosition, planeDepth);
//Console.WriteLine("Torso depth: " + torsoSkeletonPoint.Z);
//ThreeDAuth.Point3d wristRight = new ThreeDAuth.Point3d(rightWrist.Position.X, rightWrist.Position.Y, rightWrist.Position.Z);
//ThreeDAuth.DepthPoint right = this.SkeletonPointToScreen(rightWrist.Position);
//ThreeDAuth.PlanePoint arrived = new ThreeDAuth.PlanePoint(right.x, right.y, myPlane.crossesPlane(right));
//pDistributor.GivePoint(arrived);
//Console.WriteLine("Depth: " + hand.depth);
short planeDepthmm = (short)(planeDepth * 1000); // convert m to mm
Tuple<int, int> handTuple = new Tuple<int, int>(hand.x, hand.y);
if (HandTrackingOptionSet.ProjectingPoints)
{
handTuple = ProjectPoint(handTuple,
myFrame.AvgTorsoPosition,
myFrame.AvgArmLengthPixels,
planeDepthPixels,
depthFrame.Width,
depthFrame.Height);
}
if (handTuple == null)
{
// encountered a problem and won't be able to project this point, so drop this frame
return;
}
ThreeDAuth.PlanePoint planePoint = new ThreeDAuth.PlanePoint(handTuple.Item1, handTuple.Item2, myPlane.crossesPlane(hand));
//ThreeDAuth.PlanePoint planePoint = new ThreeDAuth.PlanePoint(hand.x, hand.y, myPlane.crossesPlane(hand));
pDistributor.GivePoint(planePoint);
//drawingContext.DrawRoundedRectangle(Brushes.Green, null, new Rect(hand.x, hand.y, 30, 30), null, 14, null, 14, null);
// Check if the point is in the buttons
// reset button
bool inButton = false;
if (Math.Sqrt((planePoint.x * planePoint.x) + (planePoint.y * planePoint.y)) < 50.0)
{
// in reset button
inButton = true;
if (!resetButtonTimer.IsRunning)
{
resetButtonTimer.Start();
}
}
else
{
if (resetButtonTimer.IsRunning)
{
resetButtonTimer.Reset();
}
}
// done button
if (gLearner.isRecording)
{
if (Math.Sqrt(((planePoint.x - depthFrame.Width) * (planePoint.x - depthFrame.Width)) +
(planePoint.y * planePoint.y)) < 50.0)
{
// in done button
inButton = true;
if (!doneButtonTimer.IsRunning)
{
doneButtonTimer.Start();
}
}
else
{
if (doneButtonTimer.IsRunning)
{
doneButtonTimer.Reset();
}
}
}
if (HandTrackingOptionSet.ShowUnprojectedHand)
{
drawingContext.DrawRoundedRectangle(Brushes.Yellow, null, new Rect(hand.x, hand.y, 30, 30), null, 14, null, 14, null);
}
//if (arrived.inPlane)
if (planePoint.inPlane)
{
//drawingContext.DrawRoundedRectangle(Brushes.Blue, null, new Rect(right.x, right.y, 30, 30), null, 14, null, 14, null);
//drawingContext.DrawRoundedRectangle(Brushes.Blue, null, new Rect(hand.x, hand.y, 30, 30), null, 14, null, 14, null);
drawingContext.DrawRoundedRectangle(Brushes.Blue, null, new Rect(planePoint.x, planePoint.y, 30, 30), null, 14, null, 14, null);
}
else
{
//drawingContext.DrawRoundedRectangle(Brushes.Red, null, new Rect(right.x, right.y, 30, 30), null, 14, null, 14, null);
//drawingContext.DrawRoundedRectangle(Brushes.Red, null, new Rect(hand.x, hand.y, 30, 30), null, 14, null, 14, null);
drawingContext.DrawRoundedRectangle(Brushes.Red, null, new Rect(planePoint.x, planePoint.y, 30, 30), null, 14, null, 14, null);
}
ThreeDAuth.Point3d wristLeft = new ThreeDAuth.Point3d(leftWrist.Position.X, leftWrist.Position.Y, leftWrist.Position.Z);
ThreeDAuth.DepthPoint left = this.SkeletonPointToScreen(leftWrist.Position);
if (myPlane.crossesPlane(left))
{
//drawingContext.DrawRoundedRectangle(Brushes.Blue, null, new Rect(left.X, left.Y, 30, 30), null, 14, null, 14, null);
}
else
{
//drawingContext.DrawRoundedRectangle(Brushes.Red, null, new Rect(left.X, left.Y, 30, 30), null, 14, null, 14, null);
}
// Mason's code
// If we're learning a gesture, draw the learned points
if (gLearner != null && gLearner.isRecording)
{
System.Collections.Generic.Queue<ThreeDAuth.Point2d> currentPoints = gLearner.getGesturePath();
foreach (ThreeDAuth.Point2d point in currentPoints)
{
drawingContext.DrawRoundedRectangle(Brushes.Green, null, new Rect(point.x, point.y, 30, 30), null, 14, null, 14, null);
}
}
if (gValidator != null && HandTrackingOptionSet.ShowTargetPoints)
{
Queue<ThreeDAuth.Point2d> unhitPoints = gValidator.getUnHitPoints();
Queue<ThreeDAuth.Point2d> hitPoints = gValidator.getHitPoints();
Point2d nextPoint = gValidator.getNextPoint();
foreach (Point2d point in unhitPoints)
{
drawingContext.DrawRoundedRectangle(Brushes.Yellow, null, new Rect(point.x, point.y, 30, 30), null, 14, null, 14, null);
}
foreach (Point2d point in hitPoints)
{
drawingContext.DrawRoundedRectangle(Brushes.Green, null, new Rect(point.x, point.y, 30, 30), null, 14, null, 14, null);
}
if (nextPoint != null)
{
drawingContext.DrawRoundedRectangle(Brushes.Purple, null, new Rect(nextPoint.x, nextPoint.y, 30, 30), null, 14, null, 14, null);
}
}
if (gLearner != null && gLearner.isRecording && doneButtonTimePercentage >= 1.0)
{
// Done recording
gLearner.stopRecording();
this.myImageBox.Visibility = System.Windows.Visibility.Collapsed;
List<ThreeDAuth.Point2d> password = new List<ThreeDAuth.Point2d>(gLearner.getGesturePath());
this.currentUser.password = password;
this.gestureMassage.Text = "Success. Your account has been created. Welcome to 3DAuth!";
SaveUser(currentUser);
StateMachine.AdvanceState();
SetSensorListeners();
}
if (inButton && resetButtonTimePercentage >= 1.0)
{
if (gLearner != null && gLearner.isRecording)
{
// Reset the learner
gLearner.restart();
}
else if (gValidator != null)
{
// Reset the validator
gValidator.restart();
}
}
/*
// If we're learning a gesture and we've been out of the plane for 5 seconds, stop learning
if (gLearner.isRecording && !planePoint.inPlane)
{
if (!outOfPlaneTimer.IsRunning)
{
outOfPlaneTimer.Start();
}
if (outOfPlaneTimer.ElapsedMilliseconds > BUTTON_HOLD_CUTOFF)
{
gLearner.stopRecording();
this.myImageBox.Visibility = System.Windows.Visibility.Collapsed;
List<ThreeDAuth.Point2d> password = new List<ThreeDAuth.Point2d>(gLearner.getGesturePath());
this.currentUser.password = password;
this.gestureMassage.Text = "Success. Your account has been created. Welcome to 3DAuth!";
SaveUser(currentUser);
}
}
else if (gLearner.isRecording)
{
outOfPlaneTimer.Reset();
}
* */
}
}
}
public void setCenter(DepthPoint center)
{
this.center = center;
}
/// <summary>
///
/// </summary>
/// <param name="shoulderCenter"></param>
/// <param name="spine"></param>
/// <param name="hipCenter"></param>
internal void computerTorsoDepth(DepthPoint shoulderCenter, DepthPoint spine, DepthPoint hipCenter)
{
totalTorsoPoints++;
/*
if (shoulderCenterPoints.Count == MAX_POINTS_IN_READING)
{
shoulderCenterPointsSupplemental.Enqueue(shoulderCenter);
hipCenterPointsSupplemental.Enqueue(hipCenter);
spinePointsSupplemental.Enqueue(spine);
}
*/
while (shoulderCenterPoints.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPoints.Dequeue();
hipCenterPoints.Dequeue();
spinePoints.Dequeue();
}
while (shoulderCenterPointsSupplemental.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPointsSupplemental.Dequeue();
hipCenterPointsSupplemental.Dequeue();
spinePointsSupplemental.Dequeue();
}
if (totalTorsoPoints > MIN_POINTS_BEFORE_READING && shoulderCenterPoints.Count < MAX_POINTS_IN_READING)
{
shoulderCenterPoints.Enqueue(shoulderCenter);
hipCenterPoints.Enqueue(hipCenter);
spinePoints.Enqueue(spine);
}
else
{
shoulderCenterPointsSupplemental.Enqueue(shoulderCenter);
hipCenterPointsSupplemental.Enqueue(hipCenter);
spinePointsSupplemental.Enqueue(spine);
}
// Alpha average between the supplemental points and our stored average torso pos
if (shoulderCenterPointsSupplemental.Count > 30 && _avgTorsoPosition != null)
{
List<double> shoulderCenterX = new List<double>();
List<double> shoulderCenterY = new List<double>();
List<double> shoulderCenterZ = new List<double>();
List<double> hipCenterX = new List<double>();
List<double> hipCenterY = new List<double>();
List<double> hipCenterZ = new List<double>();
List<double> spineX = new List<double>();
List<double> spineY = new List<double>();
List<double> spineZ = new List<double>();
for (int i = 0; i < shoulderCenterPoints.Count; i++)
{
shoulderCenterX.Add(shoulderCenterPoints.ElementAt(i).x);
shoulderCenterY.Add(shoulderCenterPoints.ElementAt(i).y);
shoulderCenterZ.Add(shoulderCenterPoints.ElementAt(i).depth);
hipCenterX.Add(hipCenterPoints.ElementAt(i).x);
hipCenterY.Add(hipCenterPoints.ElementAt(i).y);
hipCenterZ.Add(hipCenterPoints.ElementAt(i).depth);
spineX.Add(spinePoints.ElementAt(i).x);
spineY.Add(spinePoints.ElementAt(i).y);
spineZ.Add(spinePoints.ElementAt(i).depth);
}
shoulderCenterX.Sort();
shoulderCenterY.Sort();
shoulderCenterZ.Sort();
hipCenterX.Sort();
hipCenterY.Sort();
hipCenterZ.Sort();
spineX.Sort();
spineY.Sort();
spineZ.Sort();
int numToRemove = shoulderCenterPoints.Count / 4;
for (int i = 0; i < numToRemove; i++)
{
shoulderCenterX.RemoveAt(0);
shoulderCenterY.RemoveAt(0);
shoulderCenterZ.RemoveAt(0);
hipCenterX.RemoveAt(0);
hipCenterY.RemoveAt(0);
hipCenterZ.RemoveAt(0);
spineX.RemoveAt(0);
spineY.RemoveAt(0);
spineZ.RemoveAt(0);
shoulderCenterX.RemoveAt(shoulderCenterX.Count - 1);
shoulderCenterY.RemoveAt(shoulderCenterY.Count - 1);
shoulderCenterZ.RemoveAt(shoulderCenterZ.Count - 1);
hipCenterX.RemoveAt(hipCenterX.Count - 1);
hipCenterY.RemoveAt(hipCenterZ.Count - 1);
hipCenterZ.RemoveAt(hipCenterZ.Count - 1);
spineX.RemoveAt(spineX.Count - 1);
spineY.RemoveAt(spineY.Count - 1);
spineZ.RemoveAt(spineZ.Count - 1);
}
double shoulderCenterSumX = shoulderCenterX.Sum();
double shoulderCenterSumY = shoulderCenterY.Sum();
double shoulderCenterSumZ = shoulderCenterZ.Sum();
double hipCenterSumX = hipCenterX.Sum();
double hipCenterSumY = hipCenterY.Sum();
double hipCenterSumZ = hipCenterZ.Sum();
double spineSumX = spineX.Sum();
double spineSumY = spineX.Sum();
double spineSumZ = spineX.Sum();
double avgShoulderX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgShoulderY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgShoulderZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgSpineX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgSpineY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgSpineZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgHipX = shoulderCenterSumX / (double)shoulderCenterX.Count;
double avgHipY = shoulderCenterSumY / (double)shoulderCenterX.Count;
double avgHipZ = shoulderCenterSumZ / (double)shoulderCenterX.Count;
double avgTorsoX = (avgShoulderX + avgSpineX + avgHipX) / 3.0;
double avgTorsoY = (avgShoulderY + avgSpineY + avgHipY) / 3.0;
double avgTorsoZ = (avgShoulderZ + avgSpineZ + avgHipZ) / 3.0;
_avgTorsoPosition = new DepthPoint((int) (_avgTorsoPosition.x * (1 - TORSO_LEARNING_ALPHA) + avgTorsoX * TORSO_LEARNING_ALPHA),
(int) (_avgTorsoPosition.y * (1 - TORSO_LEARNING_ALPHA) + avgTorsoY * TORSO_LEARNING_ALPHA),
(long) (_avgTorsoPosition.depth * (1 - TORSO_LEARNING_ALPHA) + avgTorsoZ * TORSO_LEARNING_ALPHA));
}
if (shoulderCenterPointsSupplemental.Count == MAX_POINTS_IN_READING)
{
// Determine if we should switch to the supplemental queue because the user has moved
double torsoSumX = 0.0;
double torsoSumY = 0.0;
double torsoSumZ = 0.0;
double torsoSDX = 0.0;
double torsoSDY = 0.0;
double torsoSDZ = 0.0;
for (int i = 0; i < shoulderCenterPointsSupplemental.Count; i++)
{
torsoSumX += (shoulderCenterPointsSupplemental.ElementAt(i).x +
hipCenterPointsSupplemental.ElementAt(i).x +
spinePointsSupplemental.ElementAt(i).x) / 3.0;
torsoSumY += (shoulderCenterPointsSupplemental.ElementAt(i).y +
hipCenterPointsSupplemental.ElementAt(i).y +
spinePointsSupplemental.ElementAt(i).y) / 3.0;
torsoSumZ += (shoulderCenterPointsSupplemental.ElementAt(i).depth +
hipCenterPointsSupplemental.ElementAt(i).depth +
spinePointsSupplemental.ElementAt(i).depth) / 3.0;
}
double avgTorsoX = torsoSumX / shoulderCenterPointsSupplemental.Count;
double avgTorsoY = torsoSumY / shoulderCenterPointsSupplemental.Count;
double avgTorsoZ = torsoSumZ / shoulderCenterPointsSupplemental.Count;
for (int i = 0; i < shoulderCenterPointsSupplemental.Count; i++)
{
torsoSDX += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).x +
hipCenterPointsSupplemental.ElementAt(i).x +
spinePointsSupplemental.ElementAt(i).x) / 3.0) - avgTorsoX, 2);
torsoSDY += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).y +
hipCenterPointsSupplemental.ElementAt(i).y +
spinePointsSupplemental.ElementAt(i).y) / 3.0) - avgTorsoY, 2);
torsoSDZ += Math.Pow(((shoulderCenterPointsSupplemental.ElementAt(i).depth +
hipCenterPointsSupplemental.ElementAt(i).depth +
spinePointsSupplemental.ElementAt(i).depth) / 3.0) - avgTorsoZ, 2);
}
torsoSDX /= (shoulderCenterPointsSupplemental.Count - 1);
torsoSDY /= (shoulderCenterPointsSupplemental.Count - 1);
torsoSDZ /= (shoulderCenterPointsSupplemental.Count - 1);
DepthPoint current = AvgTorsoPosition;
double xZScore = Math.Abs((current.x - avgTorsoX) / torsoSDX);
double yZScore = Math.Abs((current.y - avgTorsoY) / torsoSDY);
double zZScore = Math.Abs((current.depth - avgTorsoZ) / torsoSDZ);
if (xZScore + yZScore + zZScore > Z_SCORE_SUM_BOUND)
{
//Console.WriteLine("Adjusting torso center, z score: " + (xZScore + yZScore + zZScore));
shoulderCenterPoints = shoulderCenterPointsSupplemental;
spinePoints = spinePointsSupplemental;
hipCenterPoints = hipCenterPointsSupplemental;
shoulderCenterPointsSupplemental.Clear();
spinePointsSupplemental.Clear();
hipCenterPointsSupplemental.Clear();
}
else
{
while (shoulderCenterPointsSupplemental.Count > MAX_POINTS_IN_READING)
{
shoulderCenterPointsSupplemental.Dequeue();
spinePointsSupplemental.Dequeue();
hipCenterPointsSupplemental.Dequeue();
}
}
}
//long shoulderCenterZ = shoulderCenter.depth;
//long spineZ = spine.depth;
//long hipCenterZ = hipCenter.depth;
this.torsoPosition = new DepthPoint((shoulderCenter.x + spine.x + hipCenter.x) / 3,
(shoulderCenter.y + spine.y + hipCenter.y) / 3,
(long) ((shoulderCenter.depth + spine.depth + hipCenter.depth) / 3));
/*
this.torsoPosition.x = (shoulderCenter.x + spine.x + hipCenter.x) / 3;
this.torsoPosition.y = (shoulderCenter.y + spine.y + hipCenter.y) / 3;
this.torsoPosition.depth = (shoulderCenter.depth + spine.Position.Z + hipCenter.Position.Z) / 3;
* */
/*
//System.Console.WriteLine("shoulder z is: " + shoulderCenterZ + " spine z is: " + spineZ + " hipCenter z is: " + hipCenterZ);
if (torsoCounter < MINDATAPOINTSREQ)
{
torsoPositions[torsoCounter].X = (shoulderCenter.Position.X + spine.Position.X + hipCenter.Position.X) / 3;
torsoPositions[torsoCounter].Y = (shoulderCenter.Position.Y + spine.Position.Y + hipCenter.Position.Y) / 3;
torsoPositions[torsoCounter].Z = (shoulderCenter.Position.Z + spine.Position.Z + hipCenter.Position.Z) / 3;
torsoCounter++;
}
else
{
this.torsoPosition = new Point3d();
for (int i = 0; i < torsoCounter; i++)
{
this.torsoPosition =
this.torsoPosition + torsoPositions[i];
}
this.torsoPosition.X /= (torsoCounter);
this.torsoPosition.Y /= (torsoCounter);
this.torsoPosition.Z /= (torsoCounter);
//System.Console.WriteLine("The depth of torso is: " + this.torsoPosition);
}
*/
}
public bool IsValidPoint(DepthPoint point)
{
long now = System.DateTime.UtcNow.Ticks;
if (previousPoints.Count < MIN_POINTS)
{
previousPoints.Enqueue(new Tuple<DepthPoint, long>(point, now));
return true;
}
else
{
// Apply the 6-dimensional gaussian
// If it's 2+ std dev's away in any dimension, reject it
ICollection<double> xPos = new LinkedList<double>();
ICollection<double> yPos = new LinkedList<double>();
ICollection<double> depthPos = new LinkedList<double>();
ICollection<long> times = new LinkedList<long>();
ICollection<double> xVel = new LinkedList<double>();
ICollection<double> yVel = new LinkedList<double>();
ICollection<double> depthVel = new LinkedList<double>();
foreach (Tuple<DepthPoint, long> pointTuple in previousPoints)
{
xPos.Add(pointTuple.Item1.x);
yPos.Add(pointTuple.Item1.y);
depthPos.Add(pointTuple.Item1.depth);
times.Add(pointTuple.Item2);
if (xPos.Count > 1 && yPos.Count > 1 && depthPos.Count > 1 && times.Count > 1)
{
int prev2Index = times.Count - 2;
int prev1Index = times.Count - 1;
long timeInterval = times.ElementAt(prev1Index) - times.ElementAt(prev2Index);
double deltaX = xPos.ElementAt(prev1Index) - xPos.ElementAt(prev2Index);
double deltaY = yPos.ElementAt(prev1Index) - yPos.ElementAt(prev2Index);
double deltaDepth = depthPos.ElementAt(prev1Index) - depthPos.ElementAt(prev2Index);
xVel.Add(deltaX / timeInterval);
yVel.Add(deltaY / timeInterval);
depthVel.Add(deltaDepth / timeInterval);
}
}
GaussianHolder xPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xPos);
GaussianHolder yPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yPos);
GaussianHolder depthPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthPos);
GaussianHolder xVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xVel);
GaussianHolder yVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yVel);
GaussianHolder depthVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthVel);
double newPointTimeDelta = now - times.ElementAt(times.Count - 1);
double newXVel = (point.x - xPos.ElementAt(xPos.Count - 1)) / newPointTimeDelta;
double newYVel = (point.y - yPos.ElementAt(yPos.Count - 1)) / newPointTimeDelta;
double newDepthVel = (point.depth - depthPos.ElementAt(depthPos.Count - 1)) / newPointTimeDelta;
// approximate the new expected position
GaussianHolder deltaXPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(xVelGaussian, newPointTimeDelta);
GaussianHolder deltaYPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(yVelGaussian, newPointTimeDelta);
GaussianHolder deltaDepthPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(depthVelGaussian, newPointTimeDelta);
GaussianHolder newXPosGaussian = GaussianHolder.GenerateSumOfGaussians(xPosGaussian, deltaXPosGaussian);
GaussianHolder newYPosGaussian = GaussianHolder.GenerateSumOfGaussians(yPosGaussian, deltaYPosGaussian);
GaussianHolder newDepthPosGaussian = GaussianHolder.GenerateSumOfGaussians(depthPosGaussian, deltaDepthPosGaussian);
double xVelPhi = xVelGaussian.GetPhiValue(newXVel);
double yVelPhi = yVelGaussian.GetPhiValue(newYVel);
double depthVelPhi = depthVelGaussian.GetPhiValue(newDepthVel);
double xPosPhi = newXPosGaussian.GetPhiValue(point.x);
double yPosPhi = newYPosGaussian.GetPhiValue(point.y);
double depthPosPhi = newDepthPosGaussian.GetPhiValue(point.depth);
//Console.WriteLine("phi: " + xPosPhi + " | " + yPosPhi + " | " + depthPosPhi + " | " +
// xVelPhi + " | " + yVelPhi + " | " + depthVelPhi);
if ( (xVelPhi > PHI_CUTOFF && xVelPhi < (1 - PHI_CUTOFF)) &&
(yVelPhi > PHI_CUTOFF && yVelPhi < (1 - PHI_CUTOFF)) &&
(depthVelPhi > PHI_CUTOFF && depthVelPhi < (1 - PHI_CUTOFF)) &&
(xPosPhi > PHI_CUTOFF && xPosPhi < (1 - PHI_CUTOFF)) &&
(yPosPhi > PHI_CUTOFF && yPosPhi < (1 - PHI_CUTOFF)) &&
(depthPosPhi > PHI_CUTOFF && depthPosPhi < (1 - PHI_CUTOFF)) )
{
previousPoints.Enqueue(new Tuple<DepthPoint, long>(point, now));
if (previousPoints.Count > HISTORY_COUNT)
{
previousPoints.Dequeue();
}
return true;
}
else
{
//Console.WriteLine("Rejected a point!!!!!!!!!!!!!!!!!!!!!!");
rejectedPoints.Enqueue(new Tuple<DepthPoint, long>(point, now));
while (now - rejectedPoints.First().Item2 > ERROR_TIME_CUTOFF_TICKS)
{
//Console.WriteLine("dumping rejected points");
rejectedPoints.Dequeue();
}
if (rejectedPoints.Count > ERROR_POINT_LIMIT)
{
// Received so many error points recently we should probably start over
rejectedPoints.Clear();
previousPoints.Clear();
previousPoints.Enqueue(new Tuple<DepthPoint, long>(point, now));
//Console.WriteLine("***************** STARTING OVER ************************");
return true;
}
else
{
// Haven't received that many error points ecently and we rejected the most recent one
return false;
}
}
}
}
public bool IsValidPoint(DepthPoint point)
{
long now = System.DateTime.UtcNow.Ticks;
if (previousPoints.Count < MIN_POINTS)
{
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
return(true);
}
else
{
// Apply the 6-dimensional gaussian
// If it's 2+ std dev's away in any dimension, reject it
ICollection <double> xPos = new LinkedList <double>();
ICollection <double> yPos = new LinkedList <double>();
ICollection <double> depthPos = new LinkedList <double>();
ICollection <long> times = new LinkedList <long>();
ICollection <double> xVel = new LinkedList <double>();
ICollection <double> yVel = new LinkedList <double>();
ICollection <double> depthVel = new LinkedList <double>();
foreach (Tuple <DepthPoint, long> pointTuple in previousPoints)
{
xPos.Add(pointTuple.Item1.x);
yPos.Add(pointTuple.Item1.y);
depthPos.Add(pointTuple.Item1.depth);
times.Add(pointTuple.Item2);
if (xPos.Count > 1 && yPos.Count > 1 && depthPos.Count > 1 && times.Count > 1)
{
int prev2Index = times.Count - 2;
int prev1Index = times.Count - 1;
long timeInterval = times.ElementAt(prev1Index) - times.ElementAt(prev2Index);
double deltaX = xPos.ElementAt(prev1Index) - xPos.ElementAt(prev2Index);
double deltaY = yPos.ElementAt(prev1Index) - yPos.ElementAt(prev2Index);
double deltaDepth = depthPos.ElementAt(prev1Index) - depthPos.ElementAt(prev2Index);
xVel.Add(deltaX / timeInterval);
yVel.Add(deltaY / timeInterval);
depthVel.Add(deltaDepth / timeInterval);
}
}
GaussianHolder xPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xPos);
GaussianHolder yPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yPos);
GaussianHolder depthPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthPos);
GaussianHolder xVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xVel);
GaussianHolder yVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yVel);
GaussianHolder depthVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthVel);
double newPointTimeDelta = now - times.ElementAt(times.Count - 1);
double newXVel = (point.x - xPos.ElementAt(xPos.Count - 1)) / newPointTimeDelta;
double newYVel = (point.y - yPos.ElementAt(yPos.Count - 1)) / newPointTimeDelta;
double newDepthVel = (point.depth - depthPos.ElementAt(depthPos.Count - 1)) / newPointTimeDelta;
// approximate the new expected position
GaussianHolder deltaXPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(xVelGaussian, newPointTimeDelta);
GaussianHolder deltaYPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(yVelGaussian, newPointTimeDelta);
GaussianHolder deltaDepthPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(depthVelGaussian, newPointTimeDelta);
GaussianHolder newXPosGaussian = GaussianHolder.GenerateSumOfGaussians(xPosGaussian, deltaXPosGaussian);
GaussianHolder newYPosGaussian = GaussianHolder.GenerateSumOfGaussians(yPosGaussian, deltaYPosGaussian);
GaussianHolder newDepthPosGaussian = GaussianHolder.GenerateSumOfGaussians(depthPosGaussian, deltaDepthPosGaussian);
double xVelPhi = xVelGaussian.GetPhiValue(newXVel);
double yVelPhi = yVelGaussian.GetPhiValue(newYVel);
double depthVelPhi = depthVelGaussian.GetPhiValue(newDepthVel);
double xPosPhi = newXPosGaussian.GetPhiValue(point.x);
double yPosPhi = newYPosGaussian.GetPhiValue(point.y);
double depthPosPhi = newDepthPosGaussian.GetPhiValue(point.depth);
//Console.WriteLine("phi: " + xPosPhi + " | " + yPosPhi + " | " + depthPosPhi + " | " +
// xVelPhi + " | " + yVelPhi + " | " + depthVelPhi);
if ((xVelPhi > PHI_CUTOFF && xVelPhi < (1 - PHI_CUTOFF)) &&
(yVelPhi > PHI_CUTOFF && yVelPhi < (1 - PHI_CUTOFF)) &&
(depthVelPhi > PHI_CUTOFF && depthVelPhi < (1 - PHI_CUTOFF)) &&
(xPosPhi > PHI_CUTOFF && xPosPhi < (1 - PHI_CUTOFF)) &&
(yPosPhi > PHI_CUTOFF && yPosPhi < (1 - PHI_CUTOFF)) &&
(depthPosPhi > PHI_CUTOFF && depthPosPhi < (1 - PHI_CUTOFF)))
{
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
if (previousPoints.Count > HISTORY_COUNT)
{
previousPoints.Dequeue();
}
return(true);
}
else
{
//Console.WriteLine("Rejected a point!!!!!!!!!!!!!!!!!!!!!!");
rejectedPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
while (now - rejectedPoints.First().Item2 > ERROR_TIME_CUTOFF_TICKS)
{
//Console.WriteLine("dumping rejected points");
rejectedPoints.Dequeue();
}
if (rejectedPoints.Count > ERROR_POINT_LIMIT)
{
// Received so many error points recently we should probably start over
rejectedPoints.Clear();
previousPoints.Clear();
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
//Console.WriteLine("***************** STARTING OVER ************************");
return(true);
}
else
{
// Haven't received that many error points ecently and we rejected the most recent one
return(false);
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="leftWristJoint"></param>
/// <param name="leftShoulderJoint"></param>
/// <param name="rightWristJoint"></param>
/// <param name="rightShoulderJoint"></param>
public void computeArmLengthPixels(DepthPoint leftWristPoint, DepthPoint leftShoulderPoint, DepthPoint rightWristPoint, DepthPoint rightShoulderPoint)
{
totalArmPoints++;
if (totalArmPoints > MIN_POINTS_BEFORE_READING && leftWristPoints.Count < MAX_POINTS_IN_READING)
{
leftWristPoints.Enqueue(leftWristPoint);
leftShoulderPoints.Enqueue(leftShoulderPoint);
rightWristPoints.Enqueue(rightWristPoint);
rightShoulderPoints.Enqueue(rightShoulderPoint);
}
int leftArmX = leftWristPoint.x - leftShoulderPoint.x;
int leftArmY = leftWristPoint.y - leftShoulderPoint.y;
//int leftArmZ = leftWristPoint.Position.Z - leftShoulderPoint.Position.Z;
int rightArmX = rightWristPoint.x - rightShoulderPoint.x;
int rightArmY = rightWristPoint.y - rightShoulderPoint.y;
//int rightArmZ = rightWristPoint.Position.Z - rightShoulderPoint.Position.Z;
double leftArm = Math.Sqrt(leftArmX * leftArmX + leftArmY * leftArmY /*+ leftArmZ * leftArmZ*/);
double rightArm = Math.Sqrt(rightArmX * rightArmX + rightArmY * rightArmY /*+ rightArmZ * rightArmZ*/);
this.armLengthPixels = (int) ((leftArm + rightArm) / 2.0);
/*
* The length of both arms changes depending on the location of the person due to noise in data
* and also inaccuracy of depth cammera at too far and too close postions.
* so we collect at least 50 data points of the four joint we need and calculate the avregae
*
*/
/*
if (armCounter < MINDATAPOINTSREQ)
{
armLengths[armCounter] = (leftArm + rightArm) / 2;
armCounter++;
}
else
{
for (int i = 0; i < armCounter; i++)
{
this.armLength += armLengths[i];
}
//this.armLength /= (armCounter);
this.armLength = (leftArm + rightArm) / 2;
}*/
}
public FlatPlane(DepthPoint center, double depth)
{
this.depth = depth * 1000; // Assuming we get depth in meters from the skeleton tracker
this.center = center;
}
private Tuple<int, int> ProjectPoint(Tuple<int, int> basePoint,
DepthPoint torsoPosition,
int armLengthPixels,
int planeDepthFromTorsoPixels,
double alpha,
int windowWidth,
int windowHeight)
{
// return basePoint;
//armLength *= 1000; // Convert meters to mm
// Get pixel length of the arm
// We wish to find how many millimeters away from the corner of the target box our torso center is
// If we're at torsoPosition and the plane is planeDepthFromTorso away from us,
// then we construct a right triangle where one side is the vector
// of our torso to the plane,
// the other side is the vector from the center of the plane to the corner of the plane
// and the hypotenuse is our required arm
// Vector of torso to plane has length planeDepthFromTorso
// Vector of our required arm length is alpha * armLength
// By the pythagorean theorem, (alpha * armLength)^2 = (planeDepthFromTorso)^2 + (distance to corner)^2
// so distance to corner = + sqrt( (alpha * armLength)^2 - (planeDepthFromTorso)^2 )
double tempValue = Math.Pow(alpha * armLengthPixels, 2) - Math.Pow(planeDepthFromTorsoPixels, 2);
// If this tempValue is less than 0 then holding the arm straight out infront of you
// (with the given alpha) will not cross the plane and we have bigger problems
if (tempValue < 0)
{
// Sanity check
return null;
}
double distanceToCornermm = Math.Sqrt(tempValue);
// Now find the x and y offset required to get to the corner
// if we consider our torso position as at the center of the box (they should be standing there)
// then we consider the angle the vector to the upper right corner forms with the horizontal plane
// to be a value theta
// our x offset to the corner will be cos(theta) * distanceToCornermm
// (cos(angle) = adj / hyp, so adj = cos(angle) * hyp)
// our y offset to the corner will be sin(theta) * distanceToCornermm
// (sin(angle) = adj / hyp, so adj = sin(angle) * hyp)
// By similar triangles (rectangles), tan(theta) = windowHeight / windowWidth, both of which we know
// so theta = arctan(windowHeight / windowWidth);
double theta = Math.Atan((double)windowHeight / (double)windowWidth);
double xOffset = Math.Cos(theta) * distanceToCornermm;
double yOffset = Math.Sin(theta) * distanceToCornermm;
Microsoft.Kinect.SkeletonPoint cornerSkeletonPoint = new Microsoft.Kinect.SkeletonPoint();
cornerSkeletonPoint.X = (float)(torsoPosition.x + xOffset);
cornerSkeletonPoint.Y = (float)(torsoPosition.y + yOffset);
//DepthPoint lowerRightCornerDepthPoint = this.SkeletonPointToScreen(cornerSkeletonPoint);
// This cornerPoint should represent the bottom right corner more or less
// So its x value should be our pre-projected target box width,
// and its y value should be our pre-projected target box height
double xScale = (double)windowWidth / (double)(torsoPosition.x + xOffset);
double yScale = (double)windowHeight / (double)(torsoPosition.y + yOffset);
Tuple<int, int> shiftedPoint = new Tuple<int, int>(basePoint.Item1 - torsoPosition.x, basePoint.Item2 - torsoPosition.y);
int xProj = (int)Math.Floor(xScale * shiftedPoint.Item1);
int yProj = (int)Math.Floor(yScale * shiftedPoint.Item2);
// unshift the point
xProj = xProj + torsoPosition.x;
yProj = yProj + torsoPosition.y;
//bound checking
if (xProj > windowWidth) xProj = windowWidth - 1;
if (yProj > windowHeight) yProj = windowHeight - 1;
if (xProj <= 0) xProj = 1;
if (yProj <= 0) yProj = 1;
//Console.WriteLine("(" + basePoint.Item1 + ", " + basePoint.Item2 + ") -> (" + xProj + ", " + yProj + ")" +
//" Arm length: " + armLengthPixels);
return new Tuple<int, int>(xProj, yProj);
}
/// <summary>
/// Start Siavash
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void SensorDepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
{
if (lastPoint == null)
{
lastPoint = new DepthPoint(0, 0, 0);
}
using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
{
if (depthFrame != null)
{
this.maxDepth = depthFrame.MaxDepth;
this.minDepth = depthFrame.MinDepth;
this.closestPoint.Depth = Convert.ToInt16(this.maxDepth);
if (this.imagePixelData == null || this.imadeData == null)
{
this.imadeData = new short[depthFrame.PixelDataLength];
this.imagePixelData = new DepthImagePixel[depthFrame.PixelDataLength];
}
depthFrame.CopyDepthImagePixelDataTo(imagePixelData);
depthFrame.CopyPixelDataTo(imadeData);
// This cutoff allows us to control how often the closest point is calculated (it doesn't necessarily need to be calculated every frame)
closestPointCounter %= CLOSEST_POINT_COUNTER_CUTOFF;
if (closestPointCounter == 0)
{
findTheClosestPoint(depthFrame.PixelDataLength, depthFrame.Width, depthFrame.Height);
}
closestPointCounter++;
// Flood fill from this point then send a point to the distributor
// If the nearest point hasn't broken the plane, then don't bother doing anything with it
// The array index is computed as x + y*width,
// So x = idx % width
// y = (idx - x)/width
int xIdx = this.pixelIndex % depthFrame.Width;
int yIdx = this.pixelIndex / depthFrame.Width;
myPointCluster = ThreeDAuth.Util.FloodFill2(imagePixelData, xIdx, yIdx, depthFrame.Width, depthFrame.Height - 1);
ThreeDAuth.DepthPoint centroid = myPointCluster.Centroid;
// send centroid to filter and draw if valid
int counter = 0;
while (myPointCluster.points.Count < 50 && counter < 20)
{
imagePixelData[pixelIndex].Depth = short.MaxValue;
counter++;
findTheClosestPoint(depthFrame.PixelDataLength, depthFrame.Width, depthFrame.Height);
xIdx = this.pixelIndex % depthFrame.Width;
yIdx = this.pixelIndex / depthFrame.Width;
myPointCluster = ThreeDAuth.Util.FloodFill2(imagePixelData, xIdx, yIdx, depthFrame.Width, depthFrame.Height - 1);
centroid = myPointCluster.Centroid;
}
if (counter < 20)
{
using (DrawingContext dc = this.liveFeedbackGroup.Open())
{
drawHands(dc, centroid, positionMotionFilter.IsValidPoint(centroid), depthFrame);
}
if (depthViewWindow != null && depthViewWindow.Running)
{
depthViewWindow.GiveFrame(depthFrame, myPointCluster);
}
}
}
}
}
