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(); }