/// <summary>
/// Render the navigation HUD and the trajectory on the graphics device.
/// The graphics device must be ready, otherwise
/// the method will throw an exception.
/// </summary>
/// <param name="camera">Camera 4d transform matrix.</param>
public override void Render(double[][] camera)
{
RefVehicle.Render(camera);
if (RenderAllParticles)
{
foreach (var particle in VehicleParticles)
{
particle.RenderTrajectory(camera, Color.Blue);
}
}
else
{
RenderTrajectory(camera);
}
RenderMap(camera);
}
/// <summary>
/// Resample the vehicle particles with their corresponding weights and map models
/// using systematic resampling (aka Wheel resampling).
/// The weights are transferred into the unit range (0, 1] and random number is
/// generated in the range (0, 1/n], adding 1/n to iterate through the unit range
/// obtaining appropiate samples.
/// This method is fast and simple although it loses the important property of
/// giving independent samples, but in practice works very well.
/// </summary>
public void ResampleParticles()
{
var particles = new TrackVehicle <MeasurerT, PoseT, MeasurementT> [VehicleParticles.Length];
double random = (double)Util.Uniform.Next() / VehicleWeights.Length;
double[] weights = new double[VehicleWeights.Length];
Map[] models = new Map[MapModels.Length];
double maxweight = 0;
for (int i = 0, k = 0; i < weights.Length; i++)
{
// k should never be out of range, but because of floating point arithmetic,
// the probabilities may not add up to one. If the random number hits this tiny difference
// it will throw an exception. In such rare case, just expand the last guy's probability
for (; random > 0 && k < VehicleWeights.Length; k++)
{
random -= VehicleWeights[k];
}
particles[i] = RefVehicle.TrackClone(VehicleParticles[k - 1], true);
models [i] = new Map(MapModels[k - 1]);
weights [i] = 1.0 / weights.Length;
random += 1.0 / weights.Length;
if (VehicleWeights[k - 1] > maxweight)
{
maxweight = VehicleWeights[k - 1];
BestParticle = i;
}
}
// debug: force the first particle to remain there
// use with "make the first particle the real particle" on Update()
// to also carry its map model
//particles[0] = VehicleParticles[0];
//models[0] = MapModels[0];
VehicleParticles = particles;
VehicleWeights = weights;
MapModels = models;
}
/// <summary>
/// Render the navigation HUD and the trajectory on the graphics device.
/// </summary>
/// <remarks>
/// The graphics device must be ready, otherwise
/// the method will throw an exception.
/// </remarks>
/// <param name="camera">Camera 4d transform matrix.</param>
public virtual void Render(double[][] camera)
{
RenderMap(camera);
RenderTrajectory(camera);
RefVehicle.Render(camera);
}
/// <summary>
/// Render the navigation HUD and the trajectory on the graphics device.
/// The graphics device must be ready, otherwise
/// the method will throw an exception.
/// </summary>
/// <param name="camera">Camera 4d transform matrix.</param>
public override void Render(double[][] camera)
{
RefVehicle.Render(camera);
RenderTrajectory(camera);
RenderMap(camera);
}
