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