/// <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 CreateFromRowsTest()
{
Vector3 row0 = new Vector3(1, 2, 3);
Vector3 row1 = new Vector3(4, 5, 6);
Vector3 row2 = new Vector3(7, 8, 9);
Matrix3x3 matrix = Matrix3x3.CreateFromRows(row0, row1, row2);
float[] array = matrix.ToArray();
for (int i = 0; i < 9; i++)
{
Assert.AreEqual(array[i], (float)(i + 1));
}
Assert.AreEqual(row0, matrix.GetRow(0));
Assert.AreEqual(row1, matrix.GetRow(1));
Assert.AreEqual(row2, matrix.GetRow(2));
Assert.ThrowsException <ArgumentException>(() =>
{
matrix.GetRow(-1);
}
);
Assert.ThrowsException <ArgumentException>(() =>
{
matrix.GetRow(3);
}
);
}
private void POS(Point[] imagePoints, Vector3 eps, out Matrix3x3 rotation1, out Matrix3x3 rotation2, out Vector3 translation1, out Vector3 translation2)
{
Vector3 vector = new Vector3(imagePoints[1].X, imagePoints[2].X, imagePoints[3].X);
Vector3 vector2 = new Vector3(imagePoints[1].Y, imagePoints[2].Y, imagePoints[3].Y);
Vector3 vector3 = vector * eps - imagePoints[0].X;
Vector3 vector4 = vector2 * eps - imagePoints[0].Y;
Vector3 vector5 = modelPseudoInverse * vector3;
Vector3 vector6 = modelPseudoInverse * vector4;
Vector3 vector7 = default(Vector3);
Vector3 vector8 = default(Vector3);
Vector3 vector9 = default(Vector3);
float num = vector6.Square - vector5.Square;
float num2 = Vector3.Dot(vector5, vector6);
float num3 = 0f;
float num4 = 0f;
if (num == 0f)
{
num4 = (float)(-System.Math.PI / 2.0 * (double)System.Math.Sign(num2));
num3 = (float)System.Math.Sqrt(System.Math.Abs(2f * num2));
}
else
{
num3 = (float)System.Math.Sqrt(System.Math.Sqrt(num * num + 4f * num2 * num2));
num4 = (float)System.Math.Atan(-2f * num2 / num);
if (num < 0f)
{
num4 += (float)System.Math.PI;
}
num4 /= 2f;
}
float factor = (float)((double)num3 * System.Math.Cos(num4));
float factor2 = (float)((double)num3 * System.Math.Sin(num4));
vector7 = vector5 + modelNormal * factor;
vector8 = vector6 + modelNormal * factor2;
float num5 = vector7.Normalize();
float num6 = vector8.Normalize();
vector9 = Vector3.Cross(vector7, vector8);
rotation1 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
float num7 = (num5 + num6) / 2f;
Vector3 vector10 = rotation1 * modelPoints[0];
translation1 = new Vector3(imagePoints[0].X / num7 - vector10.X, imagePoints[0].Y / num7 - vector10.Y, focalLength / num7);
vector7 = vector5 - modelNormal * factor;
vector8 = vector6 - modelNormal * factor2;
num5 = vector7.Normalize();
num6 = vector8.Normalize();
vector9 = Vector3.Cross(vector7, vector8);
rotation2 = Matrix3x3.CreateFromRows(vector7, vector8, vector9);
num7 = (num5 + num6) / 2f;
vector10 = rotation2 * modelPoints[0];
translation2 = new Vector3(imagePoints[0].X / num7 - vector10.X, imagePoints[0].Y / num7 - vector10.Y, focalLength / num7);
}
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();
modelVectors = Matrix3x3.CreateFromRows(model[1] - model[0], model[2] - model[0], model[3] - model[0]);
modelPseudoInverse = modelVectors.PseudoInverse();
}
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 num = 0f;
float num2 = 1f;
Vector3 vector = new Vector3(points[0].X);
Vector3 vector2 = new Vector3(points[0].Y);
Vector3 vector3 = new Vector3(points[1].X, points[2].X, points[3].X);
Vector3 vector4 = new Vector3(points[1].Y, points[2].Y, points[3].Y);
int i = 0;
Vector3 vector5 = default(Vector3);
Vector3 vector6 = default(Vector3);
Vector3 vector7 = default(Vector3);
Vector3 vector8 = default(Vector3);
Vector3 vector9 = default(Vector3);
Vector3 vector10 = new Vector3(1f);
for (; i < 100; i++)
{
vector8 = vector3 * vector10 - vector;
vector9 = vector4 * vector10 - vector2;
vector5 = modelPseudoInverse * vector8;
vector6 = modelPseudoInverse * vector9;
float num3 = vector5.Normalize();
float num4 = vector6.Normalize();
num2 = (num3 + num4) / 2f;
vector7 = Vector3.Cross(vector5, vector6);
num = focalLength / num2;
Vector3 vector11 = vector10;
vector10 = modelVectors * vector7 / num + 1f;
if ((vector10 - vector11).Abs().Max < 0.0001f)
{
break;
}
}
rotation = Matrix3x3.CreateFromRows(vector5, vector6, vector7);
Vector3 vector12 = rotation * modelPoints[0];
translation = new Vector3(points[0].X / num2 - vector12.X, points[0].Y / num2 - vector12.Y, focalLength / num2);
}
// 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>
/// 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;
var X0 = new Vector3(points[0].X);
var Y0 = new Vector3(points[0].Y);
var XI = new Vector3(points[1].X, points[2].X, points[3].X);
var YI = new Vector3(points[1].Y, points[2].Y, points[3].Y);
var count = 0;
var iVector = new Vector3();
var jVector = new Vector3();
var kVector = new Vector3();
var imageXs = new Vector3();
var imageYs = new Vector3();
var 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
var iNorm = iVector.Normalize();
var 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
var 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
var temp = rotation * modelPoints[0];
translation = new Vector3(
points[0].X / scale - temp.X,
points[0].Y / scale - temp.Y,
focalLength / scale);
}
