private void btnStep_Click(object sender, EventArgs e) { ScanObservation m1 = new ScanObservation(m3DScaleFactor, mPrevLaser); ScanObservation m2 = new ScanObservation(m3DScaleFactor, mCurrLaser); MatchResult mR = null; mSM = new MbIcpScanMatcher(); mEstimated = new Pose2D(); if (chkSeed.Checked) { mR = mSM.Match(m1, m2, mSeed); } else { mR = mSM.Match(m1, m2, new Pose2D()); } if (mR.Converged) { Pose2D es = mR.EstimatedOdometry; mEstimated = compound(mEstimated, es); mCurrLaser.posRobot = new Pose2D(mEstimated.Position, mEstimated.Rotation); mMapPoints.Clear(); ComputeMapPoints(mPrevLaser, 0); ComputeMapPoints(mCurrLaser, 1); rwDrawer.Invalidate(); } }
private Vector4[] ToPoints(ScanObservation scan, Pose2D q) { Matrix2 R = new Matrix2(System.Math.Cos(q.Rotation), -System.Math.Sin(q.Rotation), System.Math.Sin(q.Rotation), System.Math.Cos(q.Rotation)); List <Vector4> points = new List <Vector4>(); Vector2 tmpC; Vector2 tmpP; int len = scan.RangeScanner.Range.Length - 1; for (int i = 0; i <= len; i++) { double dist = scan.RangeScanner.Range[i] * scan.Factor; double angle = scan.RangeScanner.RangeTheta[i]; if (dist < MaxLaserRange && !scan.RangeScanner.RangeFilters[i]) { tmpC = new Vector2(System.Math.Cos(angle) * dist, System.Math.Sin(angle) * dist); tmpC = R * tmpC; tmpC.X += q.X; tmpC.Y += q.Y; tmpP = new Vector2(dist, System.Math.Atan2(tmpC.Y, tmpC.X)); points.Add(new Vector4(tmpC.X, tmpC.Y, tmpP.X, tmpP.Y)); } } return(points.ToArray()); }
public MatchResult Match(Manifold manifold, ScanObservation scan1, ScanObservation scan2, Pose2D seed) { Vector2[] points1, points2; bool flag = (scan1.Length >= 2) && (scan2.Length >= 2) ? true : false; if (flag) { throw new InvalidOperationException("Cannot match scans with fewer than 2 scanpoints\uFFFD"); } if (scan1.FieldOfView != scan2.FieldOfView) { goto label_0; } flag = scan1.Resolution == scan2.Resolution ? true : false; if (flag) { double[] angles = ComputeScanAngles(scan1); points1 = ToPoints(scan1, angles); points2 = ToPoints(scan2, angles); } else { points1 = ToPoints(scan1, ComputeScanAngles(scan1)); points2 = ToPoints(scan2, ComputeScanAngles(scan2)); } bool[] filter1 = ToFilter(scan1); bool[] filter2 = ToFilter(scan2); double dangle = scan1.FieldOfView / (double)scan1.Length; return(MatchPoints(manifold, points1, points2, seed, dangle, filter1, filter2)); }
private void button1_Click(object sender, EventArgs e) { IScanMatchers sc = new IdcScanMatcher(); string[] lines = File.ReadAllLines(linkLabel1.Text); string cmdLaser1 = "SEN {Time 6.42} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174} {FOV 3.1415} {Range " + lines[0] + "}"; string cmdLaser2 = "SEN {Time 6.43} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174} {FOV 3.1415} {Range " + lines[1] + "}"; USARParser p1 = new USARParser(cmdLaser1); USARParser p2 = new USARParser(cmdLaser2); Laser m1 = new Laser(p1); Laser m2 = new Laser(p2); ScanObservation so1 = new ScanObservation(1000, m1); ScanObservation so2 = new ScanObservation(1000, m2); string [] parts = lines[2].Split("/".ToCharArray(), StringSplitOptions.RemoveEmptyEntries); float[] p = USARParser.parseFloats(parts[0], ","); float t = float.Parse(parts[1]); Pose2D pSeed = new Pose2D(p[0], p[1], t); MatchResult r = sc.Match(so1, so2, pSeed); textBox1.Text += "My Estm. :" + r.EstimatedOdometry.ToString() + "\r\n"; textBox1.Text += "UvA Estm. :" + lines[3] + "\r\n"; }
protected virtual bool[] ToFilter(ScanObservation scan) { int i2; ScanObservation scanObservation = scan; bool[] filter = new bool[checked (checked (checked ((int)scanObservation.Range.Length) - 1) + 1)]; int i1 = checked (checked ((int)scanObservation.Range.Length) - 1); int i = 0; while (i <= i2) { filter[i] = (scanObservation.Range[i] >= scanObservation.MinRange) && (scanObservation.Range[i] <= scanObservation.MaxRange) ? true : false; i = checked (i + 1); i2 = i1; } return(filter); scanObservation = null; return(ToFilter); }
protected virtual double[] ComputeScanAngles(ScanObservation scan) { int i2; double fov = scan.FieldOfView; double res = scan.Resolution; int length = checked (checked ((int)Math.Round(fov / res)) + 1); double fromAngle = -fov / 2.0; double[] angles = new double[checked (checked (length - 1) + 1)]; int i1 = checked (length - 1); int i = 0; while (i <= i2) { angles[i] = fromAngle + (res * (double)i); i = checked (i + 1); i2 = i1; } return(angles); }
protected virtual Vector2[] ToPoints(ScanObservation scan, double[] angles) { int i2; ScanObservation scanObservation = scan; Vector2[] points = new Vector2[checked (checked (checked ((int)scanObservation.Range.Length) - 1) + 1)]; int i1 = checked (checked ((int)scanObservation.Range.Length) - 1); int i = 0; while (i <= i2) { double dist = scanObservation.Range[i] * (double)scanObservation.Factor; double angle = angles[i]; points[i] = new Vector2(Math.Cos(angle) * dist, Math.Sin(angle) * dist); i = checked (i + 1); i2 = i1; } return(points); scanObservation = null; return(ToPoints); }
MatchResult IScanMatchers.Match(ScanObservation scan1, ScanObservation scan2, Pose2D initQ) { ChangeMBParameterToUnitFactor(scan1.Factor); int StartTime = 0; double Duration = 0; Vector4[] p = null; Vector4[] c = null; p = this.ToPoints(scan1, new Pose2D()); preProcessOnRef(p); //Debug.DrawPoints(p, 0); Pose2D q = initQ; //Vector4[] f = this.ToPoints(scan2, q); //Debug.DrawPoints(f, 1); StartTime = Environment.TickCount; int NumConverged = 0; int NumIterations = 0; double error_ratio, error; double cosw, sinw, dtx, dty, tmp1, tmp2; while (NumIterations < MAX_ITERATIONS) { c = this.ToPoints(scan2, q); //Debug.DrawPoints(c, 2); bool ErrorInM = false; Heap assoc = CorrelatePointsMB(p, c, ref ErrorInM); if (ErrorInM) { Duration = (Environment.TickCount - StartTime) / 1000.0; return(new MatchResult(initQ, q, NumIterations, Duration, false)); } List <Correlation <Vector2> > assocFiltered = getFilteredCorrespondances(assoc); //Debug.DrawPointRelations(assocFiltered.ToArray()); Pose2D q_min = getQmin(assocFiltered); q = IcpScanMatcher.CompoundPoses(q_min, q); // ------ /* Compute the error of the associations */ cosw = System.Math.Cos(q_min.Rotation); sinw = System.Math.Sin(q_min.Rotation); dtx = q_min.X; dty = q_min.Y; error = 0; for (int i = 0; i < assocFiltered.Count; i++) { tmp1 = assocFiltered[i].Point2.X * cosw - assocFiltered[i].Point2.Y * sinw + dtx - assocFiltered[i].Point1.X; tmp1 *= tmp1; tmp2 = assocFiltered[i].Point2.X * sinw + assocFiltered[i].Point2.Y * cosw + dty - assocFiltered[i].Point1.Y; tmp2 *= tmp2; error = error + tmp1 + tmp2; } error_ratio = error / error_k1; // ---- /* Check the exit criteria */ /* Error ratio */ if (System.Math.Abs(1.0 - error_ratio) <= Min_Error || (System.Math.Abs(q_min.X) < Min_Error && System.Math.Abs(q_min.Y) < Min_Error && System.Math.Abs(q_min.Rotation) < Min_Error)) { NumConverged++; } else { NumConverged = 0; } error_k1 = error; if (NumConverged > IterSmoothConv) { //'Converged in iteration %d', k Duration = (Environment.TickCount - StartTime) / 1000.0; return(new MatchResult(initQ, q, NumIterations, Duration, true)); } NumIterations++; } Duration = (Environment.TickCount - StartTime) / 1000.0; return(new MatchResult(initQ, q, MAX_ITERATIONS, Duration, false)); }
private void Run() { string[] lines = File.ReadAllLines(llLogFile.Text); int startIndex = int.Parse(txtPrevLaserIndex.Text); mSeed = new Pose2D(); mPrevPose = null; mCurrPose = null; mEstimated = new Pose2D(); //mSM = new MbIcpScanMatcher(); mSM = new WeightedScanMatcher(); bool Completed = false; while (mRun.ThreadState != ThreadState.AbortRequested) { if (startIndex >= lines.Length) { break; } string l = lines[startIndex++]; CarmenLogParser(l, ref Completed); //USARSimLogParser(l, ref Completed); if (Completed) { Completed = false; ScanObservation m1 = new ScanObservation(m3DScaleFactor, mPrevLaser); ScanObservation m2 = new ScanObservation(m3DScaleFactor, mCurrLaser); MatchResult mR = null; if (chkSeed.Checked) { //mSeed.Position *= 0.003f; //mSeed.Rotation *= 0.9f; mR = mSM.Match(m1, m2, mSeed); } else { //Pose2D initQ = new Pose2D(0.1, 0.1, 0.1); mR = mSM.Match(m1, m2, new Pose2D()); } if (mR.Converged) { Pose2D es = mR.EstimatedOdometry; mEstimated = compound(mEstimated, es); mCurrLaser.posRobot = new Pose2D(mEstimated.Position, mEstimated.Rotation); lock (mMapPoints) { ComputeMapPoints(mCurrLaser, 0); } rwDrawer.Invalidate(); } //if (max(abs(mSeed)) > 0.01f) { mPrevLaser = mCurrLaser; mSeed = new Pose2D(); } } Thread.Sleep(10); } }