/// <summary>
/// Obtain several measurements from the hidden state.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <returns>Pixel-range measurements.</returns>
public override List <PixelRangeMeasurement> Measure(GameTime time)
{
var measurements = new List <PixelRangeMeasurement>();
NextFrame(out DepthFrame, out ColorFrame, out interest);
for (int i = 0; i < interest.Count; i++)
{
float range = GetRange(interest[i].I, interest[i].K, (float)interest[i].Value);
measurements.Add(new PixelRangeMeasurement(interest[i].I - ResX / 2, interest[i].K - ResY / 2, range));
}
if (ShowSidebar && depthsensed != null)
{
DepthMatrixToTexture(depthsensed, DepthFrame);
ColorMatrixToTexture(colorsensed, ColorFrame);
}
MappedMeasurements.Clear();
foreach (PixelRangeMeasurement z in measurements)
{
MappedMeasurements.Add(Measurer.MeasureToMap(Pose, z));
}
return(measurements);
}
/// <summary>
/// Obtain several measurements from the hidden state.
/// Ladmarks may be misdetected.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <returns>Pixel-range measurements.</returns>
public override List <MeasurementT> Measure(GameTime time)
{
List <double[]> mlinear = Measurements[FrameIndex].Item2;
if (FrameIndex < RecLength - 1)
{
updateWants();
FrameIndex++;
}
else
{
WantsToStop = true;
}
List <MeasurementT> measurements = new List <MeasurementT>();
MeasurementT dummy = new MeasurementT();
MappedMeasurements.Clear();
foreach (double[] z in mlinear)
{
MeasurementT measurement = dummy.FromLinear(z);
measurements.Add(measurement);
MappedMeasurements.Add(Measurer.MeasureToMap(Pose, measurement));
}
return(measurements);
}
/// <summary>
/// Obtain several measurements from the hidden state.
/// Ladmarks may be misdetected.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <returns>Pixel-range measurements.</returns>
public override List <MeasurementT> Measure(GameTime time)
{
List <MeasurementT> measurements = new List <MeasurementT>();
Map visible = new Map(3);
DataAssociation = new List <int>();
double[][] diraccov = new double[3][] { new double[3] {
0.001, 0, 0
},
new double[3] {
0, 0.001, 0
},
new double[3] {
0, 0, 0.001
} };
// add every measurement with probability = DetectionProbability
for (int i = 0; i < Landmarks.Count; i++)
{
double pd = DetectionProbability(Landmarks[i]);
if (pd > 0)
{
if (Util.Uniform.Next() < pd)
{
measurements.Add(MeasureDetected(Landmarks[i]));
DataAssociation.Add(i);
visible.Add(new Gaussian(Landmarks[i], diraccov, 1.0));
}
else
{
visible.Add(new Gaussian(Landmarks[i], diraccov, 0.0));
// weight indicates visible but not detected
}
}
}
// poisson distributed clutter measurement count
// a cap of 10 lambda is enforced because technically
// the number is unbounded so it could freeze the system
int nclutter;
try {
// the poisson generator may underflow if lambda is too small
nclutter = Math.Min(clutterGen.Generate(), (int)(ClutterCount * 10));
}
catch (ArithmeticException)
{
nclutter = 0;
}
for (int i = 0; i < nclutter; i++)
{
measurements.Add(Measurer.RandomMeasure());
DataAssociation.Add(int.MinValue);
}
MappedMeasurements.Clear();
foreach (MeasurementT z in measurements)
{
MappedMeasurements.Add(Measurer.MeasureToMap(Pose, z));
}
WayVisibleMaps.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, visible));
return(measurements);
}
