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