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