Пример #1
0
        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();
                }
            }
        }