private void Sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
byte[] pixelsColor;
short[] pixelsDepth;
#region ColorImageFrame
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame == null)
return;
pixelsColor = new byte[colorFrame.PixelDataLength];
colorFrame.CopyPixelDataTo(pixelsColor);
fWidth = (UInt32)colorFrame.Width;
fHeight = (UInt32)colorFrame.Height;
uint stride = fWidth * (uint)kinect.pixelFeed;
colorFrame.CopyPixelDataTo(pinnedImageBuffer, (int)(stride * fHeight)); //SALIENCY
kinect.colorImageBitmap.WritePixels(kinect.colorImageBitmapRect, pixelsColor, (int)stride, 0);
lineFeed = kinect.colorImageBitmap.BackBufferStride;
imageIntPr = kinect.colorImageBitmap.BackBuffer;
kinect.colorBitmap=kinect.ColorImageFrameToBitmap(colorFrame);
}
#endregion
#region DepthImageFrame
using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
{
if (depthFrame == null)
return;
pixelsDepth = new short[depthFrame.PixelDataLength];
depthFrame.CopyPixelDataTo(pixelsDepth);
kinect.frameReady.Set();
}
#endregion
using (SkeletonFrame frame = e.OpenSkeletonFrame())
{
if (frame == null)
return;
Canvas_Skeleton.Children.Clear();
Canvas_Robot.Children.Clear();
frame.CopySkeletonDataTo(frameSkeletons);
#region Remove Subject from list
foreach (Subject checkSub in sceneSubjects.ToList())
{
bool remove = true;
foreach (Skeleton ske in frameSkeletons)
{
if (checkSub.idKinect == ske.TrackingId)
{
remove = false;
break;
}
}
if (remove)
sceneSubjects.Remove(checkSub);
}
#endregion
#region Scan skeleton from Kinect
for (int j = 0; j < frameSkeletons.Length; j++)
{
Skeleton skeleton = frameSkeletons[j];
int i = 0;
if (skeleton.TrackingState == SkeletonTrackingState.Tracked || skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
{
#region check subject present
bool present = false;
foreach (Subject checkSub in sceneSubjects)
{
if (checkSub.idKinect == skeleton.TrackingId)
{
present = true;
break;
}
i++;
}
if (!present)
{
Subject newSub = new Subject();
newSub.idKinect = skeleton.TrackingId;
newSub.angle = 0;
sceneSubjects.Add(newSub);
i = sceneSubjects.Count - 1;
if (RecognitionCheckbox.IsChecked == true)
_subrecognition.Invoke();
}
#endregion
#region Tracking subject
System.Windows.Point spinInCanvas = new System.Windows.Point();
switch (skeleton.TrackingState)
{
case SkeletonTrackingState.Tracked:
sceneSubjects[i].trackedState = true;
if (drawSkeleton)
{
var userBrush = skeletonBrushes[i];
//Draw head and torso
var figure = kinect.CreateFigure(skeleton, userBrush, new[] { JointType.Head, JointType.ShoulderCenter, JointType.ShoulderLeft, JointType.Spine, JointType.ShoulderRight, JointType.ShoulderCenter, JointType.HipCenter }, Canvas_Skeleton);
Canvas_Skeleton.Children.Add(figure);
//Draw left arm
figure = kinect.CreateFigure(skeleton, userBrush, new[] { JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, JointType.HandLeft }, Canvas_Skeleton);
Canvas_Skeleton.Children.Add(figure);
//Draw right arm
figure = kinect.CreateFigure(skeleton, userBrush, new[] { JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, JointType.HandRight }, Canvas_Skeleton);
Canvas_Skeleton.Children.Add(figure);
}
foreach (Joint joint in skeleton.Joints)
{
switch (joint.JointType)
{
case JointType.HandRight:
sceneSubjects[i].righthand_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.HandLeft:
sceneSubjects[i].lefthand_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.Spine:
sceneSubjects[i].spincenter_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
//SDK del kinect prende come(0,0) il punto in alto a destra della depth e il punto (640,480) in basso a sinistra
//il mondo di FACE invece va da 0 a 1 in entrambi gli assi (X,Y) per questo si normalizza in base alle info del SDK
//spinPoints.Insert(i, GetJointPoint(skeleton.Joints[JointType.Spine], Canvas_Skeleton));
spinInCanvas = kinect.GetJointPoint(skeleton.Joints[JointType.Spine], Canvas_Skeleton);
//System.Diagnostics.Debug.WriteLine("Tracked [" + sceneSubjects[i].id + "] SpinCanvas (" + spinInCanvas.X + "," + spinInCanvas.Y + ")");
sceneSubjects[i].normalizedspincenter_xy = new List<float> {
(float)Math.Round((decimal)(spinInCanvas.X) / ((kinect.sensor.DepthStream.FrameWidth)), 2, MidpointRounding.ToEven),
(float)Math.Round((decimal)((kinect.sensor.DepthStream.FrameHeight - spinInCanvas.Y) / (kinect.sensor.DepthStream.FrameHeight)), 2, MidpointRounding.ToEven)
};
//System.Diagnostics.Debug.WriteLine("TrackeNormd [" + sceneSubjects[i].id + "] SpinCanvas (" + sceneSubjects[i].normalizedspincenter_xy[0] + "," + sceneSubjects[i].normalizedspincenter_xy [1] + ")");
if (joint.Position.Z > 0)
sceneSubjects[i].angle = (float)Math.Round((Math.Atan((joint.Position.X / joint.Position.Z)) * (180 / (Math.PI))), 2);
sceneSubjects[i].speak_prob = Math.Abs(Math.Abs(sceneSubjects[i].angle - environment.soundAngle) - 57) / 57;
break;
case JointType.Head:
sceneSubjects[i].head_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
//write the id over head
System.Windows.Point xyhead0 = kinect.GetJointPoint(skeleton.Joints[JointType.Head], Canvas_Skeleton);
if (sceneSubjects[i].id != 0)
{
DrawPointString(Convert.ToString(sceneSubjects[i].id), xyhead0.X, (float)(xyhead0.Y - 50), Brushes.Green);
DrawPointString(sceneSubjects[i].name[0], xyhead0.X, (float)(xyhead0.Y - 65), Brushes.Red);
}
else
{
DrawPointString(Convert.ToString(sceneSubjects[i].idKinect), xyhead0.X, (float)(xyhead0.Y - 50), Brushes.Blue);
// DrawPointString(Convert.ToString(sceneSubjects[i].gesture), xyhead0.X, (float)(xyhead0.Y - 65), Brushes.Green);
}
break;
case JointType.WristLeft:
sceneSubjects[i].leftwrist_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.WristRight:
sceneSubjects[i].rightwrist_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.ElbowLeft:
sceneSubjects[i].leftelbow_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.ElbowRight:
sceneSubjects[i].rightelbow_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.ShoulderLeft:
sceneSubjects[i].leftshoulder_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
case JointType.ShoulderRight:
sceneSubjects[i].rightshoulder_xyz = new List<float> { joint.Position.X, joint.Position.Y, joint.Position.Z };
break;
}
}
#region Gesture
if (sceneSubjects[i].head_xyz != null && sceneSubjects[i].spincenter_xyz != null)
{
sceneSubjects[i].gesture = 0;
// TRUE if the right or left hand is over the treshold
// FALSE if the subject is not trackable (i.e. the head joint is equals to the spin joint), See Tracked Passive Subjects
double treshold = sceneSubjects[i].head_xyz[1] - ((sceneSubjects[i].head_xyz[1] - sceneSubjects[i].spincenter_xyz[1]) / 3);
if (((sceneSubjects[i].righthand_xyz[1] > treshold) || (sceneSubjects[i].lefthand_xyz[1] > treshold)) &&
(sceneSubjects[i].head_xyz[1] - sceneSubjects[i].spincenter_xyz[1]) > 0)
{
sceneSubjects[i].gesture = 1;
}
}
#endregion
break;
case SkeletonTrackingState.PositionOnly:
sceneSubjects[i].trackedState = false;
//The skeleton position in the case of PositionOnly tracking state is the HipPoint, NOT the SpinPoint
sceneSubjects[i].spincenter_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
spinInCanvas = kinect.GetPassivePoint(skeleton, Canvas_Skeleton);
//System.Diagnostics.Debug.WriteLine("NoTracked [" + sceneSubjects[i].id + "] SpinCanvas (" + spinInCanvas.X + "," + spinInCanvas.Y + ")");
sceneSubjects[i].normalizedspincenter_xy = new List<float> {
(float)Math.Round((decimal)(spinInCanvas.X / kinect.sensor.ColorStream.FrameWidth), 2, MidpointRounding.ToEven),
(float)Math.Round((decimal)((kinect.sensor.ColorStream.FrameHeight - spinInCanvas.Y) / kinect.sensor.ColorStream.FrameHeight), 2, MidpointRounding.ToEven)
};
//System.Diagnostics.Debug.WriteLine("NoTrackeNormd [" + sceneSubjects[i].id + "] SpinCanvas (" + sceneSubjects[i].normalizedspincenter_xy[0] + "," + sceneSubjects[i].normalizedspincenter_xy[1] + ")");
sceneSubjects[i].lefthand_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].righthand_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].head_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].leftwrist_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].rightwrist_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].leftelbow_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].rightelbow_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].leftshoulder_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
sceneSubjects[i].rightshoulder_xyz = new List<float> { skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z };
if (skeleton.Position.Z > 0)
sceneSubjects[i].angle = (float)Math.Round((Math.Atan((skeleton.Position.X / skeleton.Position.Z)) * (180 / (Math.PI))), 2);
sceneSubjects[i].speak_prob = Math.Abs(Math.Abs(sceneSubjects[i].angle - environment.soundAngle) - 57) / 57;
if(sceneSubjects[i].id!=0){
DrawPointString(Convert.ToString(sceneSubjects[i].id), spinInCanvas.X, (float)(spinInCanvas.Y - 110),Brushes.Red);
DrawPointString(sceneSubjects[i].name[0], spinInCanvas.X, (float)(spinInCanvas.Y - 138),Brushes.Red);
}
else
DrawPointString(Convert.ToString(sceneSubjects[i].idKinect), spinInCanvas.X, (float)(spinInCanvas.Y - 110),Brushes.Blue);
DrawPoint(spinInCanvas.X, (float)(spinInCanvas.Y));
break;
}
#endregion
#region FaceTracker
if (FacetrackingCheckbox.IsChecked == true)
{
if (faceTracker == null)
{
try
{
faceTracker = new FaceTracker(kinect.sensor);
}
catch (InvalidOperationException)
{
// During some shutdown scenarios the FaceTracker
// is unable to be instantiated. Catch that exception
// and don't track a face.
Debug.WriteLine("AllFramesReady - creating a new FaceTracker threw an InvalidOperationException");
faceTracker = null;
}
}
if (faceTracker != null)
{
FaceTrackFrame frameFace = faceTracker.Track(kinect.sensor.ColorStream.Format, pixelsColor, kinect.sensor.DepthStream.Format, pixelsDepth, skeleton);
if (frameFace.TrackSuccessful)
{
//txtTracked.Content = "TRACKED";
//txtRoll.Content = frameFace.Rotation.Z;
//txtPitch.Content = frameFace.Rotation.X;
//txtYaw.Content = frameFace.Rotation.Y;
//Debug.WriteLine("TRACKED " + frameFace.Rotation.Z + frameFace.Rotation.X + frameFace.Rotation.Y);
sceneSubjects[i].headorient_rpy = new List<float> { frameFace.Rotation.Z, frameFace.Rotation.X, frameFace.Rotation.Y };
}
else if (sceneSubjects[i].headorient_rpy != null)
{
sceneSubjects[i].headorient_rpy.Clear();
}
}
}
#endregion
}
environment.numberSubject = sceneSubjects.Count;
WriteStack();
}
#endregion
}
}
// The engineLoop thread executes RunEngine() function. This thread waits until a signal is received
// (frameReady.WaitOne()). In this case, the signal means that the copy of frame bytes is ready.
// Since the engineLoop thread is not the UI thread (the thread which manages the interface),
// it cannot modify objects belonging to the interface. A Dispatcher object (the last line of the function)
// is necessary for updating the interface.
private void RunEngine()
{
while (kinect.frameReady.WaitOne())
{
content = engine.Process(imageIntPr + 1, fWidth, fHeight, 1U, kinect.pixelFeed, lineFeed, 0, "GRAYSCALE");
if (content == null) return;
Canvas_Shore.Dispatcher.BeginInvoke(System.Windows.Threading.DispatcherPriority.Normal,
new Action(delegate()
{
Canvas_Shore.Children.Clear();
}));
List<Subject> sh = new List<Subject>() { };
for (uint i = 0; i < content.GetObjectCount(); i++)
{
try
{
ShoreNetObject sObj = content.GetObject(i);
if (sObj.GetShoreType() == "Face")
{
bool present = false;
System.Windows.Point middleEyes = new System.Windows.Point((sObj.GetMarkerOf("LeftEye").GetX() + sObj.GetMarkerOf("RightEye").GetX()) / 2, sObj.GetMarkerOf("LeftEye").GetY());
sceneSubjectsCopy = sceneSubjects.ToList();
for (int j = 0; j < sceneSubjectsCopy.Count; j++)
{
if (sceneSubjectsCopy[j].normalizedspincenter_xy != null)
{
//calcolo lo spine
double zeta = sceneSubjectsCopy[j].spincenter_xyz[2];
System.Windows.Point spinInCanvas = new System.Windows.Point();
if (sceneSubjectsCopy[j].trackedState)
spinInCanvas = new System.Windows.Point((int)(sceneSubjectsCopy[j].normalizedspincenter_xy[0] * kinect.sensor.DepthStream.FrameWidth),
(int)(kinect.sensor.DepthStream.FrameHeight - (sceneSubjectsCopy[j].normalizedspincenter_xy[1] * kinect.sensor.DepthStream.FrameHeight)));
else
spinInCanvas = new System.Windows.Point((int)(sceneSubjectsCopy[j].normalizedspincenter_xy[0] * kinect.sensor.ColorStream.FrameWidth),
(int)(kinect.sensor.ColorStream.FrameHeight - (sceneSubjectsCopy[j].normalizedspincenter_xy[1] * kinect.sensor.ColorStream.FrameHeight)));
double ErrorX = spinInCanvas.X - middleEyes.X;
if (Math.Abs(ErrorX) < DeltaErrorX)
{
try
{
sceneSubjects[j].gender = (sObj.GetAttributeOf("Gender") == "Female") ? "Female" :"Male";
sceneSubjects[j].age = (int)sObj.GetRatingOf("Age");
sceneSubjects[j].happiness_ratio = sObj.GetRatingOf("Happy");
sceneSubjects[j].surprise_ratio = sObj.GetRatingOf("Surprised");
sceneSubjects[j].anger_ratio = sObj.GetRatingOf("Angry");
sceneSubjects[j].sadness_ratio =sObj.GetRatingOf("Sad");
sceneSubjects[j].uptime = sObj.GetRatingOf("Uptime");
sceneSubjects[j].middleeyes_xy = new List<float>() { (float)middleEyes.X, (float)middleEyes.Y };
present = true;
}
catch (Exception e)
{
Console.WriteLine("Shore Error" + e.Message);
}
break;
}
}
}
if (!present)
{
Subject newSub = new Subject();
newSub.idKinect = 0;
newSub.angle = 0;
newSub.gender = (sObj.GetAttributeOf("Gender") == "Female") ? "Female" : "Male";
newSub.age = (int)sObj.GetRatingOf("Age");
newSub.happiness_ratio = sObj.GetRatingOf("Happy");
newSub.surprise_ratio = sObj.GetRatingOf("Surprised");
newSub.anger_ratio = sObj.GetRatingOf("Angry");
newSub.sadness_ratio = sObj.GetRatingOf("Sad");
newSub.uptime = sObj.GetRatingOf("Uptime");
newSub.middleeyes_xy = new List<float>() { (float)middleEyes.X, (float)middleEyes.Y };
newSub.normalizedspincenter_xy = new List<float>() { };
newSub.spincenter_xyz = new List<float>() { };
newSub.lefthand_xyz = new List<float>(){ };
newSub.righthand_xyz = new List<float>() { };
newSub.head_xyz = new List<float>() { };
newSub.leftwrist_xyz = new List<float>(){ };
newSub.rightwrist_xyz = new List<float>() { };
newSub.leftelbow_xyz = new List<float>() { };
newSub.rightelbow_xyz = new List<float>() { };
newSub.leftshoulder_xyz = new List<float>() { };
newSub.rightshoulder_xyz = new List<float>() { };
sh.Add(newSub);
}
#region draw in canvas
Dictionary<string, float> expRatio = new Dictionary<string, float>()
{
{ "Angry", sObj.GetRatingOf("Angry") },
{ "Happy", sObj.GetRatingOf("Happy") },
{ "Sad", sObj.GetRatingOf("Sad") },
{ "Surprised", sObj.GetRatingOf("Surprised") }
};
float ratioVal = (float)Math.Round((decimal)expRatio.Values.Max(), 1);
string ratioName = expRatio.OrderByDescending(kvp => kvp.Value).First().Key;
// Draw subject information: Gender, age +/- deviation, expression, expression rate
StringBuilder sb = new StringBuilder();
sb.AppendLine(sObj.GetAttributeOf("Gender"));
sb.AppendLine("Age: " + sObj.GetRatingOf("Age") + " +/- " + sObj.GetRatingOf("AgeDeviation"));
sb.AppendLine(ratioName + ": " + ratioVal + "%");
double width = Math.Abs(sObj.GetRegion().GetLeft() - sObj.GetRegion().GetRight());
double height = Math.Abs(sObj.GetRegion().GetTop() - sObj.GetRegion().GetBottom());
double left = sObj.GetRegion().GetLeft();
double top = sObj.GetRegion().GetTop();
double bottom = sObj.GetRegion().GetBottom();
string gender = sObj.GetAttributeOf("Gender");
Canvas_Shore.Dispatcher.BeginInvoke(System.Windows.Threading.DispatcherPriority.Normal,
new Action(delegate()
{
Rectangle rect = new Rectangle();
rect.Width = width;
rect.Height = height;
rect.StrokeThickness = 2;
rect.Stroke = (gender == "Female") ? Brushes.Fuchsia : Brushes.Cyan;
rect.Margin = new Thickness(left, top, 0, 0); //draw the rectangle
Label lab = new Label
{
Foreground = (gender == "Female") ? Brushes.Fuchsia : Brushes.Cyan,
FontSize = 12,
FontWeight = FontWeights.Bold,
Content = sb.ToString(),
Opacity = 1,
Margin = new Thickness(left,bottom, 0, 0) //draw under the rectangle
};
Canvas_Shore.Children.Add(rect);
Canvas_Shore.Children.Add(lab);
}));
#endregion
}
}
catch(Exception e)
{
Console.WriteLine("error shore" + e.Message);
}
}
sceneSubjectsShore = sh;
}
}