/// <summary> /// Stores virtual accleration data into buffer /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void OnIdealSensorNewTData(object sender, VirtualSensorData e) { //TO DO: IF data gets pulled into buffer, gets tagged with section ID? Wait.. thought we were tagging all data touched?? //regardless, no longer doing this here. //e.section = this.CalibrationCount; //DataTracker.CurrentSection = DataTracker.SectionCounter; //derivation and store into buffer AccelerationTime t = new AccelerationTime(); if (ICherryPicker.isDataGood(e.acceleration, !e.isinferredornottracked)) { //IF THIS CONDITIONAL IS CHANGED, IT MUST ALSO BE CHANGED IN THE DATA PRODUCERS. t.setVal(e.acceleration, e.NowInTicks, !e.isinferredornottracked); //if it's good, assign the proper current value - maybe we need to move this if statement to sensor producer virtualAcc.Enqueue(t); LastVirtualFiltered = false; } else { LastVirtualFiltered = true; //DataTracker.CurrentSection = 0; } }
private void OnNewTData(object sender, KinectData e) { double[] virtualAcc = new double[] { 0, 0, 0 }; double[] virtualVel = new double[] { 0, 0, 0 }; double[] pos = new double[] { e.Joints[FurtherJoint].Position.X, e.Joints[FurtherJoint].Position.Y, e.Joints[FurtherJoint].Position.Z }; TrackingState trackingstate = e.Body.Joints[this.FurtherJoint].TrackingState; //= MathFunctions.midpoint(e.Joints[FurtherJoint], e.Joints[CloserJoint], 100); X_Filter.UpdateVal(pos[0], e.NowInTicks); Y_Filter.UpdateVal(pos[1], e.NowInTicks); Z_Filter.UpdateVal(pos[2], e.NowInTicks); //acceleration var temp = X_Filter.GetNDerivative(nd); virtualAcc[0] = temp.Item1; virtualAcc[1] = Y_Filter.GetNDerivative(nd).Item1 + 9.81; //hack make z - virtualAcc[2] = -Z_Filter.GetNDerivative(nd).Item1; //position double[] virtualPos = new double[] { pos[0], pos[1], pos[2] }; vsd = new VirtualSensorData(); vsd.NowInTicks = temp.Item2; //vsd.NowInTicks = e.NowInTicks; for (int i = 0; i < 3; i++) { vsd.acceleration[i] = virtualAcc[i]; vsd.velocity[i] = virtualVel[i]; vsd.position[i] = virtualPos[i]; } //Rotation vsd.rot = e.Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion(); //Apply filter under this note: X,Y,Z,W //Begin Quat Filter #region QuatFilter double[] quat = new double[] { vsd.rot.W, vsd.rot.X, vsd.rot.Y, vsd.rot.Z }; //If this is the first data point, record that. The filter will then not be triggered on the first data point //Assuming first data set is correct otherwise all values will be incorrectly flipped. Can improve conditionals later (delay for high confidence data, etc). if (DataTracker.vsdFirstQuat[0] == 0 && DataTracker.vsdFirstQuat[1] == 0 && DataTracker.vsdFirstQuat[2] == 0 && DataTracker.vsdFirstQuat[3] == 0) { DataTracker.vsdFirstQuat = quat; DataTracker.mvsdPrevQuat = quat; } int counter = 0; for (int i = 0; i < 4; i++) { if (System.Math.Sign(DataTracker.mvsdPrevQuat[i]) != System.Math.Sign(quat[i])) { counter++; } } if (counter == 4) { DataTracker.FlipCounter++; for (int j = 0; j < 4; j++) { quat[j] = -quat[j]; } } //NOTE: just for right wrist, this can be generalized later. Only keep high confidence reference quaternions (except for first data point) if (e.Body.Joints[JointType.ElbowRight].TrackingState == TrackingState.Tracked && e.Body.Joints[JointType.ShoulderRight].TrackingState == TrackingState.Tracked && e.Body.Joints[JointType.WristRight].TrackingState == TrackingState.Tracked) { for (int i = 0; i < 4; i++) { DataTracker.mvsdPrevQuat[i] = quat[i]; } } vsd.rot.W = quat[0]; vsd.rot.X = quat[1]; vsd.rot.Y = quat[2]; vsd.rot.Z = quat[3]; #endregion //End Rotation Filter if (this.FurtherJoint == JointType.WristRight) { //Only do strict tracking when right wrist is selected TrackingState wristState = e.Body.Joints[JointType.WristRight].TrackingState; HandState handState = e.Body.HandRightState; TrackingState thumbState = e.Body.Joints[JointType.ThumbRight].TrackingState; vsd.isinferredornottracked = (wristState != TrackingState.Tracked) || (thumbState != TrackingState.Tracked) || (handState != HandState.Open); } else { vsd.isinferredornottracked = (trackingstate == TrackingState.Inferred) || (trackingstate == TrackingState.NotTracked); } //If in Calibrator Setup (note: change calibrator setup to bool later? - 0 means not in setup) and the data is good, tag it. if (ICherryPicker.isDataGood(vsd.acceleration, !vsd.isinferredornottracked)) //don't need to repeat this in mappedVirtual { vsd.section = DataTracker.CurrentSection; DataTracker.ValidVSD = true; } else { vsd.section = 0; DataTracker.ValidVSD = false; } if (this.NewIData != null) { this.NewIData(this, vsd); //[not edited] This triggers data collection? (So changes must be before this) } if (this.NewTData != null) { this.NewTData(this, vsd); } }