/// <summary>
/// Calculates likelihood of the distance between particle and measurement (point).
/// </summary>
/// <param name="particle">Particle.</param>
/// <param name="measurement">Measurement (coordinate).</param>
/// <param name="likelihoodFunc">
/// Likelihood function.
/// <para>Can be used as parameter in <see cref="Update"/> function to calculate measurement-track association likelihood.</para>
/// </param>
/// <returns>Measurement-track likelihood.</returns>
public static double DistanceLikelihood(IParticle <ConstantVelocity2DModel> particle, PointF measurement, Func <double, double> likelihoodFunc)
{
var delta = measurement.DistanceTo(particle.State.Position);
var likelihood = likelihoodFunc(delta);
return(likelihood);
}
// given a starting point and a list of points, find the point nearest to the starting point
private AForge.Point FindNearestPoint(AForge.Point startingPoint, List <AForge.Point> pointsToConsider)
{
if (pointsToConsider.Count == 0)
{
return(startingPoint); // return the starting point if no other points are to be considered
}
List <Double> listOfDistances = new List <Double>();
foreach (AForge.Point point in pointsToConsider)
{
listOfDistances.Add(startingPoint.DistanceTo(point)); // calculate the distance between the startingPoint and each point to consider
}
double minDistance = listOfDistances.Min();
int indexOfMin = listOfDistances.IndexOf(minDistance); // find the index of the item with the shortest distance in the listOfDistances
return(pointsToConsider[indexOfMin]); // return the point with the minimum distance from the startingPoint
}
