/// <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);
}
public void CreateDiagonalTest(float v00, float v11, float v22)
{
Vector3 diagonal = new Vector3(v00, v11, v22);
Matrix3x3 matrix = Matrix3x3.CreateDiagonal(diagonal);
float[] expectedArray = new float[9] {
v00, 0, 0, 0, v11, 0, 0, 0, v22
};
CompareMatrixWithArray(matrix, expectedArray);
}
