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";
}
private void CarmenLogParser(string l, ref bool completed)
{
USARParser p = new USARParser(l, "\":", true);
string odo = p.getString("odometry").Replace(" ", "");
odo = "SEN {Type Odometry} {Name Odometry} {Pose " + odo + "}";
mUSARParser.SimulationMessage = odo;
Odometry mOdo = (Odometry)mUSARParser.MessageData;
Pose2D mNewPose = new Pose2D(mOdo.x * m3DScaleFactor, mOdo.y * m3DScaleFactor, mOdo.theta);
mPrevPose = mCurrPose;
mCurrPose = mNewPose;
if (mPrevPose != null)
{
mSeed = new Pose2D(mSeed.Position + (mCurrPose.Position - mPrevPose.Position),
mSeed.Rotation - (mCurrPose.Rotation - mPrevPose.Rotation));
}
else
{
mSeed = new Pose2D();
}
string lsr = p.getString("readings").Replace(" ", "");
string valid = p.getString("valid").Replace(" ", "");
string theta = p.getString("theta").Replace(" ", "");
float nrays = float.Parse(p.getString("nrays"));
float min_theta = float.Parse(p.getString("min_theta"));
float max_theta = float.Parse(p.getString("max_theta"));
float fov = max_theta - min_theta;
float res = fov / nrays;
string range = "SEN {Time 0.0} {Type RangeScanner} {Name Scanner1} {Resolution " + res + "} {FOV " + fov + "} {Range " + lsr + "} {Valid " +
valid + "} {Theta " + theta + "}";
mUSARParser.SimulationMessage = range;
Laser mLaser = (Laser)mUSARParser.MessageData;
if (mPrevLaser == null)
{
mPrevLaser = mLaser;
}
else
{
mCurrLaser = mLaser;
completed = true;
}
}