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