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