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