/// <summary>
/// Update the vehicle pose.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="reading">Odometry reading (dx, dy, dz, dpitch, dyaw, droll).</param>
public override void Update(GameTime time, double[] reading)
{
if (OnlyMapping)
{
BestEstimate.Pose = RefVehicle.Pose.DClone();
BestEstimate.WayPoints = new TimedState(RefVehicle.WayPoints);
}
else
{
BestEstimate.Update(time, reading);
}
}
/// <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);
}
