/// <summary>
/// Remove all the localization and mapping history and start it again from the current position.
/// </summary>
public virtual void ResetHistory()
{
WayTrajectories.Clear();
WayMaps.Clear();
WayTrajectories.Add(Tuple.Create(0.0, new TimedState {
Tuple.Create(0.0, Util.SClone(BestEstimate.Pose.State))
}));
WayMaps.Add(Tuple.Create(0.0, new Map(3)));
}
/// <summary>
/// Update the trajectory history with the
/// current best estimate and a given time.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
protected void UpdateTrajectory(GameTime time)
{
TimedState waypoints = new TimedState(BestEstimate.WayPoints);
WayTrajectories.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, waypoints));
}
