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); }