public void CalibrateFoot(Trackable calibrationTrackable)
{
//Stromohab_MCE_Connection.TCPProcessor.TrackableListReceivedEvent +=
// new TCPProcessor.TrackableListReceivedHandler(TCPProcessor_TrackableListReceivedEvent);
if (footName == "left")
{
currentID = 1;
}
if (footName == "right")
{
currentID = 2;
}
this.calibrationTrackable = calibrationTrackable;
previousTrackable = calibrationTrackable;
PositionData positionCalibration = PositionData.ConvertMarkerPosition((Marker)calibrationTrackable);
this.footPositionList.Add(positionCalibration);
footdown_yCoordinate = calibrationTrackable.yCoordinate;
xDirection = 0; // -1 -> indicates heading in -x direction
yDirection = 0; // +1 -> +y direction etc
zDirection = 0; // +1 (+z) is forwards, towards screen; -1 -> moving towards back of treadmill
xPrevDirection = 0;
yPrevDirection = 0;
zPrevDirection = 0;
zPreviousCoordinate = previousTrackable.zCoordinate;
yPreviousCoordinate = previousTrackable.yCoordinate;
xPreviousCoordinate = previousTrackable.xCoordinate;
}
public void CalibrateFoot(Marker calibrationMarker)
{
footPositionList = new List <PositionData>();
//TCPProcessor.WholeFrameReceivedEvent +=
// new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
this.calibrationMarker = calibrationMarker;
previousMarker = calibrationMarker;
//
currentMarker = new Marker(previousMarker);
//
PositionData positionCalibration = PositionData.ConvertMarkerPosition(calibrationMarker);
this.footPositionList.Add(positionCalibration);
footdown_yCoordinate = calibrationMarker.yCoordinate;
currentID = calibrationMarker.MarkerId;
xDirection = 0; // -1 -> indicates heading in -x direction
yDirection = 0; // +1 -> +y direction etc
zDirection = 0; // +1 (+z) is forwards, towards screen; -1 -> moving towards back of treadmill
xPrevDirection = 0;
yPrevDirection = 0;
zPrevDirection = 0;
zPreviousCoordinate = previousMarker.zCoordinate;
yPreviousCoordinate = previousMarker.yCoordinate;
xPreviousCoordinate = previousMarker.xCoordinate;
}
public List<List<PositionData>> ReadDeswappedPositionDataFromCSVFile(string filePath)
{
List<List<PositionData>> fullDataList = new List<List<PositionData>>();
try
{
using (StreamReader readFile = new StreamReader(filePath))
{
List<PositionData> currentLeftData = new List<PositionData>();
List<PositionData> currentRightData = new List<PositionData>();
string line;
string[] row;
while ((line = readFile.ReadLine()) != null)
{
row = line.Split(',');
if (row[0].Contains("Time"))
{
line = readFile.ReadLine();
row = line.Split(',');
}
PositionData currentPositionLeftMarker = new PositionData();
PositionData currentPositionRightMarker = new PositionData();
currentPositionLeftMarker.MarkerID = 0;
currentPositionRightMarker.MarkerID = 1;
currentPositionLeftMarker.Time = Convert.ToInt32(row[0]);
currentPositionLeftMarker.XCoordinate = Convert.ToInt32(row[1]);
currentPositionLeftMarker.YCoordinate = Convert.ToInt32(row[2]);
currentPositionLeftMarker.ZCoordinate = Convert.ToInt32(row[3]);
currentPositionRightMarker.Time = Convert.ToInt32(row[0]);
currentPositionRightMarker.XCoordinate = Convert.ToInt32(row[5]);
currentPositionRightMarker.YCoordinate = Convert.ToInt32(row[6]);
currentPositionRightMarker.ZCoordinate = Convert.ToInt32(row[7]);
currentLeftData.Add(currentPositionLeftMarker);
currentRightData.Add(currentPositionRightMarker);
}
fullDataList.Add(currentLeftData);
fullDataList.Add(currentRightData);
return (fullDataList);
}
}
catch (IOException ex)
{
System.Windows.Forms.MessageBox.Show(ex.Message, "Failed To Open File");
return null;
}
}
public static PositionData ConvertMarkerPosition(Marker marker)
{
PositionData position = new PositionData();
position.MarkerID = marker.MarkerId;
position.Time = marker.TimeStamp;
position.XCoordinate = marker.xCoordinate;
position.YCoordinate = marker.yCoordinate;
position.ZCoordinate = marker.zCoordinate;
return position;
}
public static PositionData ConvertMarkerPosition(Marker marker)
{
PositionData position = new PositionData();
position.MarkerID = marker.MarkerId;
position.Time = marker.TimeStamp;
position.XCoordinate = marker.xCoordinate;
position.YCoordinate = marker.yCoordinate;
position.ZCoordinate = marker.zCoordinate;
return(position);
}
private void TCPProcessor_WholeFrameReceivedEvent()
{
if (MarkerList.listOfMarkers.Count == 2)
{
List <PositionData> tempMarkerList = new List <PositionData>();
foreach (Marker singleMarker in MarkerList.listOfMarkers)
{
PositionData markerData = PositionData.ConvertMarkerPosition(singleMarker);
tempMarkerList.Add(markerData);
}
allPositionData.Add(tempMarkerList);
}
}
private void TCPProcessor_WholeFrameReceivedEvent()
{
if (MarkerList.listOfMarkers.Count == 2)
{
//FrontFoot(MarkerList.listOfMarkers[0], MarkerList.listOfMarkers[1]);
left.PreviousMarker = new Marker(left.CurrentMarker);
right.PreviousMarker = new Marker(right.CurrentMarker);
AssignFootMarkers(MarkerList.listOfMarkers);
//currentMarker = MarkerList.listOfMarkers[currentID]; //elaborate marker assignment to account for swapping
//if (currentID != previousID) System.Diagnostics.Debug.WriteLine("Possible marker swap: " + currentMarker.TimeStamp);
left.FootPositionList.Add(PositionData.ConvertMarkerPosition(left.CurrentMarker));
right.FootPositionList.Add(PositionData.ConvertMarkerPosition(right.CurrentMarker));
left.MarkerTrack();
right.MarkerTrack();
}
}
/*#region Foot overshoot
* private void Foot_xFootPeakDetected(Foot foot)
* {
* if (Foot.Frontfoot != null)
* {
* if (foot.FootName == Foot.Frontfoot.FootName)
* {
* if (foot.CurrentMarker.yCoordinate > foot.FootDown_yThreshold + 5)
* {
* PositionData accuracyData = new PositionData();
* if ((foot.FootName == "left") && (foot.CurrentMarker.xCoordinate > 0))
* {
* accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
* accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
* LeftOvershootList.Add(accuracyData);
* System.Diagnostics.Debug.WriteLine("Left foot overshoot:" + accuracyData.XCoordinate);
* }
* if ((foot.FootName == "right") && (foot.CurrentMarker.xCoordinate < 0))
* {
* accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
* accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
* RightOvershootList.Add(accuracyData);
* System.Diagnostics.Debug.WriteLine("Right foot overshoot:" + accuracyData.XCoordinate);
* }
* }
* }
* }
* }
#endregion*/
#region Foot down accuracy
private void Foot_FootDownDetected(Foot foot)
{
if (footdownevents++ > 2)//number of events to miss before recording data
{
//if (teststart == 0) teststart = foot.CurrentMarker.TimeStamp; //save time at start
PositionData accuracyData = new PositionData();
accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
System.Diagnostics.Debug.WriteLine(foot.FootName + "Down" + " Y:" + foot.CurrentMarker.yCoordinate + "(" + foot.FootDown_yThreshold + ")" + " Position:" + accuracyData.XCoordinate + " Time:" + accuracyData.Time);
if (foot.FootName == "left")
{
LeftList.Add(accuracyData);
}
if (foot.FootName == "right")
{
RightList.Add(accuracyData);
}
}
}
private void TCPProcessor_TrackableListReceivedEvent(List <Trackable> newTrackableList)
{
trackableList = new List <Trackable>(newTrackableList);
Marker m1 = (Marker)(trackableList.Find(LeftFoot));
Marker m2 = (Marker)(trackableList.Find(RightFoot));
if (currentID == 1)
{
currentMarker = m1;
}
if (currentID == 2)
{
currentMarker = m2;
}
PositionData footData = PositionData.ConvertMarkerPosition(currentMarker);
this.footPositionList.Add(footData);
MarkerTrack();
//FrontFoot(m1, m2);
}
/*#region Foot overshoot
private void Foot_xFootPeakDetected(Foot foot)
{
if (Foot.Frontfoot != null)
{
if (foot.FootName == Foot.Frontfoot.FootName)
{
if (foot.CurrentMarker.yCoordinate > foot.FootDown_yThreshold + 5)
{
PositionData accuracyData = new PositionData();
if ((foot.FootName == "left") && (foot.CurrentMarker.xCoordinate > 0))
{
accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
LeftOvershootList.Add(accuracyData);
System.Diagnostics.Debug.WriteLine("Left foot overshoot:" + accuracyData.XCoordinate);
}
if ((foot.FootName == "right") && (foot.CurrentMarker.xCoordinate < 0))
{
accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
RightOvershootList.Add(accuracyData);
System.Diagnostics.Debug.WriteLine("Right foot overshoot:" + accuracyData.XCoordinate);
}
}
}
}
}
#endregion*/
#region Foot down accuracy
private void Foot_FootDownDetected(Foot foot)
{
if (footdownevents++ > 2)//number of events to miss before recording data
{
//if (teststart == 0) teststart = foot.CurrentMarker.TimeStamp; //save time at start
PositionData accuracyData = new PositionData();
accuracyData.XCoordinate = foot.CurrentMarker.xCoordinate;
accuracyData.Time = foot.CurrentMarker.TimeStamp - teststart;
System.Diagnostics.Debug.WriteLine(foot.FootName + "Down" + " Y:" + foot.CurrentMarker.yCoordinate + "(" + foot.FootDown_yThreshold + ")" + " Position:" + accuracyData.XCoordinate + " Time:" + accuracyData.Time);
if (foot.FootName == "left") LeftList.Add(accuracyData);
if (foot.FootName == "right") RightList.Add(accuracyData);
}
}
public List<List<PositionData>> ReadFeetDown_Accuracy(string filePath)
{
List<List<PositionData>> fullDataList = new List<List<PositionData>>();
try
{
using (StreamReader readFile = new StreamReader(new FileStream(filePath,FileMode.Open, FileAccess.Read, FileShare.Read)))
{
List<PositionData> currentLeftData = new List<PositionData>();
List<PositionData> currentRightData = new List<PositionData>();
string line;
string[] row;
while ((line = readFile.ReadLine()) != null)
{
row = line.Split(',');
if (row[0].Contains("Time"))
{
line = readFile.ReadLine();
//line = readFile.ReadLine();
//line = readFile.ReadLine();
row = line.Split(',');
}
if (row[0] == "")
{
break;
}
else
{
PositionData currentPositionLeftMarker = new PositionData();
PositionData currentPositionRightMarker = new PositionData();
currentPositionLeftMarker.MarkerID = 0;
currentPositionRightMarker.MarkerID = 1;
if (row[0] != "")
{
currentPositionLeftMarker.Time = Convert.ToInt32(row[0]);
currentPositionLeftMarker.XCoordinate = Convert.ToInt32(row[1]);
currentPositionLeftMarker.YCoordinate = -9999.99;
currentPositionLeftMarker.ZCoordinate = -9999.99;
currentLeftData.Add(currentPositionLeftMarker);
}
if (row[3] != "")
{
currentPositionRightMarker.Time = Convert.ToInt32(row[2]);
currentPositionRightMarker.XCoordinate = Convert.ToInt32(row[3]);
currentPositionRightMarker.YCoordinate = -9999.99;
currentPositionRightMarker.ZCoordinate = -9999.99;
currentRightData.Add(currentPositionRightMarker);
}
}
}
fullDataList.Add(currentLeftData);
fullDataList.Add(currentRightData);
return (fullDataList);
}
}
catch (IOException ex)
{
System.Windows.Forms.MessageBox.Show(ex.Message, "Failed To Open File");
return null;
}
}
private void FootDownAccuracy(int[,] xData, int[,] yData, string filepath)
{
int c = 0;
int leftDownY_threshold = yData[0, 1];
int rightDownY_threshold = yData[0, 2];
int median = 0;
int xMean = 0;
List <PositionData> left = new List <PositionData>();
List <PositionData> right = new List <PositionData>();
int i = xData.Length / 3;
//bool leftLock = false;
//bool rightLock = false;
int leftDownStart = 0;
int leftDownStop = 0;
int rightDownStart = 0;
int rightDownStop = 0;
PositionData leftdown;
PositionData rightdown;
for (int j = 1; j < i; j++)
{ //Left foot down detect
if (yData[j, 1] < leftDownY_threshold + 2)
{
if (leftDownStart == 0)
{
leftDownStart = j;
}
}
else
{
if (leftDownStart != 0)
{
if ((j - leftDownStart) < 14)//check if there's just a blip in the Y
{
leftDownStart = 0;
}
else
{
leftDownStop = j;
median = leftDownStart + (leftDownStop - leftDownStart) / 4;
if ((median > 0) && (median < (i - 1)))//check within bounds of array
{
xMean = (xData[(median - 1), 1] + xData[median, 1] + xData[(median + 1), 1]) / 3;
leftdown = new PositionData(0, xData[median, 0], xMean, yData[median, 1], 0);
left.Add(leftdown);
}
leftDownStart = 0;
leftDownStop = 0;
median = 0;
xMean = 0;
}
}
}
//Right foot down detect
if (yData[j, 2] < rightDownY_threshold)
{
if (rightDownStart == 0)
{
rightDownStart = j;
}
}
else
{
if (rightDownStart != 0)
{
if ((j - rightDownStart) < 14)
{
rightDownStart = 0;
}
else
{
rightDownStop = j;
median = rightDownStart + (rightDownStop - rightDownStart) / 4;
if ((median > 0) && (median < (i - 1)))
{
xMean = (xData[median - 1, 2] + xData[median, 2] + xData[median + 1, 2]) / 3;
rightdown = new PositionData(0, xData[median, 0], xMean, yData[median, 1], 0);
right.Add(rightdown);
}
rightDownStart = 0;
rightDownStop = 0;
median = 0;
xMean = 0;
}
}
}
}
Environment.CurrentDirectory = lblLoadPath.Text + "Trajectorized\\";
lblWritingFile.Text = "Writing file: " + filepath;
StreamWriter toFile = new StreamWriter(filepath, false);
toFile.Write("Time, LeftDown_X, Time, RightDown_X\n");
while ((c < left.Count) && (c < right.Count))
{
toFile.Write(left[c].Time + "," + left[c].XCoordinate + "," + right[c].Time + "," + right[c].XCoordinate + "\n");
c++;
}
toFile.Flush();
toFile.Close();
}
private void UnSwapMarkersInList(string ID, string testname)
{
if (markerPositionData != null)
{
int c = 0;
double[] dif3D;
while (++c < markerPositionData.Count)
{
dif3D = new double[4];
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
double difX = markerPositionData[c - 1][i].XCoordinate - markerPositionData[c][j].XCoordinate;//find current sample nearest to previous sample0
double difY = markerPositionData[c - 1][i].YCoordinate - markerPositionData[c][j].YCoordinate;
double difZ = markerPositionData[c - 1][i].ZCoordinate - markerPositionData[c][j].ZCoordinate;
dif3D[i * 2 + j] = Math.Sqrt(Math.Pow(difX, 2) + Math.Pow(difY, 2) + Math.Pow(difZ, 2));
}
}
//if ((markerPositionData[c][0].Time > 80259456) && (markerPositionData[c][0].Time < 80260049))
//{
// System.Diagnostics.Debug.WriteLine("Time: " + markerPositionData[c][0].Time + " dif3D: " + (int)(dif3D[0]+0.5) + "\t" + (int)(dif3D[1]+0.5) + "\t" + (int)(dif3D[2]+0.5) + "\t" + (int)(dif3D[3]+0.5));
//}
bool swap = true;
//if ((dif3D[0] < dif3D[1]) && (dif3D[0] < dif3D[2])) swap = false;
//if ((dif3D[0] < dif3D[1]) && (dif3D[3] < dif3D[2])) swap = false;
//if ((dif3D[3] < dif3D[1]) && (dif3D[3] < dif3D[2])) swap = false;
if ((dif3D[0] + dif3D[3]) < (dif3D[1] + dif3D[2]))
{
swap = false;
}
foreach (string item in txtIgnoreSwaps.Lines)
{
try
{
if (Convert.ToDouble(item) == markerPositionData[c][0].Time)
{
swap = false;
}
}
catch (FormatException)
{
}
}
if (swap == true)
{
PositionData tempSample = markerPositionData[c][1];
markerPositionData[c][1] = markerPositionData[c][0];
markerPositionData[c][0] = tempSample;
}
}
Environment.CurrentDirectory = lblLoadPath.Text;
if (!Directory.Exists("Trajectorized"))
{
Directory.CreateDirectory("Trajectorized");
}
if (!Directory.Exists("Trajectorized\\" + ID))
{
Directory.CreateDirectory("Trajectorized\\" + ID);
}
DataToFile("Trajectorized\\" + ID + "\\" + testname + "_PostUnSwapped.csv");
}
}
private void FootDownAccuracy(int[,] xData, int[,]yData, string filepath)
{
int c = 0;
int leftDownY_threshold = yData[0, 1];
int rightDownY_threshold = yData[0, 2];
int median = 0;
int xMean = 0;
List<PositionData> left = new List<PositionData>();
List<PositionData> right = new List<PositionData>();
int i = xData.Length / 3;
//bool leftLock = false;
//bool rightLock = false;
int leftDownStart = 0;
int leftDownStop = 0;
int rightDownStart = 0;
int rightDownStop = 0;
PositionData leftdown;
PositionData rightdown;
for (int j = 1; j < i; j++)
{ //Left foot down detect
if (yData[j, 1] < leftDownY_threshold+2)
{
if (leftDownStart == 0)
{
leftDownStart = j;
}
}
else
{
if (leftDownStart != 0)
{
if ((j - leftDownStart) < 14)//check if there's just a blip in the Y
{
leftDownStart = 0;
}
else
{
leftDownStop = j;
median = leftDownStart + (leftDownStop - leftDownStart) / 4;
if ((median > 0) && (median < (i - 1)))//check within bounds of array
{
xMean = (xData[(median - 1), 1] + xData[median, 1] + xData[(median + 1), 1]) / 3;
leftdown = new PositionData(0, xData[median, 0], xMean, yData[median, 1], 0);
left.Add(leftdown);
}
leftDownStart = 0;
leftDownStop = 0;
median = 0;
xMean = 0;
}
}
}
//Right foot down detect
if (yData[j, 2] < rightDownY_threshold)
{
if (rightDownStart == 0)
{
rightDownStart = j;
}
}
else
{
if (rightDownStart != 0)
{
if ((j - rightDownStart) < 14)
{
rightDownStart = 0;
}
else
{
rightDownStop = j;
median = rightDownStart + (rightDownStop - rightDownStart) / 4;
if ((median > 0) && (median < (i - 1)))
{
xMean = (xData[median - 1, 2] + xData[median, 2] + xData[median + 1, 2]) / 3;
rightdown = new PositionData(0, xData[median, 0], xMean, yData[median, 1], 0);
right.Add(rightdown);
}
rightDownStart = 0;
rightDownStop = 0;
median = 0;
xMean = 0;
}
}
}
}
Environment.CurrentDirectory = lblLoadPath.Text + "Trajectorized\\";
lblWritingFile.Text = "Writing file: " + filepath;
StreamWriter toFile = new StreamWriter(filepath, false);
toFile.Write("Time, LeftDown_X, Time, RightDown_X\n");
while ((c < left.Count) && (c < right.Count))
{
toFile.Write(left[c].Time + "," + left[c].XCoordinate + "," + right[c].Time + "," + right[c].XCoordinate + "\n");
c++;
}
toFile.Flush();
toFile.Close();
}
