/// <summary>
/// Obtain the cumulative odometry reading since the last call to this function.
/// Note that this version doesn't actually "reset" the odometry
/// and gives the discrete recorded points in the odometry file.
/// Calling multiple times this function would give the same result, even
/// if the base behavior would be to return zero after the first time.
/// </summary>
/// <returns>State diff.</returns>
public override double[] ReadOdometry(GameTime time)
{
double[] odometry = Odometry[FrameIndex].Item2;
WayOdometry.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(odometry)));
return(odometry);
}
/// <summary>
/// Obtain the cumulative odometry reading since the last call to this function.
/// </summary>
/// <returns>State diff.</returns>
public virtual double[] ReadOdometry(GameTime time)
{
double[] reading = OdometryPose.DiffOdometry(refodometry);
OdometryPose = Pose.DClone();
refodometry = Pose.DClone();
WayOdometry.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(reading)));
return(reading);
}
/// <summary>
/// Remove all the localization history and start it again from the current position.
/// </summary>
public void ResetHistory()
{
WayOdometry.Clear();
WayPoints.Clear();
WayPoints.Add(Tuple.Create(0.0, Util.SClone(Pose.State)));
}
