private void Calibrate()
{
/* The purpose of the calibration is to find the rotation offset between the orientation of the sensor
* and the orientation of the bone. It assumes that the orientation of the IMU sensor is given in the
* global (inertial/tracking) frame. The output is the orientation of the sensor in the SMPL body
* frame.
* To perform the calibration, the subject must stand still in the i-pose. Then, this method should
* be invoked.
*/
if (!_MTWsInitialized)
{
throw new UnityException("Cannot perform calibration, MTWs are not initialized.");
}
if (!_connectedMtwData.ContainsKey(_headId))
{
throw new UnityException("Cannot perform calibration, head sensor not connected.");
}
// reset head quaternions in case we need to do calibration multiple times
_connectedMtwData[_headId].calibrationQuat = Quaternion.identity;
_connectedMtwData[_headId].boneQuat = Quaternion.identity;
// quaternion that rotates the sensor axes so that they align with the SMPL frame
// this assumes that the sensor is placed correctly on the head
Quaternion R = new Quaternion(0.5f, 0.5f, 0.5f, 0.5f);
// the quaternion that maps from inertial to SMPL frame
Quaternion calib = Quaternion.Inverse(_connectedMtwData[_headId].quat * R);
// set the calibration quat also on the head sensor for debugging
// note that if we do this, we can only calibrate once when the system is running
_connectedMtwData[_headId].calibrationQuat = calib;
// put the model in i-Pose, so that we can read off the correct bone orientations
_poseUpdater.setNewPose(SMPLPoseUpdater.iPose);
for (int i = 0; i < _imuOrder.Count; i++)
{
uint imuId = _imuOrder[i];
if (!_connectedMtwData.ContainsKey(imuId))
{
continue;
}
_connectedMtwData[imuId].calibrationQuat = calib;
_connectedMtwData[imuId].boneQuat = Quaternion.identity;
Quaternion qBone = _poseUpdater.GetGlobalBoneOrientation(_imuIdToBoneName[imuId]);
Quaternion qSensor = _connectedMtwData[imuId].quat;
_connectedMtwData[imuId].boneQuat = Quaternion.Inverse(qSensor) * qBone;
}
}
