private TrackingState GetJointTrackingState(EigenWrapper.Matrix covariance, DateTime lastTrackedTime)
{
double logNorm = Math.Log(covariance.Norm());
TrackingState state = TrackingState.NotTracked;
TimeSpan trackingAge = DateTime.UtcNow - lastTrackedTime;
//For debugging only
//System.Diagnostics.Debug.WriteLine("Joint has age {0} ms with norm {1}", trackingAge.TotalMilliseconds, logNorm);
//TODO: Test to see if these thresholds need to be modified
if (trackingAge.TotalMilliseconds > 1000)
{
//If the tracking age is old, but it still has a decent prediction, the joint is inferred
if (logNorm < 2)
{
state = TrackingState.Inferred;
}
}
else
{
//If the tracking age is new, determine if it is tracked, inferred, or not tracked (default) based on how good the prediction is
if (logNorm < 0.75)
{
state = TrackingState.Tracked;
}
else if (logNorm < 2)
{
state = TrackingState.Inferred;
}
}
return(state);
}
static void TestMatrixNormCorrectness()
{
double[,] A = { { 51, 634, 70 }, { 2, 57, 8 }, { 53, 63, 91 } }; //Det = 0
EigenWrapper.Matrix AMat = new EigenWrapper.Matrix(3, 3);
//Set the values for the matrix validation tests
AMat[0, 0] = 51;
AMat[0, 1] = 2;
AMat[0, 2] = 53;
AMat[1, 0] = 634;
AMat[1, 1] = 57;
AMat[1, 2] = 63;
AMat[2, 0] = 70;
AMat[2, 1] = 8;
AMat[2, 2] = 91;
//Test the C# matrix norm
Console.WriteLine("C# matrix norm correctness test:");
Console.WriteLine("The test matrix A is:");
PrintMatrix(A);
Console.WriteLine("The matrix norm of A is:");
double[,] Atrans = MatrixMathCS.Transpose(A);
Console.WriteLine(MatrixMathCS.MatrixNorm(Atrans).ToString());
Console.WriteLine();
//Test the Eigen matrix norm
Console.WriteLine("P/Invoke Eigen 2D double array matrix norm correctness test:");
Console.WriteLine("The test matrix A is:");
PrintMatrix(A);
Console.WriteLine("The matrix norm of A is:");
Console.WriteLine(EigenWrapper.MatrixMath.MatrixNorm(A));
Console.WriteLine();
//Test the Matrix class norm
Console.WriteLine("P/Invoke Eigen matrix class matrix norm correctness test:");
Console.WriteLine("The test matrix A is:");
PrintMatrix(AMat);
Console.WriteLine("The matrix norm of A is:");
Console.WriteLine(AMat.Norm().ToString());
}
