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