Ejemplo n.º 1
0
        /// <summary>
        /// Render the 5-sigma ellipse of gaussian.
        /// </summary>
        /// <param name="gaussian">Gaussian to be rendered.</param>
        /// <param name="camera">Camera 4d transform matrix.</param>
        /// <param name="incolor">Color to be used for filling the gaussian.</param>
        public void RenderGaussian(Gaussian gaussian, double[][] camera, Color incolor)
        {
            Color outcolor = Color.Blue;

            incolor.A  = 200;
            outcolor.A = 200;

            double weight;

            if (gaussian.Weight < 1.0)
            {
                weight     = 0.04 * gaussian.Weight;
                incolor.A  = (byte)(200 * gaussian.Weight);
                outcolor.A = (byte)(200 * gaussian.Weight);
            }
            else if (gaussian.Weight < 2.0)
            {
                weight   = 0.01 * (gaussian.Weight - 1) + 0.04;
                outcolor = Color.Black;
            }
            else
            {
                weight   = 0.04;
                outcolor = Color.Red;
            }

            if (ShowVisible)
            {
                outcolor = (BestEstimate.Visible(gaussian.Mean)) ? Color.Blue : Color.Red;
            }

            double[]   camloc  = camera.TransformH(gaussian.Mean);
            double[][] camrot  = camera.Submatrix(0, 2, 0, 2);
            double     camzoom = 1.0 / camera[3][3];

            double[][] covariance = camrot.Multiply(gaussian.Covariance).MultiplyByTranspose(camrot).Submatrix(0, 1, 0, 1);

            var decomp = new EigenvalueDecomposition(covariance.ToMatrix());

            double[,]  stddev = decomp.DiagonalMatrix;

            for (int i = 0; i < stddev.GetLength(0); i++)
            {
                stddev[i, i] = (stddev[i, i] > 0) ? camzoom * Math.Sqrt(stddev[i, i]) : 0;
            }

            double[,]  rotation = decomp.Eigenvectors;
            double[][] linear = rotation.Multiply(stddev).ToArray();

            if (linear.Determinant() < 0)
            {
                linear = linear.ReverseColumns();
            }

            double[][]            points   = new double[pinterval.Length][];
            VertexPositionColor[] vertices = new VertexPositionColor[pinterval.Length];

            for (int i = 0; i < points.Length; i++)
            {
                points  [i] = linear.Multiply(pinterval[i]);
                points  [i] = new double[3] {
                    points[i][0], points[i][1], 0
                }.Add(camloc);
                vertices[i] = new VertexPositionColor(points[i].ToVector3(), incolor);
            }

            short[] index = new short[vertices.Length];

            for (int i = 0, k = 0; k < vertices.Length; i++, k += 2)
            {
                index[k] = (short)i;
            }

            for (int i = vertices.Length - 1, k = 1; k < vertices.Length; i--, k += 2)
            {
                index[k] = (short)i;
            }

            Graphics.DrawUserIndexedPrimitives(PrimitiveType.TriangleStrip, vertices, 0, vertices.Length, index, 0, vertices.Length - 2);
            Graphics.DrawUser2DPolygon(points, (float)weight, outcolor, true);
        }
Ejemplo n.º 2
0
 /// <summary>
 /// Add the specified landmark into the map.
 /// </summary>
 /// <param name="landmark">Landmark to be added.</param>
 public void Add(Gaussian landmark)
 {
     landmarks.Add(landmark);
 }
Ejemplo n.º 3
0
 /// <summary>
 /// Render the 5-sigma ellipse of gaussian (using the default color).
 /// </summary>
 /// <param name="gaussian">Gaussian to be rendered.</param>
 /// <param name="camera">Camera 4d transform matrix.</param>
 public void RenderGaussian(Gaussian gaussian, double[][] camera)
 {
     RenderGaussian(gaussian, camera, Color.DeepSkyBlue);
 }