protected override void PushSolverFrame(SensorEvent sensorEvent)
{
ulong timestamp = sensorEvent.Timestamp - this.firstTimestamp;
SolverFrame solverFrame;
SolverFrame existingSolverFrame;
double[] existingData;
if (this.solverData.TryGetValue(timestamp, out existingSolverFrame))
{
switch (sensorEvent.Type)
{
case SensorEvent.SensorType.ACCEL:
existingData = existingSolverFrame.Rotation;
solverFrame = new SolverFrame(timestamp, ref this.accelMagnetPosition, ref existingData);
break;
case SensorEvent.SensorType.GYRO:
existingData = existingSolverFrame.Position;
solverFrame = new SolverFrame(timestamp, ref existingData, ref this.gyroRotation);
break;
default:
return;
}
this.solverData.Remove(timestamp);
}
else
{
solverFrame = new SolverFrame(timestamp, ref this.accelMagnetPosition, ref this.gyroRotation);
}
this.solverData.Add(solverFrame.Timestamp, solverFrame);
}
protected virtual void SolveFromSolverData()
{
uint frameCounter = 0;
ulong currentTimestamp = 0;
ulong workingTimestamp = 0;
ulong limitTimestamp = 0;
ulong timestampRange = Convert.ToUInt64(this.nsPerFrame * 0.5);
ulong nsPerFrame = Convert.ToUInt64(Math.Floor(this.nsPerFrame));
SolverFrame currentSolverFrame = null;
SolverFrame maxSolverFrame = this.solverData.Last().Value;
while (currentTimestamp < maxSolverFrame.Timestamp)
{
if (!this.solverData.TryGetValue(currentTimestamp, out currentSolverFrame))
{
workingTimestamp = currentTimestamp + 1;
limitTimestamp = currentTimestamp + timestampRange;
while (workingTimestamp < limitTimestamp)
{
if (this.solverData.TryGetValue(workingTimestamp, out currentSolverFrame))
{
goto found;
}
workingTimestamp++;
}
found :;
if (null == currentSolverFrame)
{
throw new CameraTrackException("Unable to solve frame " + (frameCounter + 1).ToString());
}
}
this.chanFrames.Add(new ChanFrame(frameCounter + 1, currentSolverFrame));
frameCounter++;
currentTimestamp += nsPerFrame;
currentSolverFrame = null;
}
}