protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
Cl3DModel.Cl3DModelPointIterator max = iter.CopyIterator();
float distanceMax = 0;
do
{
float distance = (float)Math.Sqrt(Math.Pow(iter.U, 2) + Math.Pow(iter.V, 2));
if (distanceMax < distance)
distanceMax = distance;
} while (iter.MoveToNext());
iter = p_Model.GetIterator();
do
{
iter.U = (iter.U / distanceMax) * m_RadiousOfConformalMap;
iter.V = (iter.V / distanceMax) * m_RadiousOfConformalMap;
} while (iter.MoveToNext());
Cl3DModel.Cl3DModelPointIterator LeftEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner);
Cl3DModel.Cl3DModelPointIterator RightEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner);
Cl3DModel.Cl3DModelPointIterator NoseTip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
Cl3DModel.Cl3DModelPointIterator tmp;
if (LeftEye.X > RightEye.X)
{
tmp = LeftEye.CopyIterator();
RightEye = LeftEye.CopyIterator();
LeftEye = tmp.CopyIterator();
}
double EyesDistance = Math.Sqrt(Math.Pow(LeftEye.U - RightEye.U,2) + Math.Pow(LeftEye.V - RightEye.V,2));
double NoseLeftEyeDistance = Math.Sqrt(Math.Pow(LeftEye.U - NoseTip.U, 2) + Math.Pow(LeftEye.V - NoseTip.V, 2));
double NoseRightEyeDistance = Math.Sqrt(Math.Pow(NoseTip.U - RightEye.U, 2) + Math.Pow(NoseTip.V - RightEye.V, 2));
double AverageNoseTipEyeDistance = (NoseLeftEyeDistance + NoseRightEyeDistance) / 2;
Matrix GenModel = new Matrix(2, 3);
GenModel[0, 0] = 0;
GenModel[1, 0] = 0; // nose
GenModel[0, 1] = (EyesDistance / 2);
GenModel[1, 1] = AverageNoseTipEyeDistance; // left eye
GenModel[0, 2] = -(EyesDistance / 2);
GenModel[1, 2] = AverageNoseTipEyeDistance; // right eye
Matrix Model = new Matrix(2, 3);
Model[0, 0] = NoseTip.U;
Model[1, 0] = NoseTip.V; // nose
Model[0, 1] = LeftEye.U;
Model[1, 1] = LeftEye.V; // left eye
Model[0, 2] = RightEye.U;
Model[1, 2] = RightEye.V; // right eye
Matrix rotationMatrix = null;
Matrix translationMatrix = null;
ClTools.CalculateRotationAndTranslation(GenModel, Model, out rotationMatrix, out translationMatrix);
Iridium.Numerics.LinearAlgebra.Matrix q = new Iridium.Numerics.LinearAlgebra.Matrix(2, 1);
iter = p_Model.GetIterator();
do
{
q[0, 0] = iter.U;
q[1, 0] = iter.V;
Iridium.Numerics.LinearAlgebra.Matrix NewQ = rotationMatrix * q +translationMatrix;
iter.U = (float)NewQ[0, 0];
iter.V = (float)NewQ[1, 0];
} while (iter.MoveToNext());
float U = NoseTip.U;
float V = NoseTip.V;
iter = p_Model.GetIterator();
do
{
iter.U -= U;
iter.V -= V;
} while (iter.MoveToNext());
if (centerConformalMaps)
{
float middleU = 0;
float middleV = 0;
iter = p_Model.GetIterator();
do
{
middleU += iter.U;
middleV += iter.V;
} while (iter.MoveToNext());
middleU /= p_Model.ModelPointsCount;
middleV /= p_Model.ModelPointsCount;
iter = p_Model.GetIterator();
do
{
iter.U -= middleU;
iter.V -= middleV;
} while (iter.MoveToNext());
}
}
public static bool CountSurfaceCoefficients(List<Cl3DModel.Cl3DModelPointIterator> p_pNeighborhood, ref double p_A, ref double p_B, ref double p_C, ref double p_D, ref double p_E, ref double p_F)
{
if (p_pNeighborhood.Count < 3)
return false;
double x = 0;
double y = 0;
double z = 0;
double xy = 0;
double xz = 0;
double yz = 0;
double xyz = 0;
double x2 = 0;
double y2 = 0;
double x2y = 0;
double x2z = 0;
double y2z = 0;
double xy2 = 0;
double x3 = 0;
double y3 = 0;
double x4 = 0;
double y4 = 0;
double x2y2 = 0;
double xy3 = 0;
double x3y = 0;
double N = 0;
for (int i = 0; i < p_pNeighborhood.Count; i++)
{
double iX = p_pNeighborhood[i].X;
double iY = p_pNeighborhood[i].Y;
double iZ = p_pNeighborhood[i].Z;
x += iX;
y += iY;
z += iZ;
xy += iX * iY;
xz += iX * iZ;
yz += iY * iZ;
xyz += iX * iY * iZ;
x2 += iX * iX;
y2 += iY * iY;
x2y += iX * iX * iY;
x2z += iX * iX * iZ;
y2z += iY * iY * iZ;
//xy2 += iX * iY * iZ * iZ; //gives strange but good results
xy2 += iX * iY * iY;
x3 += iX * iX * iX;
y3 += iY * iY * iY;
x4 += iX * iX * iX * iX;
y4 += iY * iY * iY * iY;
x2y2 += iX * iX * iY * iY;
xy3 += iX * iY * iY * iY;
x3y += iX * iX * iX * iY;
N++;
}
Iridium.Numerics.LinearAlgebra.Matrix MatrixLeft = new Iridium.Numerics.LinearAlgebra.Matrix(6, 6);
Iridium.Numerics.LinearAlgebra.Matrix MatrixRight = new Iridium.Numerics.LinearAlgebra.Matrix(6, 1);
MatrixLeft[0, 0] = N;
MatrixLeft[0, 1] = x;
MatrixLeft[0, 2] = y;
MatrixLeft[0, 3] = x2;
MatrixLeft[0, 4] = xy;
MatrixLeft[0, 5] = y2;
MatrixLeft[1, 0] = x;
MatrixLeft[1, 1] = x2;
MatrixLeft[1, 2] = xy;
MatrixLeft[1, 3] = x3;
MatrixLeft[1, 4] = x2y;
MatrixLeft[1, 5] = xy2;
MatrixLeft[2, 0] = y;
MatrixLeft[2, 1] = xy;
MatrixLeft[2, 2] = y2;
MatrixLeft[2, 3] = x2y;
MatrixLeft[2, 4] = xy2;
MatrixLeft[2, 5] = y3;
MatrixLeft[3, 0] = x2;
MatrixLeft[3, 1] = x3;
MatrixLeft[3, 2] = x2y;
MatrixLeft[3, 3] = x4;
MatrixLeft[3, 4] = x3y;
MatrixLeft[3, 5] = x2y2;
MatrixLeft[4, 0] = xy;
MatrixLeft[4, 1] = x2y;
MatrixLeft[4, 2] = xy2;
MatrixLeft[4, 3] = x3y;
MatrixLeft[4, 4] = x2y2;
MatrixLeft[4, 5] = xy3;
MatrixLeft[5, 0] = y2;
MatrixLeft[5, 1] = xy2;
MatrixLeft[5, 2] = y3;
MatrixLeft[5, 3] = x2y2;
MatrixLeft[5, 4] = xy3;
MatrixLeft[5, 5] = y4;
MatrixRight[0, 0] = z;
MatrixRight[1, 0] = xz;
MatrixRight[2, 0] = yz;
MatrixRight[3, 0] = x2z;
MatrixRight[4, 0] = xyz;
MatrixRight[5, 0] = y2z;
Iridium.Numerics.LinearAlgebra.Matrix MatrixOut = null;
try
{
MatrixLeft = MatrixLeft.Inverse();
MatrixOut = MatrixLeft * MatrixRight;
}
catch (Exception)
{
return false;
}
p_A = MatrixOut[0, 0];
p_B = MatrixOut[1, 0];
p_C = MatrixOut[2, 0];
p_D = MatrixOut[3, 0];
p_E = MatrixOut[4, 0];
p_F = MatrixOut[5, 0];
return true;
}
/// <summary>
/// each colomn is one vertex - CORRECTED
/// </summary>
/// <param name="PointsModel1"></param>
/// <param name="PointsModel2"></param>
/// <param name="p_mRotationMatrix"></param>
/// <param name="p_mTranslationMatrix"></param>
public static void CalculateRotationAndTranslation(Matrix PointsModel1, Matrix PointsModel2, out Iridium.Numerics.LinearAlgebra.Matrix p_mRotationMatrix, out Iridium.Numerics.LinearAlgebra.Matrix p_mTranslationMatrix)
{
if (PointsModel1.ColumnCount != PointsModel2.ColumnCount || PointsModel1.RowCount != PointsModel2.RowCount)
throw new Exception("Two Matrixes do not have the same count");
Matrix MeanValModel1 = new Matrix(PointsModel1.RowCount, 1);
Matrix MeanValModel2 = new Matrix(PointsModel1.RowCount, 1);
for (int j = 0; j < PointsModel1.RowCount; j++)
{
for (int i = 0; i < PointsModel1.ColumnCount; i++)
{
MeanValModel1[j, 0] += PointsModel1[j, i];
MeanValModel2[j, 0] += PointsModel2[j, i];
}
}
MeanValModel1 *= (1.0f / PointsModel1.ColumnCount);
MeanValModel2 *= (1.0f / PointsModel1.ColumnCount);
Matrix CenteredModel1 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);
Matrix CenteredModel2 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);
for (int i = 0; i < PointsModel1.RowCount; i++)
{
for (int j = 0; j < PointsModel1.ColumnCount; j++)
{
CenteredModel1[i, j] = PointsModel1[i, j] - MeanValModel1[i, 0];
CenteredModel2[i, j] = PointsModel2[i, j] - MeanValModel2[i, 0];
}
}
CenteredModel2.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Covariance = CenteredModel1 * CenteredModel2;
Iridium.Numerics.LinearAlgebra.SingularValueDecomposition SVD = new Iridium.Numerics.LinearAlgebra.SingularValueDecomposition(Covariance);
Iridium.Numerics.LinearAlgebra.Matrix U = SVD.LeftSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix V = SVD.RightSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix s = new Iridium.Numerics.LinearAlgebra.Matrix(PointsModel1.RowCount, 1.0);
if (Covariance.Rank() < 2)
throw new Exception("Cannot allign generic model (cov rank is less than 2)");
if (Covariance.Rank() == 2) // m-1 where m is dimension space (3D)
{
double detU = Math.Round(U.Determinant());
double detV = Math.Round(V.Determinant());
double detC = Covariance.Determinant();
if ((int)detU * (int)detV == 1)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = 1;
else if ((int)detU * (int)detV == -1)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
else
throw new Exception("Determinant of U and V are not in conditions");
}
else
{
if (Covariance.Determinant() < 0)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
}
V.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Rotation = U * s * V;
Iridium.Numerics.LinearAlgebra.Matrix Translation = MeanValModel1 - Rotation * MeanValModel2;
p_mRotationMatrix = Rotation;
p_mTranslationMatrix = Translation;
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
{
return;
}
List <Cl3DModel.Cl3DModelPointIterator> ListOfNosePoints = new List <Cl3DModel.Cl3DModelPointIterator>();
List <Cl3DModel.Cl3DModelPointIterator> ListOfEyesPoints = new List <Cl3DModel.Cl3DModelPointIterator>();
string gausString = "Gaussian_" + m_iNeighborhoodSize.ToString();
string meanString = "Mean_" + m_iNeighborhoodSize.ToString();
do
{
double H = 0;
double K = 0;
if (!iter.GetSpecificValue(gausString, out K))
{
continue;
}
if (!iter.GetSpecificValue(meanString, out H))
{
continue;
}
if (H < 0 && K > m_NoseThresholdK) // Nose
{
ListOfNosePoints.Add(iter.CopyIterator());
}
else if (H > 0 && K > m_EyesThresholdK) // Eyes
{
ListOfEyesPoints.Add(iter.CopyIterator());
}
}while (iter.MoveToNext());
List <List <Cl3DModel.Cl3DModelPointIterator> > NoseRegions = null;
List <List <Cl3DModel.Cl3DModelPointIterator> > EyesRegions = null;
// NOSE
DivideToTheRegions(ListOfNosePoints, out NoseRegions);
// EYES
DivideToTheRegions(ListOfEyesPoints, out EyesRegions);
// COLOR IT
/*for (int i = 0; i < NoseRegions.Count; i++)
* {
* foreach (Cl3DModel.Cl3DModelPointIterator point in NoseRegions[i])
* point.Color = ClTools.GetColorRGB((float)i / (NoseRegions.Count + EyesRegions.Count), 1);
* }
* for (int i = 0; i < EyesRegions.Count; i++)
* {
* foreach (Cl3DModel.Cl3DModelPointIterator point in EyesRegions[i])
* point.Color = ClTools.GetColorRGB((float)(i + NoseRegions.Count) / (EyesRegions.Count + NoseRegions.Count), 1);
* }
*/
// ----------------- NOSE
// remove smallest regions leave only 3 biggest one
List <Cl3DModel.Cl3DModelPointIterator> NoseTipsPoints = new List <Cl3DModel.Cl3DModelPointIterator>();
if (NoseRegions.Count > m_iMaxCountOfNoseRegions)
{
NoseRegions.Sort(new ClCompareCount());
for (int i = NoseRegions.Count - 1; i >= m_iMaxCountOfNoseRegions; i--)
{
NoseRegions.RemoveAt(i);
}
}
//----------- NOSE searching for one point in each region
for (int i = 0; i < NoseRegions.Count; i++)
{
double MaxKVal = 0;
Cl3DModel.Cl3DModelPointIterator MaxPoint = null;
foreach (Cl3DModel.Cl3DModelPointIterator point in NoseRegions[i])
{
double K;
if (!point.GetSpecificValue(gausString, out K))
{
continue;
}
if (MaxPoint == null)
{
MaxPoint = point;
MaxKVal = K;
continue;
}
if (MaxKVal < K)
{
MaxKVal = K;
MaxPoint = point;
}
}
NoseTipsPoints.Add(MaxPoint);
}
// --------------- EYES
// remove smallest regions leave only 5 biggest one
List <Cl3DModel.Cl3DModelPointIterator> EyesPoints = new List <Cl3DModel.Cl3DModelPointIterator>();
if (EyesRegions.Count > m_iMaxCountOfEyeRegions)
{
EyesRegions.Sort(new ClCompareCount());
for (int i = EyesRegions.Count - 1; i >= m_iMaxCountOfEyeRegions; i--)
{
EyesRegions.RemoveAt(i);
}
}
//--------------------------- EYES searching for one point in each region
for (int i = 0; i < EyesRegions.Count; i++)
{
double MaxKval = 0;
Cl3DModel.Cl3DModelPointIterator MinPoint = null;
foreach (Cl3DModel.Cl3DModelPointIterator point in EyesRegions[i])
{
double val;
if (!point.GetSpecificValue(gausString, out val))
{
continue;
}
if (MinPoint == null)
{
MinPoint = point;
MaxKval = val;
continue;
}
if (MaxKval < val)
{
MaxKval = val;
MinPoint = point;
}
}
EyesPoints.Add(MinPoint);
}
//------------- Looking for three face points ------------------------------
Iridium.Numerics.LinearAlgebra.Matrix Model = new Iridium.Numerics.LinearAlgebra.Matrix(3, 3);
Iridium.Numerics.LinearAlgebra.Matrix ModelGen = new Iridium.Numerics.LinearAlgebra.Matrix(3, 3);
// Generic model
Vector GenericNoseTip = new Vector(new double[] { 0, 0, 0 });
Vector GenericLeftEye = new Vector(new double[] { -21f, 40.5f, -41.5f });
Vector GenericRightEye = new Vector(new double[] { 21f, 40.5f, -41.5f });
Vector GenLeftCornerOfLeftEye = new Vector(new double[] { -44f, 40f, -44f });
Vector GenRightCornerOfRightEye = new Vector(new double[] { 44f, 40f, -44f });
Vector GenLeftCornerOfNose = new Vector(new double[] { -20f, -2f, -31f });
Vector GenRightCornerOfNose = new Vector(new double[] { 20f, -2f, -31f });
Vector GenLeftCornerOfLips = new Vector(new double[] { -28f, -32f, -34f });
Vector GenRightCornerOfLips = new Vector(new double[] { 28f, -32f, -34f });
Vector MeanGenericModel = new Vector(new double[] { (GenericNoseTip[0] + GenericLeftEye[0] + GenericRightEye[0]) / 3,
(GenericNoseTip[1] + GenericLeftEye[1] + GenericRightEye[1]) / 3,
(GenericNoseTip[2] + GenericLeftEye[2] + GenericRightEye[2]) / 3 });
// Centered generic model (generic model - mean)
ModelGen[0, 0] = (GenericNoseTip[0] - MeanGenericModel[0]);
ModelGen[1, 0] = (GenericNoseTip[1] - MeanGenericModel[1]);
ModelGen[2, 0] = (GenericNoseTip[2] - MeanGenericModel[2]);
ModelGen[0, 1] = (GenericLeftEye[0] - MeanGenericModel[0]);
ModelGen[1, 1] = (GenericLeftEye[1] - MeanGenericModel[1]);
ModelGen[2, 1] = (GenericLeftEye[2] - MeanGenericModel[2]);
ModelGen[0, 2] = (GenericRightEye[0] - MeanGenericModel[0]);
ModelGen[1, 2] = (GenericRightEye[1] - MeanGenericModel[1]);
ModelGen[2, 2] = (GenericRightEye[2] - MeanGenericModel[2]);
ModelGen.Transpose();
List <KeyValuePair <float, List <Cl3DModel.Cl3DModelPointIterator> > > deltaOfDistanceBetweenModelAndPoints = new List <KeyValuePair <float, List <Cl3DModel.Cl3DModelPointIterator> > >();
for (int iEye = 0; iEye < EyesPoints.Count - 1; iEye++)
{
for (int jEye = iEye + 1; jEye < EyesPoints.Count; jEye++)
{
for (int iNose = 0; iNose < NoseTipsPoints.Count; iNose++)
{
Cl3DModel.Cl3DModelPointIterator eyeLeft = EyesPoints[iEye];
Cl3DModel.Cl3DModelPointIterator eyeRight = EyesPoints[jEye];
Cl3DModel.Cl3DModelPointIterator noseTip = NoseTipsPoints[iNose];
if (eyeLeft.Y < noseTip.Y || eyeRight.Y < noseTip.Y)
{
continue;
}
for (int c = 0; c < 2; c++)
{
if (c == 1)
{
eyeLeft = EyesPoints[jEye]; // zmiana oczu nie wiemy ktore punkty to oczy prawdziwe sa
eyeRight = EyesPoints[iEye];
}
float MeanValX = (noseTip.X + eyeLeft.X + eyeRight.X) / 3.0f;
float MeanValY = (noseTip.Y + eyeLeft.Y + eyeRight.Y) / 3.0f;
float MeanValZ = (noseTip.Z + eyeLeft.Z + eyeRight.Z) / 3.0f;
Model[0, 0] = (noseTip.X - MeanValX);
Model[1, 0] = (noseTip.Y - MeanValY);
Model[2, 0] = (noseTip.Z - MeanValZ);
Model[0, 1] = (eyeLeft.X - MeanValX);
Model[1, 1] = (eyeLeft.Y - MeanValY);
Model[2, 1] = (eyeLeft.Z - MeanValZ);
Model[0, 2] = (eyeRight.X - MeanValX);
Model[1, 2] = (eyeRight.Y - MeanValY);
Model[2, 2] = (eyeRight.Z - MeanValZ);
//----------------- Finding best rotation and translation SVD algorithm
Iridium.Numerics.LinearAlgebra.Matrix Covariance = Model * ModelGen;
Iridium.Numerics.LinearAlgebra.SingularValueDecomposition SVD = new Iridium.Numerics.LinearAlgebra.SingularValueDecomposition(Covariance);
Iridium.Numerics.LinearAlgebra.Matrix U = SVD.LeftSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix V = SVD.RightSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix s = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1.0);
if (Covariance.Rank() < 2)
{
throw new Exception("Cannot allign generic model (cov rank is less than 2)");
}
if (Covariance.Rank() == 2) // m-1 where m is dimension space (3D)
{
double detU = Math.Round(U.Determinant());
double detV = Math.Round(V.Determinant());
double detC = Covariance.Determinant();
if ((int)detU * (int)detV == 1)
{
s[2, 2] = 1;
}
else if ((int)detU * (int)detV == -1)
{
s[2, 2] = -1;
}
else
{
throw new Exception("Determinant of U and V are not in conditions");
}
}
else
{
if (Covariance.Determinant() < 0)
{
s[2, 2] = -1;
}
}
V.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Roatation = U * s * V;
Iridium.Numerics.LinearAlgebra.Matrix MeanValOfGenModelMatrix = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
MeanValOfGenModelMatrix[0, 0] = MeanGenericModel[0];
MeanValOfGenModelMatrix[1, 0] = MeanGenericModel[1];
MeanValOfGenModelMatrix[2, 0] = MeanGenericModel[2];
Iridium.Numerics.LinearAlgebra.Matrix MeanValOfModelMatrix = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
MeanValOfModelMatrix[0, 0] = MeanValX;
MeanValOfModelMatrix[1, 0] = MeanValY;
MeanValOfModelMatrix[2, 0] = MeanValZ;
Iridium.Numerics.LinearAlgebra.Matrix Translation = MeanValOfModelMatrix - Roatation * MeanValOfGenModelMatrix;
//--------------------------- End now we have translation and rotation
float GenericModelDistance = 0;;
Iridium.Numerics.LinearAlgebra.Matrix q = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
q[0, 0] = GenericNoseTip[0];
q[1, 0] = GenericNoseTip[1];
q[2, 0] = GenericNoseTip[2];
Iridium.Numerics.LinearAlgebra.Matrix NewQ = Roatation * q + Translation;
GenericModelDistance += (float)Math.Sqrt(Math.Pow(noseTip.X - NewQ[0, 0], 2) + Math.Pow(noseTip.Y - NewQ[1, 0], 2) + Math.Pow(noseTip.Z - NewQ[2, 0], 2));
//---------
// Cl3DModel.Cl3DModelPointIterator it1 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
//it1.AddNeighbor(noseTip);
// it1.Color = Color.Green;
//---------
q[0, 0] = GenericLeftEye[0];
q[1, 0] = GenericLeftEye[1];
q[2, 0] = GenericLeftEye[2];
NewQ = Roatation * q + Translation;
GenericModelDistance += (float)Math.Sqrt(Math.Pow(eyeLeft.X - NewQ[0, 0], 2) + Math.Pow(eyeLeft.Y - NewQ[1, 0], 2) + Math.Pow(eyeLeft.Z - NewQ[2, 0], 2));
//---------
// Cl3DModel.Cl3DModelPointIterator it2 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
//it2.AddNeighbor(eyeLeft);
// it2.Color = Color.White;
//---------
q[0, 0] = GenericRightEye[0];
q[1, 0] = GenericRightEye[1];
q[2, 0] = GenericRightEye[2];
NewQ = Roatation * q + Translation;
GenericModelDistance += (float)Math.Sqrt(Math.Pow(eyeRight.X - NewQ[0, 0], 2) + Math.Pow(eyeRight.Y - NewQ[1, 0], 2) + Math.Pow(eyeRight.Z - NewQ[2, 0], 2));
//---------
// Cl3DModel.Cl3DModelPointIterator it3 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
//it3.AddNeighbor(eyeRight);
// it3.Color = Color.Orange;
// it1.AddNeighbor(it2);
// it1.AddNeighbor(it3);
// it2.AddNeighbor(it3);
//---------
// ---------- Rest of generic model to find closest points and count distance;
q[0, 0] = GenLeftCornerOfLeftEye[0];
q[1, 0] = GenLeftCornerOfLeftEye[1];
q[2, 0] = GenLeftCornerOfLeftEye[2];
NewQ = Roatation * q + Translation;
Cl3DModel.Cl3DModelPointIterator closestPoint = null;
float MinDistance;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
q[0, 0] = GenRightCornerOfRightEye[0];
q[1, 0] = GenRightCornerOfRightEye[1];
q[2, 0] = GenRightCornerOfRightEye[2];
NewQ = Roatation * q + Translation;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
q[0, 0] = GenLeftCornerOfNose[0];
q[1, 0] = GenLeftCornerOfNose[1];
q[2, 0] = GenLeftCornerOfNose[2];
NewQ = Roatation * q + Translation;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
q[0, 0] = GenRightCornerOfNose[0];
q[1, 0] = GenRightCornerOfNose[1];
q[2, 0] = GenRightCornerOfNose[2];
NewQ = Roatation * q + Translation;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
q[0, 0] = GenLeftCornerOfLips[0];
q[1, 0] = GenLeftCornerOfLips[1];
q[2, 0] = GenLeftCornerOfLips[2];
NewQ = Roatation * q + Translation;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
q[0, 0] = GenRightCornerOfLips[0];
q[1, 0] = GenRightCornerOfLips[1];
q[2, 0] = GenRightCornerOfLips[2];
NewQ = Roatation * q + Translation;
ClTools.FindClosestPointInModel(p_Model, (float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0], out closestPoint, out MinDistance);
GenericModelDistance += MinDistance;
// ----------
ClInformationSender.SendInformation("One of distances between generic model and face before thresholding: " + GenericModelDistance.ToString(System.Globalization.CultureInfo.InvariantCulture), ClInformationSender.eInformationType.eDebugText);
if (GenericModelDistance > m_fDistanceMinValueFromGenModel)
{
continue;
}
// Check if between eyes and nose there is other region (it should be not)
/*bool isBetweenNoseRegion = false;
* foreach (Cl3DModel.Cl3DModelPointIterator noseTmp in NoseTipsPoints)
* {
* if (noseTmp.PointID == noseTip.PointID)
* continue;
*
* if (noseTmp.X > eyeLeft.X && noseTmp.X < eyeRight.X && noseTmp.Y > noseTip.Y && (noseTip.Y < eyeLeft.Y || noseTip.Y < eyeRight.Y))
* isBetweenNoseRegion = true;
* }
* if (isBetweenNoseRegion)
* continue;
*/
List <Cl3DModel.Cl3DModelPointIterator> list = new List <Cl3DModel.Cl3DModelPointIterator>();
list.Add(noseTip);
list.Add(eyeLeft);
list.Add(eyeRight);
KeyValuePair <float, List <Cl3DModel.Cl3DModelPointIterator> > KeyVal = new KeyValuePair <float, List <Cl3DModel.Cl3DModelPointIterator> >(GenericModelDistance, list);
deltaOfDistanceBetweenModelAndPoints.Add(KeyVal);
}
}
}
}
if (deltaOfDistanceBetweenModelAndPoints.Count == 0)
{
throw new Exception("Cannot find nose and eyes points");
}
float min = deltaOfDistanceBetweenModelAndPoints[0].Key;
int no = 0;
for (int i = 1; i < deltaOfDistanceBetweenModelAndPoints.Count; i++)
{
if (deltaOfDistanceBetweenModelAndPoints[i].Key < min)
{
min = deltaOfDistanceBetweenModelAndPoints[i].Key;
no = i;
}
}
ClInformationSender.SendInformation("Correct MIN distance between generic model and face: " + min.ToString(System.Globalization.CultureInfo.InvariantCulture), ClInformationSender.eInformationType.eDebugText);
p_Model.AddSpecificPoint(NameOfNoseTip, deltaOfDistanceBetweenModelAndPoints[no].Value[0]);
p_Model.AddSpecificPoint(NameLeftEyeRightCorner, deltaOfDistanceBetweenModelAndPoints[no].Value[1]);
p_Model.AddSpecificPoint(NameRightEyeLeftCorner, deltaOfDistanceBetweenModelAndPoints[no].Value[2]);
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
Cl3DModel.Cl3DModelPointIterator max = iter.CopyIterator();
float distanceMax = 0;
do
{
float distance = (float)Math.Sqrt(Math.Pow(iter.U, 2) + Math.Pow(iter.V, 2));
if (distanceMax < distance)
{
distanceMax = distance;
}
} while (iter.MoveToNext());
iter = p_Model.GetIterator();
do
{
iter.U = (iter.U / distanceMax) * m_RadiousOfConformalMap;
iter.V = (iter.V / distanceMax) * m_RadiousOfConformalMap;
} while (iter.MoveToNext());
Cl3DModel.Cl3DModelPointIterator LeftEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner);
Cl3DModel.Cl3DModelPointIterator RightEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner);
Cl3DModel.Cl3DModelPointIterator NoseTip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
Cl3DModel.Cl3DModelPointIterator tmp;
if (LeftEye.X > RightEye.X)
{
tmp = LeftEye.CopyIterator();
RightEye = LeftEye.CopyIterator();
LeftEye = tmp.CopyIterator();
}
double EyesDistance = Math.Sqrt(Math.Pow(LeftEye.U - RightEye.U, 2) + Math.Pow(LeftEye.V - RightEye.V, 2));
double NoseLeftEyeDistance = Math.Sqrt(Math.Pow(LeftEye.U - NoseTip.U, 2) + Math.Pow(LeftEye.V - NoseTip.V, 2));
double NoseRightEyeDistance = Math.Sqrt(Math.Pow(NoseTip.U - RightEye.U, 2) + Math.Pow(NoseTip.V - RightEye.V, 2));
double AverageNoseTipEyeDistance = (NoseLeftEyeDistance + NoseRightEyeDistance) / 2;
Matrix GenModel = new Matrix(2, 3);
GenModel[0, 0] = 0;
GenModel[1, 0] = 0; // nose
GenModel[0, 1] = (EyesDistance / 2);
GenModel[1, 1] = AverageNoseTipEyeDistance; // left eye
GenModel[0, 2] = -(EyesDistance / 2);
GenModel[1, 2] = AverageNoseTipEyeDistance; // right eye
Matrix Model = new Matrix(2, 3);
Model[0, 0] = NoseTip.U;
Model[1, 0] = NoseTip.V; // nose
Model[0, 1] = LeftEye.U;
Model[1, 1] = LeftEye.V; // left eye
Model[0, 2] = RightEye.U;
Model[1, 2] = RightEye.V; // right eye
Matrix rotationMatrix = null;
Matrix translationMatrix = null;
ClTools.CalculateRotationAndTranslation(GenModel, Model, out rotationMatrix, out translationMatrix);
Iridium.Numerics.LinearAlgebra.Matrix q = new Iridium.Numerics.LinearAlgebra.Matrix(2, 1);
iter = p_Model.GetIterator();
do
{
q[0, 0] = iter.U;
q[1, 0] = iter.V;
Iridium.Numerics.LinearAlgebra.Matrix NewQ = rotationMatrix * q + translationMatrix;
iter.U = (float)NewQ[0, 0];
iter.V = (float)NewQ[1, 0];
} while (iter.MoveToNext());
float U = NoseTip.U;
float V = NoseTip.V;
iter = p_Model.GetIterator();
do
{
iter.U -= U;
iter.V -= V;
} while (iter.MoveToNext());
if (centerConformalMaps)
{
float middleU = 0;
float middleV = 0;
iter = p_Model.GetIterator();
do
{
middleU += iter.U;
middleV += iter.V;
} while (iter.MoveToNext());
middleU /= p_Model.ModelPointsCount;
middleV /= p_Model.ModelPointsCount;
iter = p_Model.GetIterator();
do
{
iter.U -= middleU;
iter.V -= middleV;
} while (iter.MoveToNext());
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeRightCorner = null;
Cl3DModel.Cl3DModelPointIterator SavedLeftCornerOfRightEye = null;
Cl3DModel.Cl3DModelPointIterator SavedNoseTip = null;
if (!p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner.ToString(), ref SavedLeftEyeRightCorner))
{
throw new Exception("Cannot find specific point EyePoint");
}
if (!p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner.ToString(), ref SavedLeftCornerOfRightEye))
{
throw new Exception("Cannot find specific point EyePoint");
}
if (!p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip.ToString(), ref SavedNoseTip))
{
throw new Exception("Cannot find specific point NosePoint");
}
// Normalized Generic Model (distance between eyes = 1) from IV2
Vector GenNoseTip = new Vector(new double[] { 0f, 0f, 0f });
Vector GenLeftEyeRightCorner = new Vector(new double[] { -0.5f, 0.88f, -1f });
Vector GenLeftCornerOfRightEye = new Vector(new double[] { 0.5f, 0.88f, -1f });
Vector GenLeftCornerOfLeftEye = new Vector(new double[] { -1.2f, 0.85f, -1f });
Vector GenRightCornerOfRightEye = new Vector(new double[] { 1.2f, 0.85f, -1f });
Vector GenLeftCornerOfNose = new Vector(new double[] { -0.45f, -0.05f, -0.73f });
Vector GenRightCornerOfNose = new Vector(new double[] { 0.45f, -0.05f, -0.73f });
Vector GenLeftCornerOfLips = new Vector(new double[] { -0.66f, -0.8f, -0.82f });
Vector GenRightCornerOfLips = new Vector(new double[] { 0.66f, -0.8f, -0.82f });
float DistBetweenEyes = (float)Math.Sqrt(Math.Pow(SavedLeftCornerOfRightEye.X - SavedLeftEyeRightCorner.X, 2) + Math.Pow(SavedLeftCornerOfRightEye.Y - SavedLeftEyeRightCorner.Y, 2) + Math.Pow(SavedLeftCornerOfRightEye.Z - SavedLeftEyeRightCorner.Z, 2));
float SavedDistBetweenEyeAndNose = (float)Math.Sqrt(Math.Pow(SavedNoseTip.X - SavedLeftEyeRightCorner.X, 2) + Math.Pow(SavedNoseTip.Y - SavedLeftEyeRightCorner.Y, 2) + Math.Pow(SavedNoseTip.Z - SavedLeftEyeRightCorner.Z, 2));
GenNoseTip *= DistBetweenEyes;
GenLeftEyeRightCorner *= DistBetweenEyes;
GenLeftCornerOfRightEye *= DistBetweenEyes;
GenLeftCornerOfLeftEye *= DistBetweenEyes;
GenRightCornerOfRightEye *= DistBetweenEyes;
GenLeftCornerOfNose *= DistBetweenEyes;
GenRightCornerOfNose *= DistBetweenEyes;
GenLeftCornerOfLips *= DistBetweenEyes;
GenRightCornerOfLips *= DistBetweenEyes;
float DistBetweenEyeAndNoseAfterScale = (float)Math.Sqrt(Math.Pow(GenNoseTip[0] - GenLeftEyeRightCorner[0], 2) + Math.Pow(GenNoseTip[1] - GenLeftEyeRightCorner[1], 2) + Math.Pow(GenNoseTip[2] - GenLeftEyeRightCorner[2], 2));
float muntipltBy = SavedDistBetweenEyeAndNose / DistBetweenEyeAndNoseAfterScale;
GenNoseTip *= muntipltBy;
GenLeftEyeRightCorner[1] *= muntipltBy;
GenLeftEyeRightCorner[2] *= muntipltBy;
GenLeftCornerOfRightEye[1] *= muntipltBy;
GenLeftCornerOfRightEye[2] *= muntipltBy;
GenLeftCornerOfLeftEye[1] *= muntipltBy;
GenLeftCornerOfLeftEye[2] *= muntipltBy;
GenRightCornerOfRightEye[1] *= muntipltBy;
GenRightCornerOfRightEye[2] *= muntipltBy;
GenLeftCornerOfNose[1] *= muntipltBy;
GenLeftCornerOfNose[2] *= muntipltBy;
GenRightCornerOfNose[1] *= muntipltBy;
GenRightCornerOfNose[2] *= muntipltBy;
GenLeftCornerOfLips[1] *= muntipltBy;
GenLeftCornerOfLips[2] *= muntipltBy;
GenRightCornerOfLips[1] *= muntipltBy;
GenRightCornerOfLips[2] *= muntipltBy;
Vector MeanValOfGenModel = new Vector(new double[] { (GenNoseTip[0] + GenLeftEyeRightCorner[0] + GenLeftCornerOfRightEye[0]) / 3,
(GenNoseTip[1] + GenLeftEyeRightCorner[1] + GenLeftCornerOfRightEye[1]) / 3,
(GenNoseTip[2] + GenLeftEyeRightCorner[2] + GenLeftCornerOfRightEye[2]) / 3 });
Vector MeanValOfModel = new Vector(new double[] { (SavedNoseTip.X + SavedLeftEyeRightCorner.X + SavedLeftCornerOfRightEye.X) / 3,
(SavedNoseTip.Y + SavedLeftEyeRightCorner.Y + SavedLeftCornerOfRightEye.Y) / 3,
(SavedNoseTip.Z + SavedLeftEyeRightCorner.Z + SavedLeftCornerOfRightEye.Z) / 3 });
Iridium.Numerics.LinearAlgebra.Matrix Model = new Iridium.Numerics.LinearAlgebra.Matrix(3, 3);
Iridium.Numerics.LinearAlgebra.Matrix ModelGen = new Iridium.Numerics.LinearAlgebra.Matrix(3, 3);
// Row Column
Model[0, 0] = (SavedNoseTip.X - MeanValOfModel[0]);
Model[1, 0] = (SavedNoseTip.Y - MeanValOfModel[1]);
Model[2, 0] = (SavedNoseTip.Z - MeanValOfModel[2]);
Model[0, 1] = (SavedLeftEyeRightCorner.X - MeanValOfModel[0]);
Model[1, 1] = (SavedLeftEyeRightCorner.Y - MeanValOfModel[1]);
Model[2, 1] = (SavedLeftEyeRightCorner.Z - MeanValOfModel[2]);
Model[0, 2] = (SavedLeftCornerOfRightEye.X - MeanValOfModel[0]);
Model[1, 2] = (SavedLeftCornerOfRightEye.Y - MeanValOfModel[1]);
Model[2, 2] = (SavedLeftCornerOfRightEye.Z - MeanValOfModel[2]);
ModelGen[0, 0] = (GenNoseTip[0] - MeanValOfGenModel[0]);
ModelGen[1, 0] = (GenNoseTip[1] - MeanValOfGenModel[1]);
ModelGen[2, 0] = (GenNoseTip[2] - MeanValOfGenModel[2]);
ModelGen[0, 1] = (GenLeftEyeRightCorner[0] - MeanValOfGenModel[0]);
ModelGen[1, 1] = (GenLeftEyeRightCorner[1] - MeanValOfGenModel[1]);
ModelGen[2, 1] = (GenLeftEyeRightCorner[2] - MeanValOfGenModel[2]);
ModelGen[0, 2] = (GenLeftCornerOfRightEye[0] - MeanValOfGenModel[0]);
ModelGen[1, 2] = (GenLeftCornerOfRightEye[1] - MeanValOfGenModel[1]);
ModelGen[2, 2] = (GenLeftCornerOfRightEye[2] - MeanValOfGenModel[2]);
ModelGen.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Covariance = Model * ModelGen;
Iridium.Numerics.LinearAlgebra.SingularValueDecomposition SVD = new Iridium.Numerics.LinearAlgebra.SingularValueDecomposition(Covariance);
Iridium.Numerics.LinearAlgebra.Matrix U = SVD.LeftSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix V = SVD.RightSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix s = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1.0);
if (Covariance.Rank() < 2)
{
throw new Exception("Cannot allign generic model (cov rank is less than 2)");
}
if (Covariance.Rank() == 2) // m-1 where m is dimension space (3D)
{
double detU = Math.Round(U.Determinant());
double detV = Math.Round(V.Determinant());
double detC = Covariance.Determinant();
if ((int)detU * (int)detV == 1)
{
s[2, 2] = 1;
}
else if ((int)detU * (int)detV == -1)
{
s[2, 2] = -1;
}
else
{
throw new Exception("Determinant of U and V are not in conditions");
}
}
else
{
if (Covariance.Determinant() < 0)
{
s[2, 2] = -1;
}
}
V.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Roatation = U * s * V;
Iridium.Numerics.LinearAlgebra.Matrix MeanValOfGenModelMatrix = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
MeanValOfGenModelMatrix[0, 0] = MeanValOfGenModel[0];
MeanValOfGenModelMatrix[1, 0] = MeanValOfGenModel[1];
MeanValOfGenModelMatrix[2, 0] = MeanValOfGenModel[2];
Iridium.Numerics.LinearAlgebra.Matrix MeanValOfModelMatrix = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
MeanValOfModelMatrix[0, 0] = MeanValOfModel[0];
MeanValOfModelMatrix[1, 0] = MeanValOfModel[1];
MeanValOfModelMatrix[2, 0] = MeanValOfModel[2];
Iridium.Numerics.LinearAlgebra.Matrix Translation = MeanValOfModelMatrix - Roatation * MeanValOfGenModelMatrix;
Iridium.Numerics.LinearAlgebra.Matrix q = new Iridium.Numerics.LinearAlgebra.Matrix(3, 1);
Iridium.Numerics.LinearAlgebra.Matrix NewQ = null;
//------------- Code to show triangles created to check distance between them and features points -----------------------------------
/*
* q[0, 0] = GenNoseTip.X;
* q[1, 0] = GenNoseTip.Y;
* q[2, 0] = GenNoseTip.Z;
* NewQ = Roatation * q + Translation;
* Cl3DModel.Cl3DModelPointIterator iter1 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
* iter1.Color = Color.Green;
*
* q[0, 0] = GenLeftEyeRightCorner.X;
* q[1, 0] = GenLeftEyeRightCorner.Y;
* q[2, 0] = GenLeftEyeRightCorner.Z;
* NewQ = Roatation * q + Translation;
* Cl3DModel.Cl3DModelPointIterator iter2 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
* iter2.Color = Color.White;
*
* q[0, 0] = GenLeftCornerOfRightEye.X;
* q[1, 0] = GenLeftCornerOfRightEye.Y;
* q[2, 0] = GenLeftCornerOfRightEye.Z;
* NewQ = Roatation * q + Translation;
* Cl3DModel.Cl3DModelPointIterator iter3 = p_Model.AddPointToModel((float)NewQ[0, 0], (float)NewQ[1, 0], (float)NewQ[2, 0]);
* iter3.Color = Color.Orange;
*
* iter1.AddNeighbor(iter2);
* iter1.AddNeighbor(iter3);
* iter2.AddNeighbor(iter3);
*/
//---------------------------------------------------------------------------
Cl3DModel.Cl3DModelPointIterator iter = null;
float MinDistance = 0;
Cl3DModel.Cl3DModelPointIterator MinIter = null;
//------------------ Code to correct nose tip and eyes position -----------------------------------
/*
* GenNoseTip.X = (float)NewQ[0, 0];
* GenNoseTip.Y = (float)NewQ[1, 0];
* GenNoseTip.Z = (float)NewQ[2, 0];
*
* iter = p_Model.GetIterator();
* MinDistance = (float)Math.Sqrt(Math.Pow(GenNoseTip.X - iter.X, 2) + Math.Pow(GenNoseTip.Y - iter.Y, 2) + Math.Pow(GenNoseTip.Z - iter.Z, 2));
* MinIter = iter;
* do
* {
* float NewDistance = (float)Math.Sqrt(Math.Pow(GenNoseTip.X - iter.X, 2) + Math.Pow(GenNoseTip.Y - iter.Y, 2) + Math.Pow(GenNoseTip.Z - iter.Z, 2));
* if (NewDistance < MinDistance)
* {
* MinDistance = NewDistance;
* MinIter = iter.CopyIterator();
* }
* } while (iter.MoveToNext());
* p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip, MinIter);
*
* q[0, 0] = GenLeftEyeRightCorner.X;
* q[1, 0] = GenLeftEyeRightCorner.Y;
* q[2, 0] = GenLeftEyeRightCorner.Z;
* NewQ = Roatation * q + Translation;
* GenLeftEyeRightCorner.X = (float)NewQ[0, 0];
* GenLeftEyeRightCorner.Y = (float)NewQ[1, 0];
* GenLeftEyeRightCorner.Z = (float)NewQ[2, 0];
* iter = p_Model.GetIterator();
* MinDistance = (float)Math.Sqrt(Math.Pow(GenLeftEyeRightCorner.X - iter.X, 2) + Math.Pow(GenLeftEyeRightCorner.Y - iter.Y, 2) + Math.Pow(GenLeftEyeRightCorner.Z - iter.Z, 2));
* MinIter = iter;
* do
* {
* float NewDistance = (float)Math.Sqrt(Math.Pow(GenLeftEyeRightCorner.X - iter.X, 2) + Math.Pow(GenLeftEyeRightCorner.Y - iter.Y, 2) + Math.Pow(GenLeftEyeRightCorner.Z - iter.Z, 2));
* if (NewDistance < MinDistance)
* {
* MinDistance = NewDistance;
* MinIter = iter.CopyIterator();
* }
* } while (iter.MoveToNext());
* p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner, MinIter);
*
* q[0, 0] = GenLeftCornerOfRightEye.X;
* q[1, 0] = GenLeftCornerOfRightEye.Y;
* q[2, 0] = GenLeftCornerOfRightEye.Z;
* NewQ = Roatation * q + Translation;
* GenLeftCornerOfRightEye.X = (float)NewQ[0, 0];
* GenLeftCornerOfRightEye.Y = (float)NewQ[1, 0];
* GenLeftCornerOfRightEye.Z = (float)NewQ[2, 0];
* iter = p_Model.GetIterator();
* MinDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfRightEye.X - iter.X, 2) + Math.Pow(GenLeftCornerOfRightEye.Y - iter.Y, 2) + Math.Pow(GenLeftCornerOfRightEye.Z - iter.Z, 2));
* MinIter = iter;
* do
* {
* float NewDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfRightEye.X - iter.X, 2) + Math.Pow(GenLeftCornerOfRightEye.Y - iter.Y, 2) + Math.Pow(GenLeftCornerOfRightEye.Z - iter.Z, 2));
* if (NewDistance < MinDistance)
* {
* MinDistance = NewDistance;
* MinIter = iter.CopyIterator();
* }
* } while (iter.MoveToNext());
* p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner, MinIter);
*/
//---------------------------------------------------------------------------------------------------------
q[0, 0] = GenLeftCornerOfLeftEye[0];
q[1, 0] = GenLeftCornerOfLeftEye[1];
q[2, 0] = GenLeftCornerOfLeftEye[2];
NewQ = Roatation * q + Translation;
GenLeftCornerOfLeftEye[0] = (float)NewQ[0, 0];
GenLeftCornerOfLeftEye[1] = (float)NewQ[1, 0];
GenLeftCornerOfLeftEye[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfLeftEye[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfLeftEye[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfLeftEye[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfLeftEye[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfLeftEye[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfLeftEye[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeLeftCorner, MinIter);
}
q[0, 0] = GenRightCornerOfRightEye[0];
q[1, 0] = GenRightCornerOfRightEye[1];
q[2, 0] = GenRightCornerOfRightEye[2];
NewQ = Roatation * q + Translation;
GenRightCornerOfRightEye[0] = (float)NewQ[0, 0];
GenRightCornerOfRightEye[1] = (float)NewQ[1, 0];
GenRightCornerOfRightEye[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfRightEye[0] - iter.X, 2) + Math.Pow(GenRightCornerOfRightEye[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfRightEye[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfRightEye[0] - iter.X, 2) + Math.Pow(GenRightCornerOfRightEye[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfRightEye[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeRightCorner, MinIter);
}
q[0, 0] = GenLeftCornerOfNose[0];
q[1, 0] = GenLeftCornerOfNose[1];
q[2, 0] = GenLeftCornerOfNose[2];
NewQ = Roatation * q + Translation;
GenLeftCornerOfNose[0] = (float)NewQ[0, 0];
GenLeftCornerOfNose[1] = (float)NewQ[1, 0];
GenLeftCornerOfNose[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfNose[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfNose[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfNose[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfNose[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfNose[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfNose[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfNose, MinIter);
}
q[0, 0] = GenRightCornerOfNose[0];
q[1, 0] = GenRightCornerOfNose[1];
q[2, 0] = GenRightCornerOfNose[2];
NewQ = Roatation * q + Translation;
GenRightCornerOfNose[0] = (float)NewQ[0, 0];
GenRightCornerOfNose[1] = (float)NewQ[1, 0];
GenRightCornerOfNose[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfNose[0] - iter.X, 2) + Math.Pow(GenRightCornerOfNose[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfNose[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfNose[0] - iter.X, 2) + Math.Pow(GenRightCornerOfNose[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfNose[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfNose, MinIter);
}
q[0, 0] = GenLeftCornerOfLips[0];
q[1, 0] = GenLeftCornerOfLips[1];
q[2, 0] = GenLeftCornerOfLips[2];
NewQ = Roatation * q + Translation;
GenLeftCornerOfLips[0] = (float)NewQ[0, 0];
GenLeftCornerOfLips[1] = (float)NewQ[1, 0];
GenLeftCornerOfLips[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfLips[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfLips[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfLips[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenLeftCornerOfLips[0] - iter.X, 2) + Math.Pow(GenLeftCornerOfLips[1] - iter.Y, 2) + Math.Pow(GenLeftCornerOfLips[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfLips, MinIter);
}
q[0, 0] = GenRightCornerOfLips[0];
q[1, 0] = GenRightCornerOfLips[1];
q[2, 0] = GenRightCornerOfLips[2];
NewQ = Roatation * q + Translation;
GenRightCornerOfLips[0] = (float)NewQ[0, 0];
GenRightCornerOfLips[1] = (float)NewQ[1, 0];
GenRightCornerOfLips[2] = (float)NewQ[2, 0];
iter = p_Model.GetIterator();
MinDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfLips[0] - iter.X, 2) + Math.Pow(GenRightCornerOfLips[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfLips[2] - iter.Z, 2));
MinIter = iter;
do
{
float NewDistance = (float)Math.Sqrt(Math.Pow(GenRightCornerOfLips[0] - iter.X, 2) + Math.Pow(GenRightCornerOfLips[1] - iter.Y, 2) + Math.Pow(GenRightCornerOfLips[2] - iter.Z, 2));
if (NewDistance < MinDistance)
{
MinDistance = NewDistance;
MinIter = iter.CopyIterator();
}
} while (iter.MoveToNext());
if (MinDistance < m_MinDistanceToAddPoint)
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfLips, MinIter);
}
}
