public void Draw(Renderer cam)
{
if (poses.Count == 0)
{
return;
}
lock (poses)
{
RobotPose lastPose = poses[0];
foreach (RobotPose p in poses)
{
GLUtility.DrawCross(pen, p.ToVector2(), .2f);
//GLUtility.DrawLine(pen, lastPose.ToVector2(), p.ToVector2());
lastPose = p;
}
if (poses[poses.Count - 1].covariance[0, 0] != 0)
{
//calculate the error ellipse
Matrix cov = new Matrix(2, 2);
cov[0, 0] = poses[poses.Count - 1].covariance[0, 0];
cov[0, 1] = poses[poses.Count - 1].covariance[0, 1];
cov[1, 0] = poses[poses.Count - 1].covariance[1, 0];
cov[1, 1] = poses[poses.Count - 1].covariance[1, 1];
double theta = .5 * Math.Atan2((-2 * cov[0, 1]), (cov[0, 0] - cov[1, 1]));
if (Double.IsNaN(theta))
{
theta = 0;
}
double sigu2 = (cov[0, 0] * Math.Sin(theta) * Math.Sin(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Cos(theta) * Math.Cos(theta));
double sigv2 = (cov[0, 0] * Math.Cos(theta) * Math.Cos(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Sin(theta) * Math.Sin(theta));
GLUtility.GoToTransformXYZ((float)poses[poses.Count - 1].x, (float)poses[poses.Count - 1].y, 0);
GLUtility.GoToTransformYPR((float)theta, 0, 0);
GLUtility.DrawEllipse(ellipsePen, new RectangleF((float)-sigu2 / 2, (float)-sigv2 / 2, (float)sigu2, (float)sigv2));
GLUtility.ComeBackFromTransform();
GLUtility.ComeBackFromTransform();
}
}
}
