private void btNext_Click(object sender, RoutedEventArgs e)
{
PoseCalibrationViewModel dc = (PoseCalibrationViewModel)this.DataContext;
// If for any reason data was not saved. Save it, before going to the next position.
if (!dc.SavedData && dc.FinishedTest)
{
dc.Save();
}
else
{
dc.ResetMessages();
dc.StatusText = "";
}
// Clean up log space. It has already been saved.
dc.ResetLogs();
dc.SavedData = false;
if (dc.Name == "DS")
{
dc.Name = "SS";
dc.PoseCalibrationDone = false;
}
else if (dc.Name == "SS")
{
dc.PoseCalibrationDone = false;
dc.Name = "TS";
// In this case, this is the last position. There is no next.
((Button)sender).Visibility = Visibility.Hidden;
}
this.Refresh();
}
private void UserControl_Loaded(object sender, RoutedEventArgs e)
{
PoseCalibrationViewModel dc = (PoseCalibrationViewModel)this.DataContext;
dc.ResetMessages();
dc.StatusText = "";
}
/// <summary>
/// Comparison between current window of frames and calibration frames using DTW for each tracked joint.
/// </summary>
/// <param name="DCVM">A pointer to calling class. Used for updating status texts/values</param>
private void Compare(PoseCalibrationViewModel DCVM)
{
bool err1, err2, err3, err4 = false;
string txt = "";
err1 = ProcessDTW(ref txt, r1Joints.ToArray(), m1Joints.ToArray(), ref lmean1, ref mean1);
DCVM.LeftHand = txt;
txt = "";
err2 = ProcessDTW(ref txt, r2Joints.ToArray(), m2Joints.ToArray(), ref lmean2, ref mean2);
DCVM.RightHand = txt;
txt = "";
err3 = ProcessDTW(ref txt, r3Joints.ToArray(), m3Joints.ToArray(), ref lmean3, ref mean3);
DCVM.LeftFoot = txt;
txt = "";
err4 = ProcessDTW(ref txt, r4Joints.ToArray(), m4Joints.ToArray(), ref lmean4, ref mean4);
DCVM.RightFoot = txt;
// In general, when someone stumble on the board, there is an error in hands position as well.
// Then, it doesn´s matter exactly where the error (loss of stability) was or how many joints it had affected,
// it counts only 1 error.
if (err1 || err2 || err3 || err4)
{
DCVM.TotalErrors++;
DCVM.StatusText = "Test total errors by now: " + DCVM.TotalErrors;
}
m1Joints.Clear();
m2Joints.Clear();
m3Joints.Clear();
m4Joints.Clear();
}
private void TabControl_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
if (e.Source is TabControl)
{
ApplicationViewModel app = (ApplicationViewModel)DataContext;
if (TabItemConfiguration.IsSelected)
{
if (_configurationViewModel == null)
{
_configurationViewModel = new ConfigurationViewModel(app);
}
app.ChangeViewModel(_configurationViewModel);
}
else if (TabItemParticipants.IsSelected)
{
if (_participantsViewModel == null)
{
_participantsViewModel = new ParticipantsViewModel(app);
}
app.ChangeViewModel(_participantsViewModel);
}
else if (TabItemTests.IsSelected)
{
if (_testsViewModel == null)
{
_testsViewModel = new TestsViewModel(app);
app.ChangeViewModel(_testsViewModel);
}
else
{
PoseCalibrationViewModel dcVM = (PoseCalibrationViewModel)app.PageViewModels.Find(r => r.Name == "DS" || r.Name == "SS" || r.Name == "TS");
if (dcVM != null)
{
app.ChangeViewModel(dcVM);
}
else
{
app.ChangeViewModel(_testsViewModel);
}
}
}
else if (TabItemReview.IsSelected)
{
if (_reviewViewModel == null)
{
_reviewViewModel = new ReviewViewModel(app);
}
app.ChangeViewModel(_reviewViewModel);
}
}
}
private void MenuItem_Click(object sender, RoutedEventArgs e)
{
ApplicationViewModel app = (ApplicationViewModel)DataContext;
PoseCalibrationViewModel dcVM = (PoseCalibrationViewModel)app.PageViewModels.Find(r => r.Name == "DS" || r.Name == "SS" || r.Name == "TS");
if (dcVM != null)
{
dcVM.infoUpdateTimer.Enabled = false;
}
System.Windows.Application.Current.Shutdown();
}
/// <summary>
/// Always start doing the double leg stance test.
/// </summary>
private void Continue()
{
if (_app.participantName == "")
{
StatusText = "Participant not selected.";
return;
}
_app.PreviousPageViewModel = _app.CurrentPageViewModel;
_doubleCalibrationVM = (PoseCalibrationViewModel)_app.PageViewModels.Find(r => (r.Name == "DS"));
if (_doubleCalibrationVM == null)
{
_doubleCalibrationVM = new PoseCalibrationViewModel(_app, "DS");
}
else
{
_doubleCalibrationVM.Name = "DS";
}
_app.ChangeViewModel(_doubleCalibrationVM);
}
/// <summary>
/// How to show the joints on screen.
/// </summary>
/// <param name="joints"></param>
/// <param name="jointPoints"></param>
/// <param name="CoM_Point"></param>
/// <param name="CoM"></param>
/// <param name="DCVM"></param>
private void SaveJoints(IReadOnlyDictionary <JointType, Joint> joints, IDictionary <JointType, Point> jointPoints, Point CoM_Point, CameraSpacePoint CoM, PoseCalibrationViewModel DCVM)
{
string line = "";
foreach (JointType jointType in joints.Keys)
{
line += (int)jointType + ":";
line += jointPoints[jointType].X.ToString("N5") + ":" + jointPoints[jointType].Y.ToString("N5") + ":";
}
// CoP_X and CoP_Y represents centre of pressure on the WBB platform X (M-L variation) Y (A-P variation).
// CoM represents the points on real space related to center of camera.
// Point in the depth space where to show it 2D.
// 3D position in camera space related to center of lens.
line += "COM_X:" + CoM_Point.X.ToString("N5") + ":COM_Y:" + CoM_Point.Y.ToString("N5");
line += ":" + CoM.X.ToString("N5") + ":" + CoM.Y.ToString("N5") + ":" + CoM.Z.ToString("N5");
line += ":CoP_X:" + DCVM.CalculatedCoPX.ToString("N5") + ":CoP_Y:" + DCVM.CalculatedCoPY.ToString("N5");
DCVM.LogJoints2D.Add(line);
}
/// <summary>
/// Real joint positions
/// </summary>
/// <param name="joints">Dictionary with all joint types as keys and joint structure as values</param>
/// <param name="time">Current time in milliseconds (string).</param>
/// <param name="DCVM">Pointer to calling class.</param>
private void SaveJoints3D(IReadOnlyDictionary <JointType, Joint> joints, string time, PoseCalibrationViewModel DCVM)
{
string line = time;
foreach (JointType jointType in joints.Keys)
{
line += ":" + (int)jointType + ":";
if (joints[jointType].TrackingState == TrackingState.Tracked)
{
line += "T" + ":";
}
else if (joints[jointType].TrackingState == TrackingState.Inferred)
{
line += "I" + ":";
}
else
{
line += "N" + ":";
}
line += joints[jointType].Position.X.ToString("N5") + ":" + joints[jointType].Position.Y.ToString("N5") + ":" + joints[jointType].Position.Z.ToString("N5");
}
if (IsTesting)
{
DCVM.LogJoints3D.Add(line);
}
else
{
DCVM.LogJoints3DCal.Add(line);
}
}
/// <summary>
/// Updates the body array with new information from the sensor
/// Should be called whenever a new BodyFrameArrivedEvent occurs
/// </summary>
/// <param name="bodies">Array of bodies to update</param>
public void UpdateBodyFrame(Body[] bodies)
{
PoseCalibrationViewModel DCVM = (PoseCalibrationViewModel)_app.CurrentPageViewModel;
// Process delay time.
if (Delay > 0)
{
DCVM.StatusText = "Delay ... " + Delay;
Delay--;
}
else
{
if (Delay == 0)
{
Delay--;
_firstTime = true;
if (IsCalibrating)
{
DCVM.StartCalibrationTime = DateTime.Now;
DCVM.StatusText = "Starting calibration.";
NumFramesTest = _app.NumFramesTest_DTW;
r1Joints.Clear();
r2Joints.Clear();
r3Joints.Clear();
r4Joints.Clear();
r5Joints.Clear();
}
else if (IsTesting)
{
DCVM.StartTestTime = DateTime.Now;
DCVM.StatusText = "Starting test.";
NumFramesTest = _app.NumFramesTest_DTW;
mean1 = mean2 = mean3 = mean4 = 0;
lmean1.Clear();
lmean2.Clear();
lmean3.Clear();
lmean4.Clear();
m1Joints.Clear();
m2Joints.Clear();
m3Joints.Clear();
m4Joints.Clear();
TestStopWatch.Restart();
}
}
}
if (bodies != null)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
foreach (Body body in bodies)
{
Pen drawPen = new Pen(Brushes.Red, 6);
if (body.IsTracked)
{
_faceSource.TrackingId = body.TrackingId;
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// Calculate Center of Mass using segment method.
CameraSpacePoint CoM = _CoM.CalculateCoM(joints);
if (CoM.Z < 0)
{
CoM.Z = InferredZPositionClamp;
}
DepthSpacePoint Location_CoM = coordinateMapper.MapCameraPointToDepthSpace(CoM);
Point CoM_Point = new Point(Location_CoM.X, Location_CoM.Y);
// 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;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
// Calibration or Test.
if ((IsCalibrating || IsTesting) && Delay < 0)
{
// Normalize monitored joints based on the trunk height - Scaling
if (_firstTime)
{
p1 = joints[JointType.SpineShoulder].Position;
p2 = joints[JointType.SpineBase].Position;
_normalization = ((float)Math.Sqrt(Math.Pow(p1.X - p2.X, 2) +
Math.Pow(p1.Y - p2.Y, 2) + Math.Pow(p1.Z - p2.Z, 2)));
_firstTime = false;
}
Normalize(ref r1, joints[JointType.HandLeft].Position, CoM, _normalization);
Normalize(ref r2, joints[JointType.HandRight].Position, CoM, _normalization);
Normalize(ref r3, joints[JointType.FootLeft].Position, CoM, _normalization);
Normalize(ref r4, joints[JointType.FootRight].Position, CoM, _normalization);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// C A L I B R A T I ON
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Pose calibration, recording positions to get a reference of them when they are theoretically stable.
if (IsCalibrating)
{
// Log Center of Mass position.
double elapsed = (DateTime.Now - DCVM.StartCalibrationTime).TotalMilliseconds;
SaveJoints3D(joints, elapsed.ToString("F0"), DCVM);
DCVM.LogCalLines.Add("CoM:" + elapsed.ToString("F0") + ":" + CoM.X.ToString("N3") + ":" + CoM.Y.ToString("N3")
+ ":" + CoM.Z.ToString("N3"));
// Get data series of the main joint positions for position.
if (NumFramesTest > 0)
{
// Stores normalized positions.
r1Joints.Add(r1); // Hand left
r2Joints.Add(r2); // Hand right
r3Joints.Add(r3); // Foot left
r4Joints.Add(r4); // Foot right
r5Joints.Add(joints[JointType.Head].Position); // Head - check trunk angle.
NumFramesTest--;
}
else if (NumFramesTest == 0)
{
r5.X = r5Joints.Select(val => val.X).Average(); // Head position (mean)
r5.Y = r5Joints.Select(val => val.Y).Average();
r5.Z = r5Joints.Select(val => val.Z).Average();
NumFramesTest--;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// T E S T
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Checks if user is in the right pose (balanced) or unbalanced
if (IsTesting)
{
string timeStr = TestStopWatch.ElapsedMilliseconds.ToString("F0");
DCVM.LogTestLines.Add("CoM:" + timeStr + ":" + CoM.X.ToString("N6") + ":" + CoM.Y.ToString("N6") + ":" + CoM.Z.ToString("N6"));
// save all positions to have everything if needed.
SaveJoints3D(joints, timeStr, DCVM);
// we are saving here the 2D positions that will be shown at screen, as well the CoM.
if (_app.SaveJoints)
{
SaveJoints(joints, jointPoints, CoM_Point, CoM, DCVM);
}
if (NumFramesTest > 0)
{
m1Joints.Add(r1);
m2Joints.Add(r2);
m3Joints.Add(r3);
m4Joints.Add(r4);
NumFramesTest--;
}
else if (NumFramesTest == 0)
{
Compare(DCVM);
NumFramesTest = _app.NumFramesTest_DTW;
}
// Get current positions of spineBase and Head.
CameraSpacePoint spineBase = body.Joints[JointType.SpineBase].Position;
CameraSpacePoint head = body.Joints[JointType.Head].Position;
double currAngle = Util.ScalarProduct(r5, spineBase, head);
bool tiltBodyOk = Util.ScalarProduct(r5, spineBase, head) <= _app.AnglePrecision;
string status = tiltBodyOk ? "OK " : "NOK ";
status += currAngle.ToString("N1");
DCVM.TrunkSway = status;
if (!tiltBodyOk)
{
if (_PoseErrorCounter > _NumFramesThresholdError)
{
_PoseErrorCounter = 0;
DCVM.TotalErrors++;
DCVM.LogTestLines.Add("JointError:" + timeStr + ":" + DCVM.TotalErrors);
}
else
{
_PoseErrorCounter++;
}
}
else
{
_PoseErrorCounter = 0;
}
} // end execute double stance test - analyze posture.
}
// Text to show eyes status on drawing space.
string faceText = "EYES- ";
// Only analyze eyes if it has a tracked faced.
if (_faceResult != null)
{
string timeStr = TestStopWatch.ElapsedMilliseconds.ToString("F0");
var eyeLeftClosed = _faceResult.FaceProperties[FaceProperty.LeftEyeClosed];
var eyeRightClosed = _faceResult.FaceProperties[FaceProperty.RightEyeClosed];
if (eyeLeftClosed == DetectionResult.No || eyeRightClosed == DetectionResult.No)
{
if (eyeLeftClosed == DetectionResult.No)
{
faceText += "LEFT ";
}
if (eyeRightClosed == DetectionResult.No)
{
faceText += "RIGHT ";
}
faceText += "OPEN";
if (IsTesting && Delay < 0)
{
DCVM.LogTestLines.Add("EYES:" + timeStr + ":" + faceText);
if (_EyesErrorCounter > _NumFramesThresholdError)
{
DCVM.TotalErrors++;
DCVM.StatusText = "Eye(s) opened error";
DCVM.LogTestLines.Add("EyesError:" + timeStr + ":" + DCVM.TotalErrors);
_EyesErrorCounter = 0;
}
else
{
_EyesErrorCounter++;
}
}
}
else
{
faceText += "CLOSED";
if (IsTesting && Delay < 0)
{
DCVM.LogTestLines.Add("EYES:" + timeStr + ":" + faceText);
}
_EyesErrorCounter = 0;
}
dc.DrawText(new FormattedText(faceText, CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Georgia"), 25, Brushes.White), new Point(10, 10));
}
this.DrawBody(joints, jointPoints, dc, drawPen, true);
// this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
// this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
dc.DrawEllipse(Brushes.White, null, CoM_Point, CoMSize, CoMSize);
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
