/// <summary>
/// Constructor
/// </summary>
/// <param name="label">Object label</param>
/// <param name="data">Inertial sensor</param>
public FormInertialSystem(IObjectLabel label, InertialSensorData data)
: this()
{
this.label = label;
this.data = data;
UpdateFormUI();
post();
}
private void CalibrateToCord(object sender, IData e)
{
//q2*q1' = r
//(new Quaternion(0,0,1,0)) * (new Quaternion(0,0,0,1))
//* (new Quaternion(0,0,1,0)) * (new Quaternion(0,0,0,1))
//RightArmQuat *
InertialSensorData ISD = (InertialSensorData)e;
//(new Quaternion(((InertialSensorData)e).Q[0], ((InertialSensorData)e).Q[1], ((InertialSensorData)e).Q[2], ((InertialSensorData)e).Q[3])).Normalize().Inverse();
//arbitraryCorrection = Quaternion.Identity * ISD.QMD3;
arbitraryCorrection = this.CorrectionFromIMUtoKinect;
this.DPMC.DPMCollection.First(x => x.Value.UISensorData.SensorType == SensorType.Inertial).Value.IDataProducer.NewIData -= CalibrateToCord;
}
private void CalibrateToCord(object sender, IData e)
{
InertialSensorData ISD = (InertialSensorData)e;
Quaternion Arb_Sensor = ISD.QuaternionAsQMD3, Arb_SensorInvert = ISD.QuaternionAsQMD3;
Arb_SensorInvert.Invert();
this.BindingCorrection = Arb_SensorInvert;
arbitraryCorrection = this.DisplayConstRotation * Arb_SensorInvert;
((this.DPMC.DPMCollection.First(x => x.Value.UISensorData.SensorType == SensorType.MappedIntertial).Value.IDataProducer as MappedBindedInertialSesnorProducer).Mapper as QuaternionCoordinateMapper).Initial = new Quaternion(0, 1, 0, 0) * Arb_Sensor;
//arbitraryCorrection = Quaternion.Identity * ISD.QMD3;
//arbitraryCorrection = this.CorrectionFromIMUtoKinect;
this.DPMC.DPMCollection.First(x => x.Value.UISensorData.SensorType == SensorType.Inertial).Value.IDataProducer.NewIData -= CalibrateToCord;
}
private void MyFunctionToDealWithGettingData(object sender, InertialSensorData e)
{
const double angularConst = Math.PI / (16.4 * 180);
//getting acceleration data from sensors
double xAcc = (double)e.accelaromaters[0] / 2080;
double yAcc = (double)e.accelaromaters[1] / 2080;
double zAcc = (double)e.accelaromaters[2] / 2080;
double xRot = (double)e.gyropscopes[0] * angularConst;
double yRot = (double)e.gyropscopes[1] * angularConst;
double zRot = (double)e.gyropscopes[2] * angularConst;
double[] tempAngularActualAcc = new double[] { xRot, yRot, zRot };
double[] tempActualAcc = new double[] { xAcc, yAcc, zAcc };
AccelerationTime tempActualAccTime = new AccelerationTime();
tempActualAccTime.setVal(tempActualAcc, tempAngularActualAcc, e.NowInTicks);
actualAcc.Enqueue(tempActualAccTime);
}
