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()); }