public static Skeleton ApplyTransformToFrame(Skeleton frame, Matrix3x3 trans) { Skeleton res = new Skeleton(); foreach (Joint j in frame.Joints) { Joint j2 = j; j2.Position = MultiplyPositionMatrix(trans, j2.Position); res.Joints[j2.JointType] = j2; } return res; }
public static Matrix3x3 GetRotationMatrix(Skeleton mFrame, Skeleton rFrame) { Skeleton mFrameShift = ShiftFrame(mFrame, mFrame.Joints[JointType.HipCenter].Position); Skeleton rFrameShift = ShiftFrame(rFrame, rFrame.Joints[JointType.HipCenter].Position); Matrix3x3 rotMatrix = new Matrix3x3(); double bestDist = -1; float bestR = 0; for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100)) { rotMatrix = Matrix3x3.CreateRotationX(r); double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix)); if (dist < bestDist || bestDist < 0) { bestR = r; bestDist = dist; } } float alpha = bestR; rFrameShift = ApplyTransformToFrame(rFrameShift, Matrix3x3.CreateRotationX(bestR)); bestDist = -1; bestR = 0; for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100)) { rotMatrix = Matrix3x3.CreateRotationY(r); double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix)); if (dist < bestDist || bestDist < 0) { bestR = r; bestDist = dist; } } float beta = bestR; rFrameShift = ApplyTransformToFrame(rFrameShift, Matrix3x3.CreateRotationY(bestR)); bestDist = -1; bestR = 0; for (float r = (float)-Math.PI / 2; r < Math.PI / 2; r = r + (float)(Math.PI / 100)) { rotMatrix = Matrix3x3.CreateRotationZ(r); double dist = GetDistBetweenFrames(mFrameShift, ApplyTransformToFrame(rFrameShift, rotMatrix)); if (dist < bestDist || bestDist < 0) { bestR = r; bestDist = dist; } } float gamma = bestR; return Matrix3x3.CreateFromYawPitchRoll(beta, alpha, gamma); }
public void AddMatricesTest( ) { Matrix3x3 expectedResult = new Matrix3x3( ); expectedResult.V00 = 3; expectedResult.V01 = 3; expectedResult.V02 = 6; expectedResult.V10 = 4; expectedResult.V11 = 5; expectedResult.V12 = 3; expectedResult.V20 = 4; expectedResult.V21 = 5; expectedResult.V22 = 3; Matrix3x3 result = a1 + a2; Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) ); }
public void ToArrayTest( ) { Matrix3x3 matrix = new Matrix3x3( ); matrix.V00 = 1; matrix.V01 = 2; matrix.V02 = 3; matrix.V10 = 4; matrix.V11 = 5; matrix.V12 = 6; matrix.V20 = 7; matrix.V21 = 8; matrix.V22 = 9; float[] array = matrix.ToArray( ); for ( int i = 0; i < 9; i++ ) { Assert.AreEqual<float>( array[i], (float) ( i + 1 ) ); } }
public void InverseTest( float v00, float v01, float v02, float v10, float v11, float v12, float v20, float v21, float v22 ) { Matrix3x3 matrix = new Matrix3x3( ); matrix.V00 = v00; matrix.V01 = v01; matrix.V02 = v02; matrix.V10 = v10; matrix.V11 = v11; matrix.V12 = v12; matrix.V20 = v20; matrix.V21 = v21; matrix.V22 = v22; Matrix3x3 inverse = matrix.Inverse( ); Matrix3x3 identity = matrix * inverse; Assert.AreEqual<bool>( true, ApproximateEquals( identity, Matrix3x3.Identity ) ); }
public void SubtractMatricesTest( ) { Matrix3x3 expectedResult = new Matrix3x3( ); expectedResult.V00 = -1; expectedResult.V01 = 1; expectedResult.V02 = 0; expectedResult.V10 = 2; expectedResult.V11 = -1; expectedResult.V12 = -1; expectedResult.V20 = -2; expectedResult.V21 = 1; expectedResult.V22 = 1; Matrix3x3 result = a1 - a2; Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) ); }
public static Skeleton TransRotateTrans(Skeleton frame, Matrix3x3 rot) { Skeleton frameShift = ShiftFrame(frame, frame.Joints[JointType.HipCenter].Position); Skeleton frameRot = ApplyTransformToFrame(frameShift, rot); return ShiftFrame(frameRot, NegativePosition(frame.Joints[JointType.HipCenter].Position)); }
/// <summary> /// Subtracts corresponding components of two matrices. /// </summary> /// /// <param name="matrix1">The matrix to subtract from.</param> /// <param name="matrix2">The matrix to subtract from the first matrix.</param> /// /// <returns>Returns a matrix which components are equal to difference of corresponding /// components of the two specified matrices.</returns> /// public static Matrix3x3 Subtract(Matrix3x3 matrix1, Matrix3x3 matrix2) { return(matrix1 - matrix2); }
// Calculate average error between real angles of the specified quadrilateral and angles of the // quadrilateral which is the projection of currently estimated pose private float GetError( Point[] imagePoints, Matrix3x3 rotation, Vector3 translation ) { Vector3 v1 = rotation * modelPoints[0] + translation; v1.X = v1.X * focalLength / v1.Z; v1.Y = v1.Y * focalLength / v1.Z; Vector3 v2 = rotation * modelPoints[1] + translation; v2.X = v2.X * focalLength / v2.Z; v2.Y = v2.Y * focalLength / v2.Z; Vector3 v3 = rotation * modelPoints[2] + translation; v3.X = v3.X * focalLength / v3.Z; v3.Y = v3.Y * focalLength / v3.Z; Vector3 v4 = rotation * modelPoints[3] + translation; v4.X = v4.X * focalLength / v4.Z; v4.Y = v4.Y * focalLength / v4.Z; Point[] modeledPoints = new Point[4] { new Point( v1.X, v1.Y ), new Point( v2.X, v2.Y ), new Point( v3.X, v3.Y ), new Point( v4.X, v4.Y ), }; float ia1 = GeometryTools.GetAngleBetweenVectors( imagePoints[0], imagePoints[1], imagePoints[3] ); float ia2 = GeometryTools.GetAngleBetweenVectors( imagePoints[1], imagePoints[2], imagePoints[0] ); float ia3 = GeometryTools.GetAngleBetweenVectors( imagePoints[2], imagePoints[3], imagePoints[1] ); float ia4 = GeometryTools.GetAngleBetweenVectors( imagePoints[3], imagePoints[0], imagePoints[2] ); float ma1 = GeometryTools.GetAngleBetweenVectors( modeledPoints[0], modeledPoints[1], modeledPoints[3] ); float ma2 = GeometryTools.GetAngleBetweenVectors( modeledPoints[1], modeledPoints[2], modeledPoints[0] ); float ma3 = GeometryTools.GetAngleBetweenVectors( modeledPoints[2], modeledPoints[3], modeledPoints[1] ); float ma4 = GeometryTools.GetAngleBetweenVectors( modeledPoints[3], modeledPoints[0], modeledPoints[2] ); return ( System.Math.Abs( ia1 - ma1 ) + System.Math.Abs( ia2 - ma2 ) + System.Math.Abs( ia3 - ma3 ) + System.Math.Abs( ia4 - ma4 ) ) / 4; }
// Select alternate pose estimation private void alternatePoseButton_Click( object sender, EventArgs e ) { rotationMatrix = alternateRotationMatrix; translationVector = alternateTranslationVector; UpdateEstimationInformation( ); pictureBox.Invalidate( ); }
/// <summary> /// Tests whether the matrix equals to the specified one. /// </summary> /// /// <param name="matrix">The matrix to test equality with.</param> /// /// <returns>Returns <see langword="true"/> if the two matrices are equal or <see langword="false"/> otherwise.</returns> /// public bool Equals( Matrix3x3 matrix ) { return ( this == matrix ); }
// Iterate POS algorithm starting from the specified rotation and translation and fine tune it private float Iterate( Point[] points, ref Matrix3x3 rotation, ref Vector3 translation ) { float prevError = float.MaxValue; float error = 0; // run maximum 100 iterations (seems to be overkill, since typicaly it requires around 1-2 iterations) for ( int count = 0; count < 100; count++ ) { Matrix3x3 rotation1, rotation2; Vector3 translation1, translation2; // calculates new epsilon values Vector3 eps = ( modelVectors * rotation.GetRow( 2 ) ) / translation.Z + 1; // and new pose POS( points, eps, out rotation1, out rotation2, out translation1, out translation2 ); // calculate error for both new poses float error1 = GetError( points, rotation1, translation1 ); float error2 = GetError( points, rotation2, translation2 ); // select the pose which gives smaller error if ( error1 < error2 ) { rotation = rotation1; translation = translation1; error = error1; } else { rotation = rotation2; translation = translation2; error = error2; } // stop if error is small enough or started to grow if ( ( error <= ErrorLimit ) || ( error > prevError ) ) break; prevError = error; } return error; }
/// <summary> /// Adds specified value to all components of the specified matrix. /// </summary> /// /// <param name="matrix">Matrix to add value to.</param> /// <param name="value">Value to add to all components of the specified matrix.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix increased by the specified value.</returns> /// public static Matrix3x3 operator +( Matrix3x3 matrix, float value ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix.V00 + value; m.V01 = matrix.V01 + value; m.V02 = matrix.V02 + value; m.V10 = matrix.V10 + value; m.V11 = matrix.V11 + value; m.V12 = matrix.V12 + value; m.V20 = matrix.V20 + value; m.V21 = matrix.V21 + value; m.V22 = matrix.V22 + value; return m; }
/// <summary> /// Adds specified value to all components of the specified matrix. /// </summary> /// /// <param name="matrix">Matrix to add value to.</param> /// <param name="value">Value to add to all components of the specified matrix.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix increased by the specified value.</returns> /// public static Matrix3x3 Add( Matrix3x3 matrix, float value ) { return matrix + value; }
/// <summary> /// Multiplies matrix by the specified factor. /// </summary> /// /// <param name="matrix">Matrix to multiply.</param> /// <param name="factor">Factor to multiple the specified matrix by.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix multiples by the specified factor.</returns> /// public static Matrix3x3 Multiply( Matrix3x3 matrix, float factor ) { return matrix * factor; }
/// <summary> /// Multiplies matrix by the specified factor. /// </summary> /// /// <param name="matrix">Matrix to multiply.</param> /// <param name="factor">Factor to multiple the specified matrix by.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix multiples by the specified factor.</returns> /// public static Matrix3x3 operator *( Matrix3x3 matrix, float factor ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = matrix.V00 * factor; m.V01 = matrix.V01 * factor; m.V02 = matrix.V02 * factor; m.V10 = matrix.V10 * factor; m.V11 = matrix.V11 * factor; m.V12 = matrix.V12 * factor; m.V20 = matrix.V20 * factor; m.V21 = matrix.V21 * factor; m.V22 = matrix.V22 * factor; return m; }
/// <summary> /// Tests whether the matrix equals to the specified one. /// </summary> /// /// <param name="matrix">The matrix to test equality with.</param> /// /// <returns>Returns <see langword="true"/> if the two matrices are equal or <see langword="false"/> otherwise.</returns> /// public bool Equals(Matrix3x3 matrix) { return(this == matrix); }
private void CompareMatrixWithArray( Matrix3x3 matrix, float[] array ) { float[] matrixArray = matrix.ToArray( ); for ( int i = 0; i < 9; i++ ) { Assert.AreEqual<float>( matrixArray[i], array[i] ); } }
/// <summary> /// Transpose the matrix, A<sup>T</sup>. /// </summary> /// /// <returns>Return a matrix which equals to transposition of this matrix.</returns> /// public Matrix3x3 Transpose( ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = V00; m.V01 = V10; m.V02 = V20; m.V10 = V01; m.V11 = V11; m.V12 = V21; m.V20 = V02; m.V21 = V12; m.V22 = V22; return m; }
/// <summary> /// Adds specified value to all components of the specified matrix. /// </summary> /// /// <param name="matrix">Matrix to add value to.</param> /// <param name="value">Value to add to all components of the specified matrix.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix increased by the specified value.</returns> /// public static Matrix3x3 Add(Matrix3x3 matrix, float value) { return(matrix + value); }
/// <summary> /// Multiply transposition of this matrix by itself, A<sup>T</sup>*A. /// </summary> /// /// <returns>Returns a matrix which is the result of multiplying this matrix's transposition by itself.</returns> /// public Matrix3x3 MultiplyTransposeBySelf( ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = V00 * V00 + V10 * V10 + V20 * V20; m.V10 = m.V01 = V00 * V01 + V10 * V11 + V20 * V21; m.V20 = m.V02 = V00 * V02 + V10 * V12 + V20 * V22; m.V11 = V01 * V01 + V11 * V11 + V21 * V21; m.V21 = m.V12 = V01 * V02 + V11 * V12 + V21 * V22; m.V22 = V02 * V02 + V12 * V12 + V22 * V22; return m; }
// Perform single iteration of POS (pos estimations) algorithm to find possible rotations and translation vectors private void POS( Point[] imagePoints, Vector3 eps, out Matrix3x3 rotation1, out Matrix3x3 rotation2, out Vector3 translation1, out Vector3 translation2 ) { // create vectors keeping all X and Y coordinates for the 1st, 2nd and 3rd points Vector3 XI = new Vector3( imagePoints[1].X, imagePoints[2].X, imagePoints[3].X ); Vector3 YI = new Vector3( imagePoints[1].Y, imagePoints[2].Y, imagePoints[3].Y ); // calculate scale orthographic projection (SOP) Vector3 imageXs = XI * eps - imagePoints[0].X; Vector3 imageYs = YI * eps - imagePoints[0].Y; // calculate I0 and J0 vectors Vector3 I0Vector = modelPseudoInverse * imageXs; Vector3 J0Vector = modelPseudoInverse * imageYs; Vector3 iVector = new Vector3( ); Vector3 jVector = new Vector3( ); Vector3 kVector = new Vector3( ); // find roots of complex number C^2 float j2i2dif = J0Vector.Square - I0Vector.Square; float ij = Vector3.Dot( I0Vector, J0Vector ); float r = 0, theta = 0; if ( j2i2dif == 0 ) { theta = (float) ( ( -System.Math.PI / 2 ) * System.Math.Sign( ij ) ); r = (float) System.Math.Sqrt( System.Math.Abs( 2 * ij ) ); } else { r = (float) System.Math.Sqrt( System.Math.Sqrt( j2i2dif * j2i2dif + 4 * ij * ij ) ); theta = (float) System.Math.Atan( -2 * ij / j2i2dif ); if ( j2i2dif < 0 ) theta += (float) System.Math.PI; theta /= 2; } float lambda = (float) ( r * System.Math.Cos( theta ) ); float mu = (float) ( r * System.Math.Sin( theta ) ); // first possible rotation iVector = I0Vector + ( modelNormal * lambda ); jVector = J0Vector + ( modelNormal * mu ); float iNorm = iVector.Normalize( ); float jNorm = jVector.Normalize( ); kVector = Vector3.Cross( iVector, jVector ); rotation1 = Matrix3x3.CreateFromRows( iVector, jVector, kVector ); // calculate translation vector float scale = ( iNorm + jNorm ) / 2; Vector3 temp = rotation1 * modelPoints[0]; translation1 = new Vector3( imagePoints[0].X / scale - temp.X, imagePoints[0].Y / scale - temp.Y, focalLength / scale ); // second possible rotation iVector = I0Vector - ( modelNormal * lambda ); jVector = J0Vector - ( modelNormal * mu ); iNorm = iVector.Normalize( ); jNorm = jVector.Normalize( ); kVector = Vector3.Cross( iVector, jVector ); rotation2 = Matrix3x3.CreateFromRows( iVector, jVector, kVector ); scale = ( iNorm + jNorm ) / 2; temp = rotation2 * modelPoints[0]; translation2 = new Vector3( imagePoints[0].X / scale - temp.X, imagePoints[0].Y / scale - temp.Y, focalLength / scale ); }
/// <summary> /// Calculate adjugate of the matrix, adj(A). /// </summary> /// /// <returns>Returns adjugate of the matrix.</returns> /// public Matrix3x3 Adjugate( ) { Matrix3x3 m = new Matrix3x3( ); m.V00 = V11 * V22 - V12 * V21; m.V01 = -( V01 * V22 - V02 * V21 ); m.V02 = V01 * V12 - V02 * V11; m.V10 = -( V10 * V22 - V12 * V20 ); m.V11 = V00 * V22 - V02 * V20; m.V12 = -( V00 * V12 - V02 * V10 ); m.V20 = V10 * V21 - V11 * V20; m.V21 = -( V00 * V21 - V01 * V20 ); m.V22 = V00 * V11 - V01 * V10; return m; }
/// <summary> /// Creates 4x4 tranformation matrix from 3x3 rotation matrix. /// </summary> /// /// <param name="rotationMatrix">Source 3x3 rotation matrix.</param> /// /// <returns>Returns 4x4 rotation matrix.</returns> /// /// <remarks><para>The source 3x3 rotation matrix is copied into the top left corner of the result 4x4 matrix, /// i.e. it represents 0th, 1st and 2nd row/column. The <see cref="V33"/> element is set to 1 and the rest /// elements of 3rd row and 3rd column are set to zeros.</para></remarks> /// public static Matrix4x4 CreateFromRotation(Matrix3x3 rotationMatrix) { Matrix4x4 m = Matrix4x4.Identity; m.V00 = rotationMatrix.V00; m.V01 = rotationMatrix.V01; m.V02 = rotationMatrix.V02; m.V10 = rotationMatrix.V10; m.V11 = rotationMatrix.V11; m.V12 = rotationMatrix.V12; m.V20 = rotationMatrix.V20; m.V21 = rotationMatrix.V21; m.V22 = rotationMatrix.V22; return m; }
/// <summary> /// Calculate Singular Value Decomposition (SVD) of the matrix, such as A=U*E*V<sup>T</sup>. /// </summary> /// /// <param name="u">Output parameter which gets 3x3 U matrix.</param> /// <param name="e">Output parameter which gets diagonal elements of the E matrix.</param> /// <param name="v">Output parameter which gets 3x3 V matrix.</param> /// /// <remarks><para>Having components U, E and V the source matrix can be reproduced using below code: /// <code> /// Matrix3x3 source = u * Matrix3x3.Diagonal( e ) * v.Transpose( ); /// </code> /// </para></remarks> /// public void SVD( out Matrix3x3 u, out Vector3 e, out Matrix3x3 v ) { double[,] uArray = new double[3, 3] { { V00, V01, V02 }, { V10, V11, V12 }, { V20, V21, V22 } }; double[,] vArray; double[] eArray; svd.svdcmp( uArray, out eArray, out vArray ); // build U matrix u = new Matrix3x3( ); u.V00 = (float) uArray[0, 0]; u.V01 = (float) uArray[0, 1]; u.V02 = (float) uArray[0, 2]; u.V10 = (float) uArray[1, 0]; u.V11 = (float) uArray[1, 1]; u.V12 = (float) uArray[1, 2]; u.V20 = (float) uArray[2, 0]; u.V21 = (float) uArray[2, 1]; u.V22 = (float) uArray[2, 2]; // build V matrix v = new Matrix3x3( ); v.V00 = (float) vArray[0, 0]; v.V01 = (float) vArray[0, 1]; v.V02 = (float) vArray[0, 2]; v.V10 = (float) vArray[1, 0]; v.V11 = (float) vArray[1, 1]; v.V12 = (float) vArray[1, 2]; v.V20 = (float) vArray[2, 0]; v.V21 = (float) vArray[2, 1]; v.V22 = (float) vArray[2, 2]; // build E Vector3 e = new Vector3( ); e.X = (float) eArray[0]; e.Y = (float) eArray[1]; e.Z = (float) eArray[2]; }
public static SkeletonPoint MultiplyPositionMatrix(Matrix3x3 mat, SkeletonPoint p) { SkeletonPoint pos = new SkeletonPoint(); pos.X = p.X * mat.V00 + p.Y * mat.V01 + p.Z * mat.V02; pos.Y = p.X * mat.V10 + p.Y * mat.V11 + p.Z * mat.V12; pos.Z = p.X * mat.V20 + p.Y * mat.V21 + p.Z * mat.V22; return pos; }
/// <summary> /// Initializes a new instance of the <see cref="Posit"/> class. /// </summary> /// /// <param name="model">Array of vectors containing coordinates of four real model's point (points /// must not be on the same plane).</param> /// <param name="focalLength">Effective focal length of the camera used to capture the model.</param> /// /// <exception cref="ArgumentException">The model must have 4 points.</exception> /// public Posit( Vector3[] model, float focalLength ) { if ( model.Length != 4 ) { throw new ArgumentException( "The model must have 4 points." ); } this.focalLength = focalLength; modelPoints = (Vector3[]) model.Clone( ); // compute model vectors modelVectors = Matrix3x3.CreateFromRows( model[1] - model[0], model[2] - model[0], model[3] - model[0] ); // compute pseudo inverse matrix modelPseudoInverse = modelVectors.PseudoInverse( ); }
/// <summary> /// Multiplies specified matrix by the specified vector. /// </summary> /// /// <param name="matrix">Matrix to multiply by vector.</param> /// <param name="vector">Vector to multiply matrix by.</param> /// /// <returns>Returns new vector which is the result of multiplication of the specified matrix /// by the specified vector.</returns> /// public static Vector3 Multiply(Matrix3x3 matrix, Vector3 vector) { return(matrix * vector); }
/// <summary> /// Estimate pose of a model from it's projected 2D coordinates. /// </summary> /// /// <param name="points">4 2D points of the <see cref="Model">model's</see> projection.</param> /// <param name="rotation">Gets object's rotation.</param> /// <param name="translation">Gets object's translation.</param> /// /// <exception cref="ArgumentException">4 points must be be given for pose estimation.</exception> /// public void EstimatePose( Point[] points, out Matrix3x3 rotation, out Vector3 translation ) { if ( points.Length != 4 ) { throw new ArgumentException( "4 points must be be given for pose estimation." ); } float Z0 = 0, scale = 1; Vector3 X0 = new Vector3( points[0].X ); Vector3 Y0 = new Vector3( points[0].Y ); Vector3 XI = new Vector3( points[1].X, points[2].X, points[3].X ); Vector3 YI = new Vector3( points[1].Y, points[2].Y, points[3].Y ); int count = 0; Vector3 iVector = new Vector3( ); Vector3 jVector = new Vector3( ); Vector3 kVector = new Vector3( ); Vector3 imageXs = new Vector3( ); Vector3 imageYs = new Vector3( ); Vector3 eps = new Vector3( 1 ); for ( ; count < 100; count++ ) { // calculate new scale orthographic projection (SOP) imageXs = XI * eps - X0; imageYs = YI * eps - Y0; // calculate I and J vectors iVector = modelPseudoInverse * imageXs; jVector = modelPseudoInverse * imageYs; // convert them to unit vectors i and j float iNorm = iVector.Normalize( ); float jNorm = jVector.Normalize( ); // scale of projection scale = ( iNorm + jNorm ) / 2; // calculate n vector k kVector = Vector3.Cross( iVector, jVector ); // z-coordinate Z0 of the translation vector Z0 = focalLength / scale; // calculate new epsilon values Vector3 oldEps = eps; eps = ( modelVectors * kVector ) / Z0 + 1; // check if it is time to stop if ( ( eps - oldEps ).Abs( ).Max < stop_epsilon ) break; } // create rotation matrix rotation = Matrix3x3.CreateFromRows( iVector, jVector, kVector ); // create translation vector Vector3 temp = rotation * modelPoints[0]; translation = new Vector3( points[0].X / scale - temp.X, points[0].Y / scale - temp.Y, focalLength / scale ); }
public void DeterminantTest( float expectedDeterminant, float v00, float v01, float v02, float v10, float v11, float v12, float v20, float v21, float v22 ) { Matrix3x3 matrix = new Matrix3x3( ); matrix.V00 = v00; matrix.V01 = v01; matrix.V02 = v02; matrix.V10 = v10; matrix.V11 = v11; matrix.V12 = v12; matrix.V20 = v20; matrix.V21 = v21; matrix.V22 = v22; Assert.AreEqual<float>( expectedDeterminant, matrix.Determinant ); }
/// <summary> /// Adds corresponding components of two matrices. /// </summary> /// /// <param name="matrix1">The matrix to add to.</param> /// <param name="matrix2">The matrix to add to the first matrix.</param> /// /// <returns>Returns a matrix which components are equal to sum of corresponding /// components of the two specified matrices.</returns> /// public static Matrix3x3 Add(Matrix3x3 matrix1, Matrix3x3 matrix2) { return(matrix1 + matrix2); }
public void MultiplyMatricesTest( ) { Matrix3x3 expectedResult = new Matrix3x3( ); expectedResult.V00 = 13; expectedResult.V01 = 13; expectedResult.V02 = 10; expectedResult.V10 = 11; expectedResult.V11 = 11; expectedResult.V12 = 14; expectedResult.V20 = 11; expectedResult.V21 = 14; expectedResult.V22 = 11; Matrix3x3 result = a1 * a2; Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) ); }
/// <summary> /// Initializes a new instance of the <see cref="Posit"/> class. /// </summary> /// /// <param name="model">Array of vectors containing coordinates of four real model's point.</param> /// <param name="focalLength">Effective focal length of the camera used to capture the model.</param> /// /// <exception cref="ArgumentException">The model must have 4 points.</exception> /// public CoplanarPosit( Vector3[] model, float focalLength ) { if ( model.Length != 4 ) { throw new ArgumentException( "The model must have 4 points." ); } this.focalLength = focalLength; modelPoints = (Vector3[]) model.Clone( ); // compute model vectors modelVectors = Matrix3x3.CreateFromRows( model[1] - model[0], model[2] - model[0], model[3] - model[0] ); // compute pseudo inverse of the model matrix Matrix3x3 u, v; Vector3 e; modelVectors.SVD( out u, out e, out v ); modelPseudoInverse = v * Matrix3x3.CreateDiagonal( e.Inverse( ) ) * u.Transpose( ); // computer unit vector normal to the model modelNormal = v.GetColumn( e.MinIndex ); }
private bool ApproximateEquals( Matrix3x3 matrix1, Matrix3x3 matrix2 ) { // TODO: better algorithm should be put into the framework actually return ( ( System.Math.Abs( matrix1.V00 - matrix2.V00 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V01 - matrix2.V01 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V02 - matrix2.V02 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V10 - matrix2.V10 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V11 - matrix2.V11 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V12 - matrix2.V12 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V20 - matrix2.V20 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V21 - matrix2.V21 ) <= Epsilon ) && ( System.Math.Abs( matrix1.V22 - matrix2.V22 ) <= Epsilon ) ); }
/// <summary> /// Estimate pose of a model from it's projected 2D coordinates. /// </summary> /// /// <param name="points">4 2D points of the <see cref="Model">model's</see> projection.</param> /// <param name="rotation">Gets best estimation of object's rotation.</param> /// <param name="translation">Gets best estimation of object's translation.</param> /// /// <exception cref="ArgumentException">4 points must be be given for pose estimation.</exception> /// /// <remarks><para>Because of the Coplanar POSIT algorithm's nature, it provides two pose estimations, /// which are valid from the algorithm's math point of view. For each pose an error is calculated, /// which specifies how good estimation fits to the specified real 2D coordinated. The method /// provides the best estimation through its output parameters <paramref name="rotation"/> and /// <paramref name="translation"/>. This may be enough for many of the pose estimation application. /// For those, who require checking the alternate pose estimation, it can be obtained using /// <see cref="AlternateEstimatedRotation"/> and <see cref="AlternateEstimatedTranslation"/> properties. /// The calculated error is provided for both estimations through <see cref="BestEstimationError"/> and /// <see cref="AlternateEstimationError"/> properties.</para> /// </remarks> /// public void EstimatePose( Point[] points, out Matrix3x3 rotation, out Vector3 translation ) { if ( points.Length != 4 ) { throw new ArgumentException( "4 points must be be given for pose estimation." ); } Matrix3x3 rotation1, rotation2; Vector3 translation1, translation2; // find initial rotation POS( points, new Vector3( 1 ), out rotation1, out rotation2, out translation1, out translation2 ); // iterate further and fine tune the solution float error1 = Iterate( points, ref rotation1, ref translation1 ); float error2 = Iterate( points, ref rotation2, ref translation2 ); // take the best found pose if ( error1 < error2 ) { bestRotation = rotation1; bestTranslation = translation1; bestPoseError = error1; alternateRotation = rotation2; alternateTranslation = translation2; alternatePoseError = error2; } else { bestRotation = rotation2; bestTranslation = translation2; bestPoseError = error2; alternateRotation = rotation1; alternateTranslation = translation1; alternatePoseError = error1; } rotation = bestRotation; translation = bestTranslation; }
/// <summary> /// Multiplies two specified matrices. /// </summary> /// /// <param name="matrix1">Matrix to multiply.</param> /// <param name="matrix2">Matrix to multiply by.</param> /// /// <returns>Return new matrix, which the result of multiplication of the two specified matrices.</returns> /// public static Matrix3x3 Multiply(Matrix3x3 matrix1, Matrix3x3 matrix2) { return(matrix1 * matrix2); }
/// <summary> /// Multiplies matrix by the specified factor. /// </summary> /// /// <param name="matrix">Matrix to multiply.</param> /// <param name="factor">Factor to multiple the specified matrix by.</param> /// /// <returns>Returns new matrix with all components equal to corresponding components of the /// specified matrix multiples by the specified factor.</returns> /// public static Matrix3x3 Multiply(Matrix3x3 matrix, float factor) { return(matrix * factor); }