// Handle new pose update event
private void ProcessAllData(Object source, System.Timers.ElapsedEventArgs e, string messageString)
{
// Fetch data from NatNet
NatNetClient.NatNetPoseData nnPoseData = natNetClient_.FetchFrameData();
// Log latest NatNet data to optitrack log file
LogData(nnPoseData);
index = handledata(nnPoseData, index, messageString);
int length = messageString.Length;
//if the phrase has been completed or gestured wrong and we want to redo it
if (index > length + 2 | retry2_)
{
retry2_ = false;
// Update done key
holoDataClient_.UpdateDoneKeyState(2);
//if in training mode, then reset QZMP
if (train_)
{
holoDataClient_.UpdateTrainKeyState('Q', 0);
holoDataClient_.UpdateTrainKeyState('Z', 0);
holoDataClient_.UpdateTrainKeyState('M', 0);
holoDataClient_.UpdateTrainKeyState('P', 0);
}
Console.WriteLine("Completed: " + messageString);
Console.WriteLine("Char Count: " + length);
Console.WriteLine("===============================\n\n");
// Write out logs
WriteToFile(otLogFile_, otDataLog_);
otDataLog_ = "";
WriteToFile(traceLogFile_, traceDataLog_);
traceDataLog_ = "";
// Increment index to update the KB pose based on latest data
GetLatestHLdata();
//clearHoloLensData();
SendhHoloLensData(rawStimulus_);
timer_.Enabled = false;
timer_.Dispose();
//checkend = false;
index = 1;
// Send message to server
string messageRequest = "Enter new word";
string newWord = getInput(messageRequest);
NewPhrase(newWord);
}
}
// Handle new pose update event
private void ProcessAllData(Object source, System.Timers.ElapsedEventArgs e)
{
NatNetClient.NatNetPoseData nnPoseData = natNetClient_.FetchFrameData();
index = handledata(nnPoseData, index);
if (index > 7)
{
Console.Write("playful");
holoDataClient_.SendhHoloLensData();
timer_.Enabled = false;
}
LogData(nnPoseData);
}
private void LogData(NatNetClient.NatNetPoseData nnPoseData)
{
string log = "";
log += string.Format("{0:F6},{1:F6},{2:F6},", nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z);
// Add timestamp
string timestamp = Math.Round((System.DateTime.Now - logStartTime_).TotalMilliseconds).ToString();
log += string.Format("{0}", timestamp);
WriteToLogMRB(log);
}
// Handle new pose update event
private void ProcessAllData(Object source, System.Timers.ElapsedEventArgs e, string messageString)
{
NatNetClient.NatNetPoseData nnPoseData = natNetClient_.FetchFrameData();
index = handledata(nnPoseData, index, messageString);
var words = messageString.Split(null);
int wordcount = words.Length;
int length = messageString.Length - wordcount + 1;
// Special case if require spaces, don't subtract wordcount (i.e. n spaces)
if (requireSpaces_)
{
length = messageString.Length;
}
if (index > length + 2)
{
// Update done key
holoDataClient_.UpdateDoneKeyState(2);
Console.WriteLine("Completed: " + messageString);
Console.WriteLine("Char Count: " + length);
Console.WriteLine("===============================\n\n");
// Increment index to update the KB pose based on latest data
GetLatestHLdata();
//clearHoloLensData();
SendhHoloLensData(messageString);
timer_.Enabled = false;
timer_.Dispose();
//checkend = false;
index = 1;
//Console.WriteLine("Enter new word");
// Send message to server
string messageRequest = "Enter new word";
string newWord = getInput(messageRequest);
NewPhrase(newWord);
}
LogData(nnPoseData);
}
private void LogData(NatNetClient.NatNetPoseData nnPoseData)
{
string log = "";
log += string.Format("{0:F6},{1:F6},{2:F6},", nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z);
log += string.Format("{0:F6},{1:F6},{2:F6},", nnPoseData.mPos.X, nnPoseData.mPos.Y, nnPoseData.mPos.Z);
// Add timestamp
string timestamp = Math.Round((System.DateTime.Now - logStartTime_).TotalMilliseconds).ToString();
log += string.Format("{0}", timestamp);
// Append to data log
otDataLog_ += log + "\n";
// WriteToFile(otLogFile_, log);
}
// Handle new pose update event
private void PoseDataReceived(HoloDataClient.HoloPose poseData)
{
Console.WriteLine("\n");
//Console.WriteLine("== HoloLens Data Update ==");
string display = "";
display += string.Format("\tpos ({0:N3}, {1:N3}, {2:N3})", poseData.camPos.X, poseData.camPos.Y, poseData.camPos.Z);
display += string.Format("\t\trot ({0:N3}, {1:N3}, {2:N3}, {2:N3})", poseData.camRot.W, poseData.camRot.X, poseData.camRot.Y, poseData.camRot.Z);
//Console.WriteLine(display);
NatNetClient.NatNetPoseData nnPoseData = natNetClient_.FetchFrameData();
// Assemble log line
string log = "";
log += string.Format("{0:F6},{1:F6},{2:F6},", poseData.camPos.X, poseData.camPos.Y, poseData.camPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", poseData.camRot.W, poseData.camRot.X, poseData.camRot.Y, poseData.camRot.Z);
log += string.Format("{0:F6},{1:F6},{2:F6},", nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z);
// Add timestamp
string timestamp = Math.Round((System.DateTime.Now - logStartTime_).TotalMilliseconds).ToString();
log += string.Format("{0}", timestamp);
WriteToLog(log);
List <double> hldata = new List <double>()
{
poseData.kbPos.X, poseData.kbPos.Y, poseData.kbPos.Z, poseData.kbRot.W, poseData.kbRot.X, poseData.kbRot.Y, poseData.kbRot.Z, poseData.camPos.X, poseData.camPos.Y, poseData.camPos.Z, poseData.camRot.W, poseData.camRot.X, poseData.camRot.Y, poseData.camRot.Z
};
List <double> opdata = new List <double>()
{
nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z, nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z
};
HLdata.Add(hldata);
OPdata.Add(opdata);
}
// Handle new pose update event
private void PoseDataReceived(HoloDataClient.HoloPose poseData)
{
// Fetch nnPose to be approx synchronised with HL pose
NatNetClient.NatNetPoseData nnPoseData = natNetClient_.FetchFrameData();
// Check that log file name has been initialized
if (eventLogFile_ != "")
{
// Assemble log line
string log = "";
log += string.Format("{0:F6},{1:F6},{2:F6},", poseData.camPos.X, poseData.camPos.Y, poseData.camPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", poseData.camRot.W, poseData.camRot.X, poseData.camRot.Y, poseData.camRot.Z);
log += string.Format("{0:F6},{1:F6},{2:F6},", poseData.kbPos.X, poseData.kbPos.Y, poseData.kbPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", poseData.kbRot.W, poseData.kbRot.X, poseData.kbRot.Y, poseData.kbRot.Z);
log += string.Format("{0:F6},{1:F6},{2:F6},", nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z);
log += string.Format("{0:F8},{1:F8},{2:F8},{3:F8},", nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z);
// Add timestamp
string timestamp = Math.Round((System.DateTime.Now - logStartTime_).TotalMilliseconds).ToString();
log += string.Format("{0}", timestamp);
WriteToFile(eventLogFile_, log);
}
// Append data to HL and OP data lists
List <double> hldata = new List <double>()
{
poseData.kbPos.X, poseData.kbPos.Y, poseData.kbPos.Z, poseData.kbRot.W, poseData.kbRot.X, poseData.kbRot.Y, poseData.kbRot.Z, poseData.camPos.X, poseData.camPos.Y, poseData.camPos.Z, poseData.camRot.W, poseData.camRot.X, poseData.camRot.Y, poseData.camRot.Z
};
List <double> opdata = new List <double>()
{
nnPoseData.rbPos.X, nnPoseData.rbPos.Y, nnPoseData.rbPos.Z, nnPoseData.rbRot.W, nnPoseData.rbRot.X, nnPoseData.rbRot.Y, nnPoseData.rbRot.Z
};
HLdata.Add(hldata);
OPdata.Add(opdata);
}
private int handledata(NatNetClient.NatNetPoseData nnPoseData, int index, string targetPhrase)
{
idxHl_ = 0;
if (HLdata.Count == 0)
{
return(1);
}
//get the optitrack data and hololens data
double[] rbpos = new double[3] {
(double)OPdata[idxHl_][0], (double)OPdata[idxHl_][1], (double)OPdata[idxHl_][2]
};
double[] rbquat = new double[4] {
(double)OPdata[idxHl_][3], (double)OPdata[idxHl_][4], (double)OPdata[idxHl_][5], (double)OPdata[idxHl_][6]
};
double[] kbP = new double[3] {
(double)HLdata[idxHl_][0], (double)HLdata[idxHl_][1], (double)HLdata[idxHl_][2]
};
double[] kbQ = new double[4] {
(double)HLdata[idxHl_][3], (double)HLdata[idxHl_][4], (double)HLdata[idxHl_][5], (double)HLdata[idxHl_][6]
};
double[] hlPs = new double[3] {
(double)HLdata[idxHl_][7], (double)HLdata[idxHl_][8], (double)HLdata[idxHl_][9]
};
double[] hlQs = new double[4] {
(double)HLdata[idxHl_][10], (double)HLdata[idxHl_][11], (double)HLdata[idxHl_][12], (double)HLdata[idxHl_][13]
};
double[] mkpos = new double[3] {
nnPoseData.mPos.X, nnPoseData.mPos.Y, nnPoseData.mPos.Z
};
object newmarkerpos = null;
//Transform the finger tip trace onto keyboard frame
matlab_.Feval("trans", 3, out newmarkerpos, mkpos, rbpos, rbquat, kbP, kbQ, hlPs, hlQs);
object[] res = newmarkerpos as object[];
double xpos = Convert.ToDouble(res[0]);
double ypos = Convert.ToDouble(res[1]);
double zpos = Convert.ToDouble(res[2]);
double[] newmarkerpos1 = new double[3] {
xpos, ypos, zpos
};
LogKBData(newmarkerpos1);
object result1 = null;
// Test whether new marker position is within tolerance of target key, result will return index and key
matlab_.Feval("recognitioncopy", 2, out result1, newmarkerpos1, index, targetPhrase);
object[] res2 = result1 as object[];
// Copy old index and retrieve new index from result object
int preIndex = index;
index = Convert.ToInt32(res2[0]);
// If index has been incremented
if (index == preIndex + 1)
{
// Print result for latest index increment
Console.WriteLine("result: " + res2[1] + "\t[" + preIndex + "->" + index + "]");
// Special case for highlighting DONE key
if (preIndex == 1)
{
holoDataClient_.UpdateDoneKeyState(1);
}
// Only if training phase, i.e. phrase id = 0
if (train_ == true)
{
if (preIndex == 2)
{
holoDataClient_.UpdateTrainKeyState('Q', 1);
}
if (preIndex == 3)
{
holoDataClient_.UpdateTrainKeyState('P', 1);
}
if (preIndex == 4)
{
holoDataClient_.UpdateTrainKeyState('Z', 1);
}
if (preIndex == 5)
{
holoDataClient_.UpdateTrainKeyState('M', 1);
}
}
}
// Console.WriteLine(index);
return(index);
}
private int handledata(NatNetClient.NatNetPoseData nnPoseData, int index)
{
int idxHl = 0;
if (HLdata.Count == 0)
{
Console.Write('o');
return(1);
}
double[] rbpos = new double[3] {
(double)OPdata[idxHl][0], (double)OPdata[idxHl][1], (double)OPdata[idxHl][2]
};
double[] rbquat = new double[4] {
(double)OPdata[idxHl][3], (double)OPdata[idxHl][4], (double)OPdata[idxHl][5], (double)OPdata[idxHl][6]
};
double[] kbP = new double[3] {
(double)HLdata[idxHl][0], (double)HLdata[idxHl][1], (double)HLdata[idxHl][2]
};
double[] kbQ = new double[4] {
(double)HLdata[idxHl][3], (double)HLdata[idxHl][4], (double)HLdata[idxHl][5], (double)HLdata[idxHl][6]
};
double[] hlPs = new double[3] {
(double)HLdata[idxHl][7], (double)HLdata[idxHl][8], (double)HLdata[idxHl][9]
};
double[] hlQs = new double[4] {
(double)HLdata[idxHl][10], (double)HLdata[idxHl][11], (double)HLdata[idxHl][12], (double)HLdata[idxHl][13]
};
double[] mkpos = new double[3] {
nnPoseData.mPos.X, nnPoseData.mPos.Y, nnPoseData.mPos.Z
};
object newmarkerpos = null;
matlab_.Feval("trans", 3, out newmarkerpos, mkpos, rbpos, rbquat, kbP, kbQ, hlPs, hlQs);
object[] res = newmarkerpos as object[];
double xpos = Convert.ToDouble(res[0]);
double ypos = Convert.ToDouble(res[1]);
double zpos = Convert.ToDouble(res[2]);
double[] newmarkerpos1 = new double[3] {
xpos, ypos, zpos
};
LogKBData(newmarkerpos1);
object result1 = null;
// Console.WriteLine('p');
//Console.WriteLine(index);
matlab_.Feval("recognition", 2, out result1, newmarkerpos1, index);
object[] res2 = result1 as object[];
//Console.WriteLine('p');
Console.WriteLine(res2[0]);
//Console.WriteLine('p');
Console.WriteLine(res2[1]);
//Console.WriteLine('p');
index = Convert.ToInt32(res2[0]);
// Console.WriteLine(index);
return(index);
}
private int handledata(NatNetClient.NatNetPoseData nnPoseData, int index, string targetWord)
{
//int idxhl = 0;
if (HLdata.Count == 0)
{
// Console.Write('o');
return(1);
}
double[] rbpos = new double[3] {
(double)OPdata[idxHl_][0], (double)OPdata[idxHl_][1], (double)OPdata[idxHl_][2]
};
double[] rbquat = new double[4] {
(double)OPdata[idxHl_][3], (double)OPdata[idxHl_][4], (double)OPdata[idxHl_][5], (double)OPdata[idxHl_][6]
};
double[] kbP = new double[3] {
(double)HLdata[idxHl_][0], (double)HLdata[idxHl_][1], (double)HLdata[idxHl_][2]
};
double[] kbQ = new double[4] {
(double)HLdata[idxHl_][3], (double)HLdata[idxHl_][4], (double)HLdata[idxHl_][5], (double)HLdata[idxHl_][6]
};
double[] hlPs = new double[3] {
(double)HLdata[idxHl_][7], (double)HLdata[idxHl_][8], (double)HLdata[idxHl_][9]
};
double[] hlQs = new double[4] {
(double)HLdata[idxHl_][10], (double)HLdata[idxHl_][11], (double)HLdata[idxHl_][12], (double)HLdata[idxHl_][13]
};
double[] mkpos = new double[3] {
nnPoseData.mPos.X, nnPoseData.mPos.Y, nnPoseData.mPos.Z
};
object newmarkerpos = null;
matlab_.Feval("trans", 3, out newmarkerpos, mkpos, rbpos, rbquat, kbP, kbQ, hlPs, hlQs);
object[] res = newmarkerpos as object[];
double xpos = Convert.ToDouble(res[0]);
double ypos = Convert.ToDouble(res[1]);
double zpos = Convert.ToDouble(res[2]);
double[] newmarkerpos1 = new double[3] {
xpos, ypos, zpos
};
LogKBData(newmarkerpos1);
object result1 = null;
// Console.WriteLine('p');
//Console.WriteLine(index);
//matlab_.Feval("recognitioncopy", 2, out result1, newmarkerpos1, index, targetWord);
string testPhrase = targetWord;
if (requireSpaces_)
{
testPhrase = testPhrase.Replace(' ', '_');
}
//Console.WriteLine(testPhrase);
//Console.WriteLine(requireSpaces_);
matlab_.Feval("recognitioncopy", 2, out result1, newmarkerpos1, index, testPhrase.ToLower());
object[] res2 = result1 as object[];
//Console.WriteLine('p');
//Console.WriteLine(res2[0]);
//Console.WriteLine('p');
//Console.WriteLine('p');
int preIndex = index;
index = Convert.ToInt32(res2[0]);
if (index == preIndex + 1)
{
Console.WriteLine("result: " + res2[1] + "\t[" + preIndex + "->" + index + "]");
// holoDataClient_.AddCharacter((string)res2[1]);
if (preIndex == 1)
{
holoDataClient_.UpdateDoneKeyState(1);
}
}
// Console.WriteLine(index);
return(index);
}
