MatchResult IScanMatchers.Match(ScanObservation scan1, ScanObservation scan2, Pose2D seed)
{
Vector2[] points1 = null;
Vector2[] points2 = null;
if ((((scan1.Length < 2) || (scan2.Length < 2)) ? 1 : 0) != 0)
{
throw new InvalidOperationException("Cannot match scans with fewer than 2 scanpoints");
}
if ((scan1.FieldOfView == scan2.FieldOfView) && (scan1.Resolution == scan2.Resolution))
{
double[] angles = scan1.RangeScanner.RangeTheta;//this.ComputeScanAngles(scan1);
points1 = this.ToPoints(scan1, angles);
points2 = this.ToPoints(scan2, angles);
}
else
{
points1 = this.ToPoints(scan1, scan1.RangeScanner.RangeTheta);
points2 = this.ToPoints(scan2, scan2.RangeScanner.RangeTheta);
}
bool[] filter1 = scan1.RangeScanner.RangeFilters; //this.ToFilter(scan1);
bool[] filter2 = scan2.RangeScanner.RangeFilters; //this.ToFilter(scan2);
double dangle = scan1.Resolution;
return(this.MatchPoints(points1, points2, seed, dangle, filter1, filter2));
}
protected virtual bool[] ToFilter(ScanObservation scan)
{
bool[] filter = new bool[scan.RangeScanner.Range.Length];
int len = scan.RangeScanner.Range.Length - 1;
for (int i = 0; i <= len; i++)
{
filter[i] = (scan.RangeScanner.Range[i] < scan.MinRange) || (scan.RangeScanner.Range[i] > scan.MaxRange);
}
return(filter);
}
protected virtual Vector2[] ToPoints(ScanObservation scan, double[] angles)
{
Vector2[] points = new Vector2[scan.RangeScanner.Range.Length];
int len = scan.RangeScanner.Range.Length - 1;
for (int i = 0; i <= len; i++)
{
double dist = scan.RangeScanner.Range[i] * scan.Factor;
double angle = angles[i];
points[i] = new Vector2(System.Math.Cos(angle) * dist, System.Math.Sin(angle) * dist);
}
return(points);
}
protected static float DELTA_MAXROTATION = 4; //degrees
protected virtual double[] ComputeScanAngles(ScanObservation scan)
{
double fov = scan.FieldOfView;
double res = scan.Resolution;
int length = scan.Length;
double fromAngle = -fov / 2.0;
double[] angles = new double[length];
int len = length - 1;
for (int i = 0; i <= len; i++)
{
angles[i] = fromAngle + (res * i);
}
return(angles);
}