private void GetAndDisplayModelPrediction()
{
sw = Stopwatch.StartNew();
IMUMeasurement imu = new IMUMeasurement();
for (int i = 0; i < _imuOrder.Count; i++)
{
if (_connectedMtwData.ContainsKey(_imuOrder[i]))
{
XsIMUMeasurement data = _connectedMtwData[_imuOrder[i]];
// get rotation
Matrix4x4 rot = Matrix4x4.Rotate(data.quat);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
int idx = k * 3 + j; // between 0 and 8
idx = i * 9 + idx; // between 0 and 53
imu.orientations[idx] = rot[k, j];
}
}
/*if (_imuIdToName[_imuOrder[i]] == "pelvis") {
* Debug.Log(string.Format("Pelvis rotation sent {0}", rot));
* }*/
// get acceleration
imu.accelerations[i * 3] = data.freeAcc.x;
imu.accelerations[i * 3 + 1] = data.freeAcc.y;
imu.accelerations[i * 3 + 2] = data.freeAcc.z;
}
else
{
UnityEngine.Debug.LogError("Not all IMUs required for model inference initialized");
// just set identity orientation and zero acceleration
imu.orientations[i * 9] = 1.0f;
imu.orientations[i * 9 + 4] = 1.0f;
imu.orientations[i * 9 + 8] = 1.0f;
}
}
string msg = SendIMUMeasurement(imu);
Pose p = JsonConvert.DeserializeObject <Pose>(msg);
if (p.pose.Count == 1 && p.pose[0] < 0.0f)
{
// this is not a valid pose, buffer is not yet full so wait
UnityEngine.Debug.Log("Wait for Buffer to be filled");
return;
}
// only interested in work performed by the model
sw.Stop();
UnityEngine.Debug.Log(string.Format("model inference elapsed time [ms]: {0:0.0000}", sw.Elapsed.TotalMilliseconds));
_poseUpdater.setNewPose(p.pose.ToArray());
}
public static IMUMeasurement Dummy()
{
IMUMeasurement imu = new IMUMeasurement();
for (int i = 0; i < 6; i++)
{
imu.orientations[i * 9] = 1.0f;
imu.orientations[i * 9 + 4] = 1.0f;
imu.orientations[i * 9 + 4] = 1.0f;
}
return(imu);
}
public string SendIMUMeasurement(IMUMeasurement imu)
{
if (!_modelLoaded)
{
throw new UnityException("Model not loaded on the server.");
}
var imu_s = JsonConvert.SerializeObject(imu);
// notify the server that the next message contains an IMU measurement
_client.SendSync("IMU");
// send the actual IMU data
_client.SendSync(imu_s);
// Wait for it to return
return(_client.ListenSync());
}