public PointSkeleton3D(PointSkeleton3D point,int timeStamp)
{
this.X = point.X;
this.Y = point.Y;
this.Z = point.Z;
this.TimeStamp = timeStamp;
}
//timeStamp没啥用？？
private void FingerTrack(List<PointSkeleton3D> fingertips,int timeStamp)
{
foreach (var i in Fingers)
{
if (i.Value.TrackingState == FingerTrackingState.Tracked)
{
PointSkeleton3D res = new PointSkeleton3D();
float min = float.MaxValue;
foreach (var j in fingertips)
{
float temp = PointSkeleton3D.EuclideanDistance(i.Value.Position, j);
if (temp < DIS_THRESHOLD && min > temp)
{
min = temp;
res = j;
}
}
if (min != float.MaxValue)
{
i.Value.Position.X = res.X;
i.Value.Position.Y = res.Y;
i.Value.Position.Z = res.Z;
i.Value.Position.TimeStamp = res.TimeStamp;
}
else if ((timeStamp > i.Value.Position.TimeStamp && timeStamp - i.Value.Position.TimeStamp >= FRAME_INTERVAL)
|| (timeStamp < i.Value.Position.TimeStamp && (timeStamp + KinectUtil.LOOP_TIMES) - i.Value.Position.TimeStamp >= FRAME_INTERVAL))
{
i.Value.TrackingState = FingerTrackingState.NotTracked;
}
}
}
}
public static PointSkeleton3D operator -(PointSkeleton3D point1, PointSkeleton3D point2)
{
PointSkeleton3D ret = new PointSkeleton3D();
ret.X = point1.X - point2.X;
ret.Y = point1.Y - point2.Y;
ret.Z = point1.Z - point2.Z;
return ret;
}
/// <summary>
/// Map PointSkeleton3D to PointDepth3D
/// </summary>
/// <param name="pointSkleton3D"></param>
/// <param name="depthImageFormat"></param>
/// <returns></returns>
public PointDepth3D MapSkeletonPointToDepthPoint(PointSkeleton3D pointSkleton3D,DepthImageFormat depthImageFormat)
{
SkeletonPoint point = new SkeletonPoint();
point.X = pointSkleton3D.X;
point.Y = pointSkleton3D.Y;
point.Z = pointSkleton3D.Z;
return new PointDepth3D(mapper.MapSkeletonPointToDepthPoint(point,depthImageFormat));
}
/// <summary>
/// Map PointSkeleton3D to Point2D
/// </summary>
/// <param name="pointSkleton3D"></param>
/// <param name="colorImageFormat"></param>
/// <returns></returns>
public Point2D MapSkeletonPointToColorPoint(PointSkeleton3D pointSkleton3D, ColorImageFormat colorImageFormat)
{
SkeletonPoint point = new SkeletonPoint();
point.X = pointSkleton3D.X;
point.Y = pointSkleton3D.Y;
point.Z = pointSkleton3D.Z;
ColorImagePoint ImgPoint = mapper.MapSkeletonPointToColorPoint(point, colorImageFormat);
return new Point2D(ImgPoint.X, ImgPoint.Y);
}
/// <summary>
/// Get the Euclidean Distance between two point in Skeleton Space
/// </summary>
/// <param name="point1"></param>
/// <param name="point2"></param>
/// <returns></returns>
public static float EuclideanDistance(PointSkeleton3D point1, PointSkeleton3D point2)
{
return (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X)
+ (point1.Y - point2.Y) * (point1.Y - point2.Y)
+ (point1.Z - point2.Z) * (point1.Z - point2.Z));
}
public static float Angle(PointSkeleton3D point1, PointSkeleton3D point2)
{
return (float)Math.Acos((point1.X * point2.X + point1.Y * point2.Y + point1.Z * point2.Z) / (Length(point1) * Length(point2)));
}
public static float Length(PointSkeleton3D point1)
{
return (float)(Math.Sqrt(point1.X * point1.X + point1.Y * point1.Y + point1.Z * point1.Z));
}
/// <summary>
/// Implement the IComparer interface for List<T>.Sort()
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
private int CompareByCoord_X(PointSkeleton3D p1, PointSkeleton3D p2)
{
if (p1.X == p2.X)
return 0;
return (p1.X < p2.X) ? 1 : -1;
}
private void Tracked(FingerType id, PointSkeleton3D position)
{
//if (null != position)
Fingers[id].Position = new PointSkeleton3D(position);
Fingers[id].TrackingState = FingerTrackingState.Tracked;
}
/// <summary>
/// Finger Identification with two fingertips' max Distance
/// </summary>
/// <param name="fingertips"></param>
public void Identify3(List<PointSkeleton3D> fingertips, int timeStamp)
{
if (null == fingertips)
return;
if(fingertips.Count == 5)
{
//1.Find two indexs for ThumbFinger and LittleFinger
int thumbIndex = 0, littleIndex = 0 ;
float max = float.MinValue;
for (int i = 0; i < fingertips.Count; i++)
{
for (int j = i + 1; j < fingertips.Count; j++)
{
float dis = PointSkeleton3D.EuclideanDistance(fingertips[i], fingertips[j]);
if (dis > max)
{
max = dis;
thumbIndex = i;
littleIndex = j;
}
}
}
//2.S and T Set construction, compute min distance between two Sets
PointSkeleton3D[] S = new PointSkeleton3D[2];
S[0] = new PointSkeleton3D(fingertips[littleIndex]);
S[1] = new PointSkeleton3D(fingertips[thumbIndex]);
PointSkeleton3D[] T = new PointSkeleton3D[3];
int k = 0;
for (int i = 0; i < fingertips.Count; i++)
{
if (fingertips[i] != S[0] && fingertips[i] != S[1])
T[k++] = new PointSkeleton3D(fingertips[i]);
}
int[] neighbour = new int[S.Length];
float[] minDis = new float[S.Length];
for (int i = 0; i < S.Length; i++)
{
float min = float.MaxValue;
int temp = 0;
for (int j = 0; j < T.Length; j++)
{
float dis = PointSkeleton3D.EuclideanDistance(S[i], T[j]);
if (dis < min)
{
min = dis;
temp = j;
}
}
if (min != float.MaxValue)
{
neighbour[i] = temp;
minDis[i] = min;
}
}
if (minDis[0] == Math.Max(minDis[0], minDis[1]))
{
//Fingers[FingerType.ThumbRight].Position = S[0];
//Fingers[FingerType.ThumbRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.IndexRight].Position = T[neighbour[0]];
//Fingers[FingerType.IndexRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.LittleRight].Position = S[1];
//Fingers[FingerType.LittleRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.RingRight].Position = T[neighbour[1]];
//Fingers[FingerType.RingRight].TrackingState = FingerTrackingState.Tracked;
Tracked(FingerType.ThumbRight, S[0]);
Tracked(FingerType.IndexRight, T[neighbour[0]]);
Tracked(FingerType.LittleRight, S[1]);
Tracked(FingerType.RingRight, T[neighbour[1]]);
}
else
{
//Fingers[FingerType.ThumbRight].Position = S[1];
//Fingers[FingerType.ThumbRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.IndexRight].Position = T[neighbour[1]];
//Fingers[FingerType.IndexRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.LittleRight].Position = S[0];
//Fingers[FingerType.LittleRight].TrackingState = FingerTrackingState.Tracked;
//Fingers[FingerType.RingRight].Position = T[neighbour[0]];
//Fingers[FingerType.RingRight].TrackingState = FingerTrackingState.Tracked;
Tracked(FingerType.LittleRight, S[0]);
Tracked(FingerType.RingRight, T[neighbour[0]]);
Tracked(FingerType.ThumbRight, S[1]);
Tracked(FingerType.IndexRight, T[neighbour[1]]);
}
//3.Middle Finger Remaineds
int remain = 0;
for (int i = 0; i < T.Length; i++)
{
if (i != neighbour[0] && i != neighbour[1])
remain = i;
}
//Fingers[FingerType.MiddleRight].Position = T[remain];
//Fingers[FingerType.MiddleRight].TrackingState = FingerTrackingState.Tracked;
Tracked(FingerType.MiddleRight, T[remain]);
}
FingerTrack(fingertips, timeStamp);
}
/// <summary>
/// Finger identification with angle between palm-wrist line and every fingertip
/// </summary>
/// <param name="fingertips"></param>
/// <param name="palm"></param>
/// <param name="wrist"></param>
public void Identify2(List<PointSkeleton3D> fingertips, PointSkeleton3D palm, PointSkeleton3D wrist)
{
if (null == fingertips)
return;
if (fingertips.Count == 5)
{
PointSkeleton3D v1 = palm - wrist;
float min = float.MaxValue;
PointSkeleton3D res = new PointSkeleton3D();
foreach (var i in fingertips)
{
PointSkeleton3D v2 = i - palm;
float angle = PointSkeleton3D.Angle(v1, v2);
if (angle < min)
{
min = angle;
res = i;
}
}
Fingers[FingerType.MiddleRight].Position = res;
Fingers[FingerType.MiddleRight].TrackingState = FingerTrackingState.Tracked;
}
else
{
Fingers[FingerType.MiddleRight].TrackingState = FingerTrackingState.NotTracked;
}
}
public Finger()
{
Position = new PointSkeleton3D();
TrackingState = FingerTrackingState.NotTracked;
}
/// <summary>
/// Sort Fingertips list with depth increasing
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
private int CompareByDepth(PointSkeleton3D p1, PointSkeleton3D p2)
{
if (p1.Z == p2.Z)
return 0;
return (p1.Z > p2.Z) ? 1 : -1;
}
/// <summary>
/// Smooth FingerTips with current computed result and time stamp
/// </summary>
/// <param name="curr"></param>
/// <param name="timeStamp"></param>
public void Smooth(List<PointSkeleton3D> curr,int timeStamp)
{
if (curr == null || curr.Count > 6)
{
return;
}
NoiseFilter(curr);
if (fingerTips.Count >= curr.Count)
{
List<PointSkeleton3D> candinateDelete = new List<PointSkeleton3D>();
foreach (var i in fingerTips)
{
PointSkeleton3D res = new PointSkeleton3D();
float min = float.MaxValue;
//Find nearest cooresponding point in two Lists
foreach (var j in curr)
{
float temp = PointSkeleton3D.EuclideanDistance(i, j);
if (temp < DIS_THRESHOLD && min > temp)
{
min = temp;
res = new PointSkeleton3D(j);
}
}
//Update result with weighted method
if (min != float.MaxValue)
{
if (min < 0.008f)
{
i.TimeStamp = timeStamp;
}
else
{
i.X = i.X * ALPHA + (1 - ALPHA) * res.X;
i.Y = i.Y * ALPHA + (1 - ALPHA) * res.Y;
i.Z = i.Z * ALPHA + (1 - ALPHA) * res.Z;
i.TimeStamp = timeStamp;
}
}
//the point lost, Check the timestamp difference
else if ((timeStamp > i.TimeStamp && timeStamp - i.TimeStamp >= FRAME_INTERVAL)
|| (timeStamp < i.TimeStamp && (timeStamp + KinectUtil.LOOP_TIMES) - i.TimeStamp >= FRAME_INTERVAL))
{
candinateDelete.Add(i);
}
}
foreach (var k in candinateDelete)
{
fingerTips.Remove(k);
}
}
else
{
fingerTips.Clear();
foreach (var i in curr)
{
PointSkeleton3D point = new PointSkeleton3D(i,timeStamp);
fingerTips.Add(point);
}
}
}
void Sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
int personCount = 0;
foreach (Skeleton sk in skeletons)
{
if (sk.TrackingState == SkeletonTrackingState.Tracked)
{
//get left and right hand for finding Finger Rect Region
leftHand = new PointSkeleton3D(sk.Joints[JointType.HandLeft].Position);
rightHand = new PointSkeleton3D(sk.Joints[JointType.HandRight].Position);
if (ViewEnum.CameraView == CurrViewType && drawFlag) //Draw Skeleton every frame in CameraView.xmal.cs
DrawSkeletonAndFingers(sk, this.Fingers);
personCount++;
}
}
if (ViewEnum.FullView == CurrViewType) //Recognize Swip left or right every frame in FullView.xmal.cs
this.activeRecognizer.Recognize(sender, skeletonFrame, skeletons);
skeletonReadyFlag = (personCount == 0 ? false : true);
//set draw skeleton flag true
if (Fingers.FingerTrackedCount == 2 && Fingers[FingerType.ThumbRight].TrackingState == FingerTrackingState.Tracked && Fingers[FingerType.IndexRight].TrackingState == FingerTrackingState.Tracked)
{
drawFlag = true;
}
//cancle draw skeleton flag
if (Fingers.FingerTrackedCount == 1 && Fingers[FingerType.ThumbRight].TrackingState == FingerTrackingState.Tracked)
{
drawFlag = false;
DrawSkeletonAndFingers(null, null);
}
if (CurrViewType == ViewEnum.CameraView && drawFlag)
PlayPiano(this.Fingers);
}
}