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);
}
internal KinectSkeleton PredictSkeleton(double msAheadOfNow)
{
KinectSkeleton newSkeleton = new KinectSkeleton();
Joint[] tempJoints = new Joint[KinectBase.HelperMethods.TotalJointCount];
for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++)
{
Joint newJoint = new Joint();
EigenWrapper.Matrix covariance;
newJoint.JointType = newSkeleton.skeleton[i].JointType;
EigenWrapper.Matrix state = filteredJoints[i].PredictAndDiscardFromNow(msAheadOfNow, out covariance);
newJoint.Position = FilteredMatrixToPoint(state);
newJoint.TrackingState = GetJointTrackingState(covariance, lastTrackedTime[i]);
tempJoints[i] = newJoint;
}
//Calculate the orientations for all the skeletons
Quaternion[] orientations = CalculateOrientations(tempJoints);
//Add the orientations to the joints and pass them into the newSkeleton object
for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++)
{
tempJoints[i].Orientation = orientations[i];
newSkeleton.skeleton[i] = tempJoints[i];
}
//TODO: Handle all the per skeleton stuff here (e.g. skeleton tracking state, tracking ID, etc)
newSkeleton.leftHandClosed = lhFilter.PredictMeasurement();
newSkeleton.rightHandClosed = rhFilter.PredictMeasurement();
newSkeleton.Position = newSkeleton.skeleton[JointType.HipCenter].Position;
newSkeleton.SkeletonTrackingState = GetSkeletonTrackingState(newSkeleton);
return(newSkeleton);
}
static void TestMatrixDeterminantCorrectness()
{
double[,] A = { { 1, 4, 7 }, { 2, 5, 8 }, { 3, 6, 9 } }; //Det = 0
double[,] B = { { 1, 4, 3 }, { 2, 5, 8 }, { 3, 6, 9 } }; //Det = 12
EigenWrapper.Matrix AMat = new EigenWrapper.Matrix(3, 3);
EigenWrapper.Matrix BMat = new EigenWrapper.Matrix(3, 3);
//Set the values for the matrix validation tests
AMat[0, 0] = 1;
AMat[0, 1] = 2;
AMat[0, 2] = 3;
AMat[1, 0] = 4;
AMat[1, 1] = 5;
AMat[1, 2] = 6;
AMat[2, 0] = 7;
AMat[2, 1] = 8;
AMat[2, 2] = 9;
BMat[0, 0] = 1;
BMat[0, 1] = 2;
BMat[0, 2] = 3;
BMat[1, 0] = 4;
BMat[1, 1] = 5;
BMat[1, 2] = 6;
BMat[2, 0] = 3;
BMat[2, 1] = 8;
BMat[2, 2] = 9;
//Test the C# determinant accuracy
Console.WriteLine("C# determinant correctness test:");
Console.WriteLine("The first input matrix is:");
PrintMatrix(A);
double[,] Atrans = MatrixMathCS.Transpose(A);
Console.WriteLine("The determinant is {0}", MatrixMathCS.Determinant(Atrans));
Console.WriteLine("The second input matrix is:");
PrintMatrix(B);
double[,] Btrans = MatrixMathCS.Transpose(B);
Console.WriteLine("The determinant is {0}", MatrixMathCS.Determinant(Btrans));
Console.WriteLine();
//Test the Eigen determinant accuracy
Console.WriteLine("P/Invoke Eigen 2D double array determinant correctness test:");
Console.WriteLine("The first input matrix is:");
PrintMatrix(A);
Console.WriteLine("The determinant is {0}", EigenWrapper.MatrixMath.MatrixDeterminant(A));
Console.WriteLine("The second input matrix is:");
PrintMatrix(B);
Console.WriteLine("The determinant is {0}", EigenWrapper.MatrixMath.MatrixDeterminant(B));
Console.WriteLine();
//Test the Matrix class determinant accuracy
Console.WriteLine("P/Invoke Eigen matrix class has inverse correctness test:");
Console.WriteLine("The first input matrix is:");
PrintMatrix(AMat);
Console.WriteLine("The determinant is {0}", AMat.Determinant());
Console.WriteLine("The second input matrix is:");
PrintMatrix(BMat);
Console.WriteLine("The determinant is {0}", EigenWrapper.Matrix.Determinant(BMat));
}
private System.Windows.Media.Media3D.Point3D FilteredMatrixToPoint(EigenWrapper.Matrix matrix)
{
System.Windows.Media.Media3D.Point3D point = new System.Windows.Media.Media3D.Point3D();
point.X = matrix[0, 0];
point.Y = matrix[3, 0];
point.Z = matrix[6, 0];
return(point);
}
private EigenWrapper.Matrix PointToObMatrix(System.Windows.Media.Media3D.Point3D point)
{
EigenWrapper.Matrix matrix = new EigenWrapper.Matrix(3, 1);
matrix[0, 0] = point.X;
matrix[1, 0] = point.Y;
matrix[2, 0] = point.Z;
return(matrix);
}
//This method only gets the predicted positions. It is used for the skeleton comparison to save CPU power.
internal Point3D[] PredictPositionsOnly(double msAheadOfNow)
{
Point3D[] tempPositions = new Point3D[KinectBase.HelperMethods.TotalJointCount];
for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++)
{
EigenWrapper.Matrix state = filteredJoints[i].PredictAndDiscardFromNow(msAheadOfNow);
tempPositions[i] = FilteredMatrixToPoint(state);
}
return(tempPositions);
}
static void TestMatrixInvertSpeed(EigenWrapper.Matrix A)
{
EigenWrapper.Matrix C;
Stopwatch watch = Stopwatch.StartNew();
for (int i = 0; i < testIterations; i++)
{
C = A.Inverse();
}
watch.Stop();
Console.WriteLine("P/Invoked Eigen matrix class inverse test took: " + watch.ElapsedMilliseconds + " ms.");
}
static void TestMatrixMultiplySpeed(EigenWrapper.Matrix A, EigenWrapper.Matrix B)
{
EigenWrapper.Matrix C;
Stopwatch watch = Stopwatch.StartNew();
for (int i = 0; i < testIterations; i++)
{
C = A * B;
}
watch.Stop();
Console.WriteLine("P/Invoked Eigen matrix class multiply test took: " + watch.ElapsedMilliseconds + " ms.");
}
static void TestMatrixVectorMultiplyCorrectness()
{
double[,] ATest = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
double[] BTest = { 1, 2, 3 };
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(2, 3);
EigenWrapper.Vector BVecTest = new EigenWrapper.Vector(3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
BVecTest[0] = 1;
BVecTest[1] = 2;
BVecTest[2] = 3;
//This doesn't test well because of the transposed interpretation of a 2D double array in the C# version
//Testing the C# multiplcation accuracy
//Console.WriteLine("C# matrix * vector correctness test:");
//Console.WriteLine("The input matrix is:");
//PrintMatrix(ATest);
//Console.WriteLine("The input scalar is:");
//PrintVector(BTest);
//Console.WriteLine("The result is:");
//double[,] Atrans = MatrixMathCS.Transpose(ATest);
//double[] C = MatrixMathCS.VectorTimesMatrix(BTest, Atrans);
//PrintVector(C);
//Console.WriteLine();
//Test the Eigen multiplication accuracy
Console.WriteLine("P/Invoke Eigen 2D double array matrix * vector correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The input scalar is:");
PrintVector(BTest);
Console.WriteLine("The result is:");
double[] D = EigenWrapper.MatrixMath.MultiplyMatrixVector(ATest, BTest);
PrintVector(D);
Console.WriteLine();
//Test the Matrix class multiplication accuracy
Console.WriteLine("P/Invoke Eigen matrix class matrix * vector correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(AMatTest);
Console.WriteLine("The input scalar is:");
PrintVector(BVecTest);
Console.WriteLine("The result is:");
EigenWrapper.Vector CVec = AMatTest * BVecTest;
PrintVector(CVec);
}
static void TestMatrixScalarMultiplyCorrectness()
{
double[,] ATest = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
double b = 3;
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(2, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
//Testing the C# multiplcation accuracy
Console.WriteLine("C# matrix * scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] C = MatrixMathCS.ScalarTimesMatrix(b, Atrans);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen multiplication accuracy
Console.WriteLine("P/Invoke Eigen 2D double array matrix * scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The result is:");
double[,] D = EigenWrapper.MatrixMath.MultiplyMatrixScalar(ATest, b);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class multiplication accuracy
Console.WriteLine("P/Invoke Eigen matrix class matrix * scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(AMatTest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The b*A result is:");
EigenWrapper.Matrix CMat = b * AMatTest;
PrintMatrix(CMat);
Console.WriteLine("The A*b result is:");
EigenWrapper.Matrix DMat = AMatTest * b;
PrintMatrix(DMat);
}
static void TestInverseCorrectness()
{
double[,] A = { { 1, -2, -3, -4 }, { 2, -5, -6, -7 }, { 3, 7, -8, -9 }, { 4, 8, 12, 16 } };
EigenWrapper.Matrix AMat = new EigenWrapper.Matrix(4, 4);
AMat[0, 0] = 1;
AMat[0, 1] = 2;
AMat[0, 2] = 3;
AMat[0, 3] = 4;
AMat[1, 0] = -2;
AMat[1, 1] = -5;
AMat[1, 2] = 7;
AMat[1, 3] = 8;
AMat[2, 0] = -3;
AMat[2, 1] = -6;
AMat[2, 2] = -8;
AMat[2, 3] = 12;
AMat[3, 0] = -4;
AMat[3, 1] = -7;
AMat[3, 2] = -9;
AMat[3, 3] = 16;
//Test the C# inverse correctness
Console.WriteLine("C# matrix inverse...");
Console.WriteLine("The input matrix is:");
PrintMatrix(A);
Console.WriteLine("The inverse matrix is:");
double[,] Atrans = MatrixMathCS.Transpose(A);
double[,] C = MatrixMathCS.InverseMatrix(Atrans);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen matrix inverse correctness
Console.WriteLine("P/Invoke Eigen 2D double array matrix inverse...");
Console.WriteLine("The input matrix is:");
PrintMatrix(A);
Console.WriteLine("The inverse matrix is:");
double[,] D = EigenWrapper.MatrixMath.InvertMatrix(A);
PrintMatrix(D);
Console.WriteLine();
//Test the matrix class inverse correctness
Console.WriteLine("P/Invoke Eigen matrix class inverse...");
Console.WriteLine("The input matrix is:");
PrintMatrix(AMat);
Console.WriteLine("The inverse matrix is:");
EigenWrapper.Matrix CMat = AMat.Inverse();
PrintMatrix(CMat);
}
static void TestMatrixScalarDivideCorrectness()
{
double[,] ATest = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
double b = 2;
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(2, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
//Testing the C# divide accuracy
Console.WriteLine("C# matrix / scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] C = MatrixMathCS.DivideMatrixbyScalar(Atrans, b);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen divide accuracy
Console.WriteLine("P/Invoke Eigen 2D double array matrix / scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The result is:");
double[,] D = EigenWrapper.MatrixMath.DivideMatrixScalar(ATest, b);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class divide accuracy
Console.WriteLine("P/Invoke Eigen matrix class matrix / scalar correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(AMatTest);
Console.WriteLine("The input scalar is:");
Console.WriteLine(b);
Console.WriteLine("The result is:");
EigenWrapper.Matrix CMat = AMatTest / b;
PrintMatrix(CMat);
}
static void PrintMatrix(EigenWrapper.Matrix matrix)
{
for (int i = 0; i < matrix.Rows; i++)
{
for (int j = 0; j < matrix.Columns; j++)
{
Console.Write(matrix[i, j].ToString());
if (j < matrix.Columns - 1)
{
Console.Write(", ");
}
}
Console.Write("\r\n");
}
}
static void TestTransposeCorrectness()
{
double[,] ATest = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(2, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
//Testing the C# transpose accuracy
Console.WriteLine("C# transpose correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] C = MatrixMathCS.Transpose(ATest);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(C);
Console.WriteLine();
//Test the Eigen transpose accuracy
Console.WriteLine("P/Invoke Eigen 2D double array transpose correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(ATest);
Console.WriteLine("The result is:");
double[,] D = EigenWrapper.MatrixMath.TransposeMatrix(ATest);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class transpose accuracy
Console.WriteLine("P/Invoke Eigen matrix class transpose correctness test:");
Console.WriteLine("The input matrix is:");
PrintMatrix(AMatTest);
Console.WriteLine("The result is:");
EigenWrapper.Matrix CMat = AMatTest.Transpose();
PrintMatrix(CMat);
}
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());
}
private void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skelFrame = e.OpenSkeletonFrame())
{
if (skelFrame != null && masterSettings.kinectOptionsList.Count > kinectID && (masterKinectSettings.mergeSkeletons || masterKinectSettings.sendRawSkeletons))
{
DateTime now = DateTime.UtcNow;
Skeleton[] skeletons = new Skeleton[skelFrame.SkeletonArrayLength];
skelFrame.CopySkeletonDataTo(skeletons);
EigenWrapper.Matrix predAccel = new EigenWrapper.Matrix(3, 1);
predAccel[2, 0] = 1;
if (accelFilterStarted)
{
predAccel = accelerationFilter.PredictAndDiscard(0);
}
if (interactStream != null)
{
Vector4 filteredAccel = new Vector4();
filteredAccel.W = 0;
filteredAccel.X = (float)predAccel[0, 0];
filteredAccel.Y = (float)predAccel[1, 0];
filteredAccel.Z = (float)predAccel[2, 0];
interactStream.ProcessSkeleton(skeletons, filteredAccel, skelFrame.Timestamp);
//System.Diagnostics.Trace.WriteLine("[" + filteredAccel.X + ", " + filteredAccel.Y + ", " + filteredAccel.Z + "]");
}
//Generate the transformation matrix for the skeletons
double kinectYaw = masterKinectSettings.kinectYaw;
Point3D kinectPosition = masterKinectSettings.kinectPosition;
Matrix3D gravityBasedKinectRotation = findRotation(new Vector3D(predAccel[0, 0], predAccel[1, 0], predAccel[2, 0]), new Vector3D(0, -1, 0));
AxisAngleRotation3D yawRotation = new AxisAngleRotation3D(new Vector3D(0, 1, 0), -kinectYaw);
RotateTransform3D tempYawTrans = new RotateTransform3D(yawRotation);
TranslateTransform3D transTrans = new TranslateTransform3D((Vector3D)kinectPosition);
Matrix3D rotationOnlyMatrix = Matrix3D.Multiply(tempYawTrans.Value, gravityBasedKinectRotation);
Matrix3D masterMatrix = Matrix3D.Multiply(rotationOnlyMatrix, transTrans.Value);
skeletonTransformation = masterMatrix;
skeletonRotQuaternion = KinectBase.HelperMethods.MatrixToQuaternion(rotationOnlyMatrix);
//Convert from Kinect v1 skeletons to KVR skeletons
KinectBase.KinectSkeleton[] kvrSkeletons = new KinectBase.KinectSkeleton[skeletons.Length];
for (int i = 0; i < kvrSkeletons.Length; i++)
{
//Set the tracking ID numbers for the hand grab data
int grabID = -1;
for (int j = 0; j < skeletonHandGrabData.Count; j++)
{
if (skeletonHandGrabData[j].skeletonTrackingID == skeletons[i].TrackingId)
{
grabID = j;
break;
}
}
if (grabID < 0)
{
skeletonHandGrabData.Add(new HandGrabInfo(skeletons[i].TrackingId));
grabID = skeletonHandGrabData.Count - 1;
}
kvrSkeletons[i] = new KinectBase.KinectSkeleton();
kvrSkeletons[i].Position = new Point3D(skeletons[i].Position.X, skeletons[i].Position.Y, skeletons[i].Position.Z);
kvrSkeletons[i].SkeletonTrackingState = convertTrackingState(skeletons[i].TrackingState);
kvrSkeletons[i].TrackingId = skeletons[i].TrackingId;
//kvrSkeletons[i].utcSampleTime = DateTime.UtcNow;
kvrSkeletons[i].sourceKinectID = kinectID;
for (int j = 0; j < skeletons[i].Joints.Count; j++)
{
KinectBase.Joint newJoint = new KinectBase.Joint();
newJoint.Confidence = KinectBase.TrackingConfidence.Unknown; //The Kinect 1 doesn't support the confidence property
newJoint.JointType = convertJointType(skeletons[i].Joints[(JointType)j].JointType);
Vector4 tempQuat = skeletons[i].BoneOrientations[(JointType)j].AbsoluteRotation.Quaternion;
newJoint.Orientation.orientationQuaternion = new Quaternion(tempQuat.X, tempQuat.Y, tempQuat.Z, tempQuat.W);
Matrix4 tempMat = skeletons[i].BoneOrientations[(JointType)j].AbsoluteRotation.Matrix;
newJoint.Orientation.orientationMatrix = new Matrix3D(tempMat.M11, tempMat.M12, tempMat.M13, tempMat.M14,
tempMat.M21, tempMat.M22, tempMat.M23, tempMat.M24,
tempMat.M31, tempMat.M32, tempMat.M33, tempMat.M34,
tempMat.M41, tempMat.M42, tempMat.M43, tempMat.M44);
SkeletonPoint tempPos = skeletons[i].Joints[(JointType)j].Position;
newJoint.Position = new Point3D(tempPos.X, tempPos.Y, tempPos.Z);
newJoint.TrackingState = convertTrackingState(skeletons[i].Joints[(JointType)j].TrackingState);
newJoint.spatialErrorStdDev = getJointError(newJoint.Position, newJoint.TrackingState);
newJoint.utcTime = now;
kvrSkeletons[i].skeleton[newJoint.JointType] = newJoint; //Skeleton doesn't need to be initialized because it is done in the KinectSkeleton constructor
}
//Get the hand states from the hand grab data array
kvrSkeletons[i].rightHandClosed = skeletonHandGrabData[grabID].rightHandClosed;
kvrSkeletons[i].leftHandClosed = skeletonHandGrabData[grabID].leftHandClosed;
}
//Add the skeleton data to the event handler and throw the event
KinectBase.SkeletonEventArgs skelE = new KinectBase.SkeletonEventArgs();
skelE.skeletons = kvrSkeletons;
skelE.kinectID = kinectID;
OnSkeletonChanged(skelE);
}
}
}
//Updates the acceleration on the GUI and the server, 30 FPS may be a little fast for the GUI, but for VRPN, it probably needs to be at least that fast
private void updateTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
{
//Update the acceleration data
bool dataValid = false;
Vector4?acceleration = null;
int? elevationAngle = null;
lock (kinect)
{
if (kinect != null && kinect.IsRunning)
{
//I wish these try/catch statements weren't necessary, but these two calls seemed to fail often
dataValid = true;
try
{
acceleration = kinect.AccelerometerGetCurrentReading();
}
catch
{
acceleration = null;
dataValid = false;
}
if (dataValid) //We can't even try to calculate the elevation angle if the accelerometer doesn't read right
{
try
{
elevationAngle = kinect.ElevationAngle;
}
catch
{
elevationAngle = null;
dataValid = false;
}
}
}
}
//Update the GUI
if (dataValid)
{
//Update the filtered acceleration
EigenWrapper.Matrix accelMat = new EigenWrapper.Matrix(3, 1);
accelMat[0, 0] = acceleration.Value.X;
accelMat[1, 0] = acceleration.Value.Y;
accelMat[2, 0] = acceleration.Value.Z;
accelerationFilter.IntegrateMeasurement(accelMat, DateTime.UtcNow, 0.01);
accelFilterStarted = true;
//lastAcceleration = acceleration.Value;
//Transmits the acceleration data using an event
KinectBase.AccelerationEventArgs accelE = new KinectBase.AccelerationEventArgs();
accelE.kinectID = this.kinectID;
accelE.acceleration = new Vector3D(acceleration.Value.X, acceleration.Value.Y, acceleration.Value.Z);
accelE.elevationAngle = elevationAngle.Value;
OnAccelerationChanged(accelE);
}
else
{
KinectBase.AccelerationEventArgs accelE = new KinectBase.AccelerationEventArgs();
accelE.kinectID = this.kinectID;
//Send the acceleration, if it is valid
if (acceleration.HasValue)
{
//Update the filtered acceleration
EigenWrapper.Matrix accelMat = new EigenWrapper.Matrix(3, 1);
accelMat[0, 0] = acceleration.Value.X;
accelMat[1, 0] = acceleration.Value.Y;
accelMat[2, 0] = acceleration.Value.Z;
accelerationFilter.IntegrateMeasurement(accelMat, DateTime.UtcNow, 0.01);
accelFilterStarted = true;
//lastAcceleration = acceleration.Value;
accelE.acceleration = new Vector3D(acceleration.Value.X, acceleration.Value.Y, acceleration.Value.Z);
}
else
{
accelE.acceleration = null;
}
//Send the Kinect angle if it is valid
if (elevationAngle.HasValue)
{
accelE.elevationAngle = elevationAngle.Value;
}
else
{
accelE.elevationAngle = null;
}
OnAccelerationChanged(accelE);
}
}
//Correctness test functions (these are not rigorous tests, just a spot check!)
static void TestMultiplyCorrectness()
{
double[,] ATest = { { 1, 3, 5, 7 }, { 2, 4, 6, 8 } };
double[,] BTest = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(4, 2);
EigenWrapper.Matrix BMatTest = new EigenWrapper.Matrix(2, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[1, 0] = 3;
AMatTest[1, 1] = 4;
AMatTest[2, 0] = 5;
AMatTest[2, 1] = 6;
AMatTest[3, 0] = 7;
AMatTest[3, 1] = 8;
BMatTest[0, 0] = 1;
BMatTest[0, 1] = 2;
BMatTest[0, 2] = 3;
BMatTest[1, 0] = 4;
BMatTest[1, 1] = 5;
BMatTest[1, 2] = 6;
double[,] C = new double[BTest.GetLength(0), ATest.GetLength(1)];
double[,] D = new double[BTest.GetLength(0), ATest.GetLength(1)];
EigenWrapper.Matrix CMat;
//Testing the C# multiplcation accuracy
Console.WriteLine("C# multiplication accuracy test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] Btrans = MatrixMathCS.Transpose(BTest);
C = MatrixMathCS.MultiplyMatrices(Atrans, Btrans);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen multiplication accuracy
Console.WriteLine("P/Invoke Eigen 2D double array multiplication correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
D = EigenWrapper.MatrixMath.MultiplyMatrices(ATest, BTest);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class multiplication accuracy
Console.WriteLine("P/Invoke Eigen matrix class multiplication correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(AMatTest);
Console.WriteLine("and:");
PrintMatrix(BMatTest);
Console.WriteLine("The result is:");
CMat = AMatTest * BMatTest;
PrintMatrix(CMat);
}
static void TestAdditionCorrectness()
{
double[,] ATest = { { 1, 4, 7 }, { 2, 5, 8 }, { 3, 6, 9 } };
double[,] BTest = { { 1, 4, 7 }, { 2, 5, 8 }, { 3, 6, 9 } };
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(3, 3);
EigenWrapper.Matrix BMatTest = new EigenWrapper.Matrix(3, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
AMatTest[2, 0] = 7;
AMatTest[2, 1] = 8;
AMatTest[2, 2] = 9;
BMatTest[0, 0] = 1;
BMatTest[0, 1] = 2;
BMatTest[0, 2] = 3;
BMatTest[1, 0] = 4;
BMatTest[1, 1] = 5;
BMatTest[1, 2] = 6;
BMatTest[2, 0] = 7;
BMatTest[2, 1] = 8;
BMatTest[2, 2] = 9;
//Testing the C# addition accuracy
Console.WriteLine("C# addition correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] Btrans = MatrixMathCS.Transpose(BTest);
double[,] C = MatrixMathCS.AddMatrices(Atrans, Btrans);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen addition accuracy
Console.WriteLine("P/Invoke Eigen 2D double array addition correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
double[,] D = EigenWrapper.MatrixMath.AddMatrices(ATest, BTest);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class addition accuracy
Console.WriteLine("P/Invoke Eigen matrix class addition correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(AMatTest);
Console.WriteLine("and:");
PrintMatrix(BMatTest);
Console.WriteLine("The result is:");
EigenWrapper.Matrix CMat = AMatTest + BMatTest;
PrintMatrix(CMat);
}
static void TestSubtractionCorrectness()
{
double[,] ATest = { { 1, 4, 7 }, { 2, 5, 8 }, { 3, 6, 9 } };
double[,] BTest = { { 10, 40, 70 }, { 20, 50, 80 }, { 30, 60, 90 } };
EigenWrapper.Matrix AMatTest = new EigenWrapper.Matrix(3, 3);
EigenWrapper.Matrix BMatTest = new EigenWrapper.Matrix(3, 3);
//Set the values for the matrix validation tests
AMatTest[0, 0] = 1;
AMatTest[0, 1] = 2;
AMatTest[0, 2] = 3;
AMatTest[1, 0] = 4;
AMatTest[1, 1] = 5;
AMatTest[1, 2] = 6;
AMatTest[2, 0] = 7;
AMatTest[2, 1] = 8;
AMatTest[2, 2] = 9;
BMatTest[0, 0] = 10;
BMatTest[0, 1] = 20;
BMatTest[0, 2] = 30;
BMatTest[1, 0] = 40;
BMatTest[1, 1] = 50;
BMatTest[1, 2] = 60;
BMatTest[2, 0] = 70;
BMatTest[2, 1] = 80;
BMatTest[2, 2] = 90;
//Testing the C# subtraction accuracy
Console.WriteLine("C# subtraction correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
double[,] Atrans = MatrixMathCS.Transpose(ATest);
double[,] Btrans = MatrixMathCS.Transpose(BTest);
double[,] C = MatrixMathCS.SubtractMatrices(Atrans, Btrans);
double[,] Ctrans = MatrixMathCS.Transpose(C);
PrintMatrix(Ctrans);
Console.WriteLine();
//Test the Eigen subtraction accuracy
Console.WriteLine("P/Invoke Eigen 2D double array subtraction correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(ATest);
Console.WriteLine("and:");
PrintMatrix(BTest);
Console.WriteLine("The result is:");
double[,] D = EigenWrapper.MatrixMath.SubtractMatrices(ATest, BTest);
PrintMatrix(D);
Console.WriteLine();
//Test the Matrix class subtraction accuracy
Console.WriteLine("P/Invoke Eigen matrix class subtraction correctness test:");
Console.WriteLine("The input matrices are:");
PrintMatrix(AMatTest);
Console.WriteLine("and:");
PrintMatrix(BMatTest);
Console.WriteLine("The result is:");
EigenWrapper.Matrix CMat = AMatTest - BMatTest;
PrintMatrix(CMat);
}
static void TestIsInvertableCorrectness()
{
double[,] A = { { 1, 4 }, { 2, 5 }, { 3, 6 } };
double[,] B = { { 1, 4, 7 }, { 2, 5, 8 }, { 3, 6, 9 } }; //Det = 0
double[,] C = { { 1, 4, 3 }, { 2, 5, 8 }, { 3, 6, 9 } }; //Det = 12
EigenWrapper.Matrix AMat = new EigenWrapper.Matrix(2, 3);
EigenWrapper.Matrix BMat = new EigenWrapper.Matrix(3, 3);
EigenWrapper.Matrix CMat = new EigenWrapper.Matrix(3, 3);
//Set the values for the matrix validation tests
AMat[0, 0] = 1;
AMat[0, 1] = 2;
AMat[0, 2] = 3;
AMat[1, 0] = 4;
AMat[1, 1] = 5;
AMat[1, 2] = 6;
BMat[0, 0] = 1;
BMat[0, 1] = 2;
BMat[0, 2] = 3;
BMat[1, 0] = 4;
BMat[1, 1] = 5;
BMat[1, 2] = 6;
BMat[2, 0] = 7;
BMat[2, 1] = 8;
BMat[2, 2] = 9;
CMat[0, 0] = 1;
CMat[0, 1] = 2;
CMat[0, 2] = 3;
CMat[1, 0] = 4;
CMat[1, 1] = 5;
CMat[1, 2] = 6;
CMat[2, 0] = 3;
CMat[2, 1] = 8;
CMat[2, 2] = 9;
//Test the Eigen multiplication accuracy
Console.WriteLine("P/Invoke Eigen 2D double array has inverse correctness test:");
Console.WriteLine("The first input matrix is:");
PrintMatrix(A);
Console.WriteLine("Has inverse? {0}", EigenWrapper.MatrixMath.HasInverse(A));
Console.WriteLine("The second input matrix is:");
PrintMatrix(B);
Console.WriteLine("Has inverse? {0}", EigenWrapper.MatrixMath.HasInverse(B));
Console.WriteLine("The third input matrix is:");
PrintMatrix(C);
Console.WriteLine("Has inverse? {0}", EigenWrapper.MatrixMath.HasInverse(C));
Console.WriteLine();
//Test the Matrix class multiplication accuracy
Console.WriteLine("P/Invoke Eigen matrix class has inverse correctness test:");
Console.WriteLine("The first input matrix is:");
PrintMatrix(AMat);
Console.WriteLine("Has inverse? {0}", AMat.HasInverse);
Console.WriteLine("The second input matrix is:");
PrintMatrix(BMat);
Console.WriteLine("Has inverse? {0}", BMat.HasInverse);
Console.WriteLine("The third input matrix is:");
PrintMatrix(CMat);
Console.WriteLine("Has inverse? {0}", CMat.HasInverse);
}
static void Main(string[] args)
{
//Create the test matrices
double[,] A = MatrixMathCS.RandomMatrix(matrixSize, matrixSize, 100, -100);
double[,] B = MatrixMathCS.RandomMatrix(matrixSize, matrixSize, 100, -100);
EigenWrapper.Matrix AMatrix = EigenWrapper.Matrix.RandomMatrix(matrixSize, matrixSize, -100, 100);
EigenWrapper.Matrix BMatrix = EigenWrapper.Matrix.RandomMatrix(matrixSize, matrixSize, -100, 100);
EigenWrapper_CppCLI.MatrixXd matrixA = new EigenWrapper_CppCLI.MatrixXd(matrixSize, matrixSize);
EigenWrapper_CppCLI.MatrixXd matrixB = new EigenWrapper_CppCLI.MatrixXd(matrixSize, matrixSize);
//Run the multiplication speed tests, if requested
if (runMultSpeed)
{
TestCSMultiplySpeed(A, B);
TestCppCLIMultiplySpeed(matrixA, matrixB);
TestEigenMultiplySpeed(A, B);
TestMatrixMultiplySpeed(AMatrix, BMatrix);
}
else
{
Console.WriteLine("Skipping multiplication speed tests...");
}
ConditionalPause();
Console.WriteLine();
//Run the matrix inversion speed tests, if requested
if (runInvSpeed)
{
TestCSInvertSpeed(A);
//CppCLI inverse hasn't been implemented
TestEigenInvertSpeed(A);
TestMatrixInvertSpeed(AMatrix);
}
else
{
Console.WriteLine("Skipping matrix inversion speed tests...");
}
ConditionalPause();
Console.WriteLine();
//Run the method correctness tests, if requested
if (runCorrectness)
{
//Test the correctness of the matrix multiplication
TestMultiplyCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix inverse calculations
TestInverseCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix addition calculations
TestAdditionCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix subtraction calculations
TestSubtractionCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the transpose
TestTransposeCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix * scalar calculation
TestMatrixScalarMultiplyCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix / scalar calculation
TestMatrixScalarDivideCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix * vector calculation
TestMatrixVectorMultiplyCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the determinant calculation
TestMatrixDeterminantCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the has inverse calculation
TestIsInvertableCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the matrix norm calculation
TestMatrixNormCorrectness();
ConditionalPause();
Console.WriteLine();
//Test the correctness of the vector cross product calculation
TestVectorCrossProduct();
ConditionalPause();
Console.WriteLine();
}
else
{
Console.WriteLine("Skipping correctness tests...");
}
Console.WriteLine();
Console.WriteLine("Press enter to exit...");
Console.ReadLine();
}