/// <summary>
/// Perform a deep copy of another general vehicle.
/// </summary>
/// <param name="that">Copied general vehicle.</param>
/// <param name="copytrajectory">If true, the vehicle historic trajectory is copied. Relatively heavy operation.</param>
protected Vehicle(Vehicle <MeasurerT, PoseT, MeasurementT> that, bool copytrajectory = false)
: this(that.Pose)
{
this.Pose = that.Pose.DClone();
this.Graphics = that.Graphics;
this.MappedMeasurements = that.MappedMeasurements;
this.Landmarks = that.Landmarks;
this.Measurer = that.Measurer;
this.KnowsGroundtruth = that.KnowsGroundtruth;
this.Groundtruth = that.Groundtruth;
this.HasDataAssociation = that.HasDataAssociation;
this.DataAssociation = that.DataAssociation;
this.motionCovariance = that.motionCovariance.MemberwiseClone();
this.MeasurementCovariance = that.MeasurementCovariance.MemberwiseClone();
if (copytrajectory)
{
this.WayPoints = new TimedState(that.WayPoints);
this.WayOdometry = new TimedArray(that.WayOdometry);
this.WayVisibleMaps = new TimedMapModel(that.WayVisibleMaps);
}
else
{
this.WayOdometry = new TimedArray();
this.WayPoints = new TimedState();
this.WayVisibleMaps = new TimedMapModel();
this.WayPoints.Add(Tuple.Create(0.0, Util.SClone(Pose.State)));
this.WayVisibleMaps.Add(Tuple.Create(0.0, new Map(3)));
}
}
/// <summary>
/// Construct a Navigator using the indicated vehicle as a reference.
/// </summary>
/// <param name="vehicle">Vehicle to track.</param>
/// <param name="onlymapping">If true, don't do SLAM, but mapping (i.e. the localization is assumed known).</param>
protected Navigator(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle, bool onlymapping = false)
{
OnlyMapping = onlymapping;
// a reference copy, which gives the navigator precise info about
// the real vehicle pose
RefVehicle = vehicle;
WayTrajectories = new TimedTrajectory();
WayTrajectories.Add(Tuple.Create(0.0, new TimedState {
Tuple.Create(0.0, Util.SClone(vehicle.Pose.State))
}));
WayMaps = new TimedMapModel();
WayMaps.Add(Tuple.Create(0.0, new Map(3)));
const int segments = 32;
pinterval = new double[segments][];
// pinterval will be the 5-sigma ellipse
for (int i = 0; i < segments; i++)
{
pinterval[i] = new double[2] {
5 * Math.Cos(2 * Math.PI * i / segments), 5 * Math.Sin(2 * Math.PI * i / segments)
};
}
}
/// <summary>
/// Construct a new Vehicle object from its initial state and appropiate constants.
/// </summary>
/// <param name="initial">Initial pose.</param>
/// <param name="measurer">Measuring config and methods.</param>
protected Vehicle(PoseT initial, MeasurerT measurer)
{
Pose = initial.DClone();
Measurer = measurer;
OdometryPose = Pose.DClone();
refodometry = Pose.DClone();
HasDataAssociation = false;
DataAssociation = new List <int>();
Landmarks = new List <double[]>();
MappedMeasurements = new List <double[]>();
Groundtruth = new TimedState();
KnowsGroundtruth = false;
WayOdometry = new TimedArray();
WayPoints = new TimedState();
WayVisibleMaps = new TimedMapModel();
WayPoints.Add(Tuple.Create(0.0, Util.SClone(Pose.State)));
WayVisibleMaps.Add(Tuple.Create(0.0, new Map(3)));
HasSidebar = false;
WantsToStop = false;
WantsSLAM = false;
WantsMapping = false;
}
/// <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>
/// 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>
/// 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>
/// Apply the motion model to the vehicle. It corresponds to a
/// 3D odometry model following the equation:
///
/// x = x + q dx q* + N(0, Q)
/// o = dq o dq* + N(0, Q')
///
/// where q is the midrotation quaternion (halfway between the old and new orientations) and N(a, b) is a normal function
/// with mean 'a' and covariance matrix 'b'.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public virtual void Update(GameTime time, double[] reading)
{
Pose = Pose.AddOdometry(reading);
OdometryPose = OdometryPose.AddOdometry(reading);
double[] noise = time.ElapsedGameTime.TotalSeconds.Multiply(
Util.RandomGaussianVector(new double[OdoSize],
MotionCovariance));
OdometryPose = OdometryPose.AddOdometry(noise);
WayPoints.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State)));
}
/// <summary>
/// Calculate the transpose of a vector (a horizontal matrix).
/// </summary>
/// <param name="vector">Vector to be transposed.</param>
/// <param name="shallow">If true, use the same vector for the matrix,
/// but changing values in one will be reflected on the other.</param>
/// <returns>Transposed vector.</returns>
public static double[][] Transpose(this double[] vector, bool shallow = false)
{
double[][] result = new double[1][];
if (shallow)
{
result[0] = vector;
}
else
{
result[0] = Util.SClone(vector);
}
return(result);
}
/// <summary>
/// Apply the motion model to the vehicle. It corresponds to a
/// 3D odometry model following the equation:
///
/// x = x + q dx q*
/// o = dq o dq*
///
/// where q is the midrotation quaternion (halfway between the old and new orientations) and N(a, b) is a normal function
/// with mean 'a' and covariance matrix 'b'.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public override void Update(GameTime time, double[] reading)
{
// no input, static friction makes the robot stay put (if there is any static friction)
if (PerfectStill && reading.IsEqual(0))
{
Pose = Pose.AddOdometry(reading);
OdometryPose = OdometryPose.AddOdometry(reading);
WayPoints.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State)));
}
else
{
base.Update(time, reading);
}
}
/// <summary>
/// Construct a RecordVehicle from prerecorded data.
/// </summary>
/// <param name="trajectory">Prerecorded trajectory.</param>
/// <param name="odometry">Prerecorded odometry.</param>
/// <param name="measurements">Prerecorded measurements.</param>
/// <param name="tags">Prerecorded tags.</param>
/// <param name="landmarks">Landmark 3d locations against which the measurements were performed.</param>
/// <param name="measurer">Measuring config and methods.</param>
public RecordVehicle(TimedState trajectory, TimedArray odometry,
TimedMeasurements measurements, TimedMessage tags,
List <double[]> landmarks, MeasurerT measurer)
: base(new PoseT().IdentityP(), measurer)
{
if (trajectory.Count != measurements.Count + 1)
{
throw new ArgumentException("Measurements length must be one short of the trajectory length.");
}
if (trajectory.Count != odometry.Count + 1)
{
throw new ArgumentException("Odometry length must be one short of the trajectory length.");
}
if (trajectory.Count < 1)
{
throw new ArgumentException("Trajectory length must be positive");
}
Groundtruth = trajectory;
Odometry = new TimedState(odometry);
Measurements = new TimedMeasurements(measurements);
Tags = tags;
Odometry.Insert(0, Tuple.Create(0.0, new double[OdoSize]));
Measurements.Insert(0, Tuple.Create(0.0, new List <double[]>()));
RecLength = Trajectory.Count;
FrameIndex = 0;
updateWants();
FrameIndex = 1;
Landmarks = landmarks;
Pose = new PoseT().FromState(Trajectory[0].Item2);
WayPoints.Clear();
WayPoints.Add(Tuple.Create(0.0, Util.SClone(Pose.State)));
}
/// <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)));
}
/// <summary>
/// Apply the motion model to the vehicle. It corresponds to a
/// 3D odometry model following the equation:
///
/// x = x + q dx q* + N(0, Q)
/// o = dq o dq* + N(0, Q')
///
/// where q is the midrotation quaternion (halfway between the old and new orientations) and N(a, b) is a normal function
/// with mean 'a' and covariance matrix 'b'.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public void UpdateNoisy(GameTime time, double[] reading)
{
Update(time, reading);
// no input, static friction makes the robot stay put (if there is any static friction)
if (!(PerfectStill && reading.IsEqual(0)))
{
double[] noise = time.ElapsedGameTime.TotalSeconds.Multiply(
Util.RandomGaussianVector(new double[OdoSize],
MotionCovariance));
Pose = Pose.AddOdometry(noise);
}
WayPoints[WayPoints.Count - 1] = Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State));
}
/// <summary>
/// Apply the motion model to the vehicle.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public override void Update(GameTime time, double[] reading)
{
Pose = new PoseT().FromState(Trajectory[FrameIndex].Item2);
WayPoints.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, Util.SClone(Pose.State)));
}