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