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());
}
void Update()
{
if (_drawBoneOrientations)
{
_poseUpdater.DrawIMUBoneOrientations();
}
CheckForMTWConnections();
if (_acceptNewMTWs)
{
return;
}
if (!_MTWsInitialized)
{
_connectedMtwData.Clear();
if (!_masterDevice.gotoMeasurement())
{
throw new UnityException("could not enter measurement mode");
}
_masterDevice.clearCallbackHandlers();
XsDevicePtrArray deviceIds = _masterDevice.children();
for (uint i = 0; i < deviceIds.size(); i++)
{
XsDevice mtw = new XsDevice(deviceIds.at(i));
MyMtwCallback callback = new MyMtwCallback();
uint deviceId = mtw.deviceId().toInt();
if (_imuOrder.Contains(deviceId))
{
XsIMUMeasurement mtwData = new XsIMUMeasurement();
_connectedMtwData.Add(deviceId, mtwData);
callback.DataAvailable += new System.EventHandler <DataAvailableArgs>(DataAvailableCallback);
mtw.addCallbackHandler(callback);
_measuringMts.Add(mtw, callback);
}
}
_MTWsInitialized = true;
UnityEngine.Debug.Log(string.Format("Initialized {0} MTWs", _measuringMts.Keys.Count));
}
if (_MTWsInitialized)
{
// draw IMU measurements in Unity
// bake mesh so that we can get updated vertex positions
_meshRenderer.BakeMesh(_currentMesh);
foreach (KeyValuePair <uint, XsIMUMeasurement> data in _connectedMtwData)
{
if (_drawIMUOriAsBoneOri)
{
_poseUpdater.setBoneOrientation(_imuIdToBoneName[data.Key], _connectedMtwData[data.Key].quat);
}
data.Value.Draw(_meshRenderer.transform.position + _currentMesh.vertices[_imuIdToVertex[data.Key]],
_drawAcceleration);
}
if (_drawIMUOriAsBoneOri)
{
_poseUpdater.setBoneOrientation("Head", _connectedMtwData[_headId].quat);
}
// send IMU measurements to inference server and display the results
if (_getModelPrediction)
{
GetAndDisplayModelPrediction();
}
else
{
// make sure the head sensor is levelled
// only compute this when model inference not toggled
float pitch = getPitch(_connectedMtwData[_headId].quat);
float roll = getRoll(_connectedMtwData[_headId].quat);
if (Mathf.Abs(pitch) < 5.0f && Mathf.Abs(roll) < 5.0f)
{
_calibrationEnabled = true;
UnityEngine.Debug.Log("Calibration ENABLED");
}
else
{
_calibrationEnabled = false;
UnityEngine.Debug.Log("Head sensor not levelled, pitch: " + pitch + " roll: " + roll);
}
}
}
}
