/// <summary>
/// Construct a PHDNavigator 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). Currently not used.</param>
public ISAM2Navigator(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle, bool onlymapping = false)
: base(vehicle, onlymapping)
{
int nmeasurement = vehicle.MeasurementCovariance.Length;
int nmotion = vehicle.MotionCovariance.Length;
double[] measurementnoise = new double[nmeasurement];
double[] motionnoise = new double[nmotion];
double[] mparams = vehicle.Measurer.ToLinear();
// NOTE the iSAM2 interface assumes independent noise in each component,
// discarding every entry that's not in the diagonal;
// although it is possible to define a complete matrix,
// it doesn't seem to be worth it when comparing performance
for (int i = 0; i < nmeasurement; ++i)
{
measurementnoise[i] = Math.Sqrt(vehicle.MeasurementCovariance[i][i]);
}
for (int i = 0; i < nmotion; ++i)
{
motionnoise[i] = MotionDt * Math.Sqrt(vehicle.MotionCovariance[i][i]);
}
bestestimate = new TrackVehicle <MeasurerT, PoseT, MeasurementT>();
MapModel = new IndexedMap(3);
plmodel = new IndexedMap(3);
CandidateMapModel = new IndexedMap(3);
BestEstimate.Pose = vehicle.Pose.DClone();
handle = NewNavigator(vehicle.Pose.State, measurementnoise, motionnoise, mparams);
nextlabel = 0;
}
TrackClone(TrackVehicle <KinectMeasurer, Pose3D, PixelRangeMeasurement> vehicle,
bool copytrajectory = false)
{
if (vehicle is KinectTrackVehicle)
{
return(new KinectTrackVehicle((KinectTrackVehicle)vehicle, copytrajectory));
}
else
{
throw new ArgumentException("A Kinect vehicle should be tracked with KinectTrackVehicles");
}
}
/// <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>
/// Reset the state of the navigator to set of equal particles.
/// </summary>
/// <param name="vehicle">New vehicle pose.</param>
/// <param name="model">New map model estimate.</param>
/// <param name="explore">New queued measurements to explore.</param>
/// <param name="particlecount">Number of particles for the Montecarlo filter.</param>
private void reset(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle, Map model, List <double[]> explore, int particlecount)
{
// do a deep copy, so no real info flows
// into the navigator, only estimates, in SLAM
// (this avoid bugs where the user unknowingly uses
// the groundtruth to estimate the groundtruth itself)
VehicleParticles = new TrackVehicle <MeasurerT, PoseT, MeasurementT> [particlecount];
MapModels = new Map [particlecount];
VehicleWeights = new double [particlecount];
toexplore = new List <double[]> [particlecount];
for (int i = 0; i < particlecount; i++)
{
VehicleParticles[i] = vehicle.TrackClone(MotionCovarianceMultiplier,
MeasurementCovarianceMultiplier,
PD, ClutterDensity, true);
MapModels [i] = new Map(model);
VehicleWeights [i] = 1.0 / particlecount;
toexplore [i] = new List <double[]>(explore);
}
BestParticle = 0;
}
TrackClone(TrackVehicle <MeasurerT, PoseT, MeasurementT> vehicle,
bool copytrajectory = false)
{
return(new TrackVehicle <MeasurerT, PoseT, MeasurementT>(vehicle, copytrajectory));
}
/// <summary>
/// Construct a OdometryNavigator using the indicated vehicle as a reference.
/// </summary>
/// <param name="vehicle">Vehicle to track.</param>
public OdometryNavigator(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle)
: base(vehicle)
{
bestestimate = new TrackVehicle <MeasurerT, PoseT, MeasurementT>(vehicle, 1, 1, 1, 0);
bestmapmodel = new Map(3);
}
