protected override void Algorithm(ref Cl3DModel p_Model)
{
string name = p_Model.ModelFileFolder + p_Model.ModelFileName + ".curv";
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Iterator in the model is not valid");
using (TextWriter tw = new StreamWriter(name, false))
{
tw.WriteLine("@----------------------------------------");
tw.WriteLine("@ Przemyslaw Szeptycki LIRIS 2008");
tw.WriteLine("@ Face model curvatures");
tw.WriteLine("@ Model name: " + p_Model.ModelFileName);
tw.WriteLine("@----------------------------------------");
tw.WriteLine("@ (PointID) X Y Z");
tw.WriteLine("@ \tCurvatureName: value");
do
{
List<String> SpecValList = iter.GetListOfSpecificValues();
string line = "(" + iter.PointID.ToString(System.Globalization.CultureInfo.InvariantCulture) + ")" + iter.X.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " + iter.Y.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " + iter.Z.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
foreach (String SPV in SpecValList)
{
double val;
iter.GetSpecificValue(SPV, out val);
line += "\t" + SPV + ": " + val.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
}
tw.WriteLine(line);
} while (iter.MoveToNext());
tw.Close();
}
}
// public override void SetProperitis(string p_sProperity, string p_sValue)
// {
// }
// public override List<KeyValuePair<string, string>> GetProperitis()
// {
// }
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
iter.X = iter.U;
iter.Y = iter.V;
iter.Z = 0;
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
if (iter.NormalVector != null)
{
iter.Color = Color.FromArgb((int)Math.Abs((iter.NormalVector[0]) * 255),
(int)Math.Abs((iter.NormalVector[1]) * 255),
(int)Math.Abs((iter.NormalVector[2]) * 255));
}
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
string name = "";
if (p_Model.ModelType.Equals("bnt"))
{
name = p_Model.ModelFileFolder + p_Model.ModelFileName + ".png";
}
else
throw new Exception("Method does not supprt this kind of files");
Bitmap Texture = new Bitmap(name);
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Model iterator not Valid");
int MinX;
int MaxX;
int MinY;
int MaxY;
MinX = iter.RangeImageX;
MaxX = iter.RangeImageX;
MinY = iter.RangeImageY;
MaxY = iter.RangeImageY;
do
{
if (MinX > iter.RangeImageX)
MinX = iter.RangeImageX;
if (MaxX < iter.RangeImageX)
MaxX = iter.RangeImageX;
if (MinY > iter.RangeImageY)
MinY = iter.RangeImageY;
if (MaxY < iter.RangeImageY)
MaxY = iter.RangeImageY;
} while (iter.MoveToNext());
iter = p_Model.GetIterator();
do
{
float x = ((float)iter.RangeImageX-MinX)/(MaxX-MinX+1);
float y = ((float)iter.RangeImageY-MinY)/(MaxY-MinY+1);
iter.Color = Texture.GetPixel( (int)(x *Texture.Width) ,(int)(y*Texture.Height));
iter.AddSpecificValue("Texture", iter.Color.ToArgb());
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator maxIter = p_Model.GetIterator();
Cl3DModel.Cl3DModelPointIterator iter = maxIter.CopyIterator();
while (iter.MoveToNext())
{
if (maxIter.Z < iter.Z)
maxIter = iter.CopyIterator();
}
if (maxIter.IsValid())
{
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip.ToString(), maxIter);
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel ConformalModel = new Cl3DModel();
ConformalModel.LoadModel(p_Model.ModelFileFolder+ConformalMapFolder+"\\"+p_Model.ModelFileName+".m_Out.pos.m");
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
List<ClTools.ClTriangle> Triangles = null;
float ModelArea = CalculateWholeModelArea(p_Model);
float ConformalMapArea = CalculateWholeModelArea(ConformalModel);
if(iter.IsValid())
do
{
float area3D = 0;
ClTools.GetListOfTriangles(out Triangles, iter);
// if (Triangles.Count != 6)
// continue;
foreach (ClTools.ClTriangle triangle in Triangles)
area3D += ClTools.CalculateTriangleArea(triangle);
area3D /= Triangles.Count;
iter.AddSpecificValue("ConnectedTrianglesArea", area3D);
Cl3DModel.Cl3DModelPointIterator ConformalIter = ConformalModel.GetIterator();
if (!ConformalIter.MoveToPoint(iter.PointID))
continue;//throw new Exception("Cannot find on conformal model point with no: " + iter.PointID.ToString());
float area2D = 0;
ClTools.GetListOfTriangles(out Triangles, ConformalIter);
foreach (ClTools.ClTriangle triangle in Triangles)
area2D += ClTools.CalculateTriangleArea(triangle);
area2D /= Triangles.Count;
float ConformalFactor = (area3D / ModelArea) / (area2D / ConformalMapArea);
ConformalIter.AddSpecificValue("ConformalFactor", ConformalFactor);
} while (iter.MoveToNext());
p_Model = ConformalModel;
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
float maxX = iter.X;
float minX = iter.X;
float maxY = iter.Y;
float minY = iter.Y;
float maxZ = iter.Z;
float minZ = iter.Z;
do
{
if (maxX < iter.X)
maxX = iter.X;
if (maxY < iter.Y)
maxY = iter.Y;
if (maxZ < iter.Z)
maxZ = iter.Z;
if (minX > iter.X)
minX = iter.X;
if (minY > iter.Y)
minY = iter.Y;
if (minZ > iter.Z)
minZ = iter.Z;
} while (iter.MoveToNext());
iter = p_Model.GetIterator();
do
{
int R = (int)(((iter.X - minX) / (maxX - minX)) * 255);
int G = (int)(((iter.Y - minY) / (maxY - minY)) * 255);
int B = (int)(((iter.Z - minZ) / (maxZ - minZ)) * 255);
iter.Color = Color.FromArgb(R, G, B);
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
if (p_Model.ModelType != "abs")
throw new Exception("Remove Half Of The Face works only for ABS files");
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Model iterator not Valid");
int MinX;
int MaxX;
int MinY;
int MaxY;
MinX = iter.RangeImageX;
MaxX = iter.RangeImageX;
MinY = iter.RangeImageY;
MaxY = iter.RangeImageY;
do
{
if (MinX > iter.RangeImageX)
MinX = iter.RangeImageX;
if (MaxX < iter.RangeImageX)
MaxX = iter.RangeImageX;
if (MinY > iter.RangeImageY)
MinY = iter.RangeImageY;
if (MaxY < iter.RangeImageY)
MaxY = iter.RangeImageY;
} while (iter.MoveToNext());
int Width = (MaxX - MinX) + 1;
int Height = (MaxY - MinY) + 1;
Cl3DModel.Cl3DModelPointIterator[,] map = new Cl3DModel.Cl3DModelPointIterator[Width, Height];
iter = p_Model.GetIterator();
do
{
map[iter.RangeImageX - MinX, iter.RangeImageY - MinY] = iter.CopyIterator();
} while (iter.MoveToNext());
int procent = (int)(Width * m_procentToRemove);
for (int i = procent; i < Width; i++)
{
for (int j = 0; j < Height; j++)
{
p_Model.RemovePointFromModel(map[i, j]);
}
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator NoseTip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
Cl3DModel.Cl3DModelPointIterator LeftEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner);
Cl3DModel.Cl3DModelPointIterator RightEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner);
float eyesDistance = LeftEye - RightEye;
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
float NoseDistance = NoseTip - iter;
float LeftEyeDistance = LeftEye - iter;
float RightEyeDistance = RightEye - iter;
if (NoseDistance > 40 && NoseDistance < 50 && LeftEyeDistance > 50 && LeftEyeDistance < 70 && RightEyeDistance > 50 && RightEyeDistance < 70)
{
iter.Color = Color.Red;
}
} while (iter.MoveToNext());
/*List<Cl3DModel.Cl3DModelPointIterator> neighbors;
ClTools.GetNeighborhoodWithEuclideanDistanceCheckNeighborhood(out neighbors, MinPoint, 20);
foreach (Cl3DModel.Cl3DModelPointIterator pt in neighbors)
pt.Color = Color.Red;
*/
/*
iter = p_Model.GetIterator();
minDistance = float.MaxValue;
MinPoint = iter;
do
{
float currDistance = (float)Math.Sqrt(Math.Pow(NoseTip - iter - 30, 2) + Math.Pow(LeftEye - iter - 64, 2) + Math.Pow(RightEye - iter - 64, 2));
if (currDistance < minDistance)
{
minDistance = currDistance;
MinPoint = iter.CopyIterator();
}
} while (iter.MoveToNext());
ClTools.GetNeighborhoodWithEuclideanDistanceCheckNeighborhood(out neighbors, MinPoint, 20);
foreach (Cl3DModel.Cl3DModelPointIterator pt in neighbors)
pt.Color = Color.Green;
*/
/*
Cl3DModel.Cl3DModelPointIterator NoseTip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
Cl3DModel.Cl3DModelPointIterator LeftEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner);
Cl3DModel.Cl3DModelPointIterator RightEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner);
Cl3DModel.Cl3DModelPointIterator RightLip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfLips);
Cl3DModel.Cl3DModelPointIterator UpperLip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.UpperLip);
Cl3DModel.Cl3DModelPointIterator LeftLip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfLips);
Cl3DModel.Cl3DModelPointIterator BottomLip = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.BottomLip);
TextWriter tw = new StreamWriter("d:\\Model.txt", true);
String toWrite = (LeftEye - RightEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (RightLip - NoseTip).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (RightLip - LeftEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (RightLip - RightEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (UpperLip - NoseTip).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (UpperLip - LeftEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (UpperLip - RightEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (LeftLip - NoseTip).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (LeftLip - LeftEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (LeftLip - RightEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (BottomLip - NoseTip).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (BottomLip - LeftEye).ToString(System.Globalization.CultureInfo.InvariantCulture) + " ";
toWrite += (BottomLip - RightEye).ToString(System.Globalization.CultureInfo.InvariantCulture);
tw.WriteLine(toWrite);
tw.Close();
*/
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
try
{
if (!File.Exists(m_sPath))
throw new Exception("External file does not exist, the path is incorrect: " + m_sPath);
ClSaveToMFileNew algSave = new ClSaveToMFileNew(p_Model.ModelFileFolder + p_Model.ModelFileName + ".m2");
algSave.MakeAlgorithm(p_Model);
Process proc = new Process();
proc.StartInfo.FileName = m_sPath;
proc.StartInfo.Arguments = "-harmonic_map \"" + p_Model.ModelFileFolder + p_Model.ModelFileName + ".m2\" \"" + p_Model.ModelFileFolder + p_Model.ModelFileName + "OUT.m\"";
proc.StartInfo.WindowStyle = ProcessWindowStyle.Hidden;
proc.Start();
if (!proc.WaitForExit(10000))
{
proc.Kill();
File.Delete(p_Model.ModelFileFolder + p_Model.ModelFileName + ".m2");
}
ClInformationSender.SendInformation("Parametrization calculated", ClInformationSender.eInformationType.eDebugText);
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
using (StreamReader FileStream = File.OpenText(p_Model.ModelFileFolder + p_Model.ModelFileName + "OUT.m"))
{
string line = "";
while ((line = FileStream.ReadLine()) != null)
{
if (!line.Contains("Vertex"))
continue;
int index = line.LastIndexOf("uv=") + 3;
string substring = line.Substring(index);
string[] subsubs = substring.Substring(substring.IndexOf('(') + 1, substring.LastIndexOf(')') - substring.IndexOf('(') - 1).Split(' ');
float U = float.Parse(subsubs[0], System.Globalization.CultureInfo.InvariantCulture);
float V = float.Parse(subsubs[1], System.Globalization.CultureInfo.InvariantCulture);
uint ID = uint.Parse(line.Split(' ')[1]);
if (!iter.MoveToPoint(ID - 1))
throw new Exception("Cannot localize point with number: " + ID.ToString());
iter.U = U;
iter.V = V;
}
}
File.Delete(p_Model.ModelFileFolder + p_Model.ModelFileName + ".m2");
if(removeOutput)
File.Delete(p_Model.ModelFileFolder + p_Model.ModelFileName + "OUT.m");
}
catch (Exception)
{
File.Delete(p_Model.ModelFileFolder + p_Model.ModelFileName + ".m2");
File.Delete(p_Model.ModelFileFolder + p_Model.ModelFileName + "OUT.m");
throw;
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator CenterPoint = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
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());
Complex z0 = new Complex(CenterPoint.U / distanceMax, CenterPoint.V / distanceMax);
iter = p_Model.GetIterator();
do
{
Complex newPoint = Mobius(m_Theta, z0, new Complex((iter.U / distanceMax), (iter.V / distanceMax)));
iter.U = (float)newPoint.Real * distanceMax;
iter.V = (float)newPoint.Imag * distanceMax;
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
while (iter.IsValid())
{
iter.X = iter.X * m_fScale;
iter.Y = iter.Y * m_fScale;
iter.Z = iter.Z * m_fScale;
if (!iter.MoveToNext())
break;
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if(iter.IsValid())
{
do
{
double H;
double K;
if (!iter.GetSpecificValue("Gaussian_"+NeighborhoodSize, out K))
continue;
if (!iter.GetSpecificValue("Mean_" + NeighborhoodSize, out H))
continue;
if (H < 0 && K < 0) // Saddle ridge
iter.Color = Color.FromArgb(0,0,255);//Blue;
else if (H == 0 && K < 0) // Minimal Surface
iter.Color = Color.Orange;
else if (H > 0 && K < 0) // Saddle Valley
iter.Color = Color.FromArgb(255,255,0);//Yellow;
else if (H < 0 && K == 0) // Ridge, Cylindrical convex (wypukłość)
iter.Color = Color.FromArgb(0,255,255);//SkyBlue;
else if (H == 0 && K == 0) // Plane
iter.Color = Color.FromArgb(255,255,255);
else if (H > 0 && K == 0) // Valley, Cylindrical concave (wklęsłość)
iter.Color = Color.FromArgb(255,0,255);//Pink;
else if (H < 0 && K > 0) // Peak, Elliptical convex (wypukłość)
iter.Color = Color.FromArgb(0,255,0);//Green;
else if (H == 0 && K > 0) // !!! Impossible
iter.Color = Color.Black;
else if (H > 0 && K > 0) // Pit, Elliptical concave (wklęsłość)
iter.Color = Color.FromArgb(255,0,0);//Red;
else
iter.Color = Color.White;
}
while (iter.MoveToNext());
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
string name = p_Model.ModelFileName;
if(m_CharsToSubstract != 0)
name = name.Substring(0, name.Length - m_CharsToSubstract);
if (!IDS)
{
string fileName = p_Model.ModelFileFolder + name + m_sFilePostFix + ".pts";
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
using (StreamReader FileStream = File.OpenText(fileName))
{
string line = "";
while ((line = FileStream.ReadLine()) != null)
{
if (line.Length == 0)
continue;
if (line[0].Equals('@'))
continue;
string[] splited = line.Split(' ');
Cl3DModel.eSpecificPoints type = Cl3DModel.eSpecificPoints.LeftEyeRightCorner;
if (splited[0].Equals("LeftEyeRightCorner"))
type = Cl3DModel.eSpecificPoints.LeftEyeRightCorner;
else if (splited[0].Equals("RightEyeLeftCorner"))
type = Cl3DModel.eSpecificPoints.RightEyeLeftCorner;
else if (splited[0].Equals("NoseTip"))
type = Cl3DModel.eSpecificPoints.NoseTip;
else
continue;
uint ID = UInt32.Parse(splited[1]);
if (!iter.MoveToPoint(ID))
throw new Exception("Cannot find point with ID: " + splited[1]);
p_Model.AddSpecificPoint(type, iter);
}
}
}
else
{
string fileName = p_Model.ModelFileFolder + name + m_sFilePostFix + ".ptsID";
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
using (StreamReader FileStream = File.OpenText(fileName))
{
string line = "";
int count = 0;
uint NoOfPoints = 0;
bool ReadHeader = false;
while ((line = FileStream.ReadLine()) != null)
{
if (!ReadHeader)
{
NoOfPoints = UInt32.Parse(line);
if (NoOfPoints != 3)
throw new Exception("Method ready to read 3 points");
ReadHeader = true;
}
else
{
uint id = UInt32.Parse(line);
iter.MoveToPoint(id);
if (count == 0)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip, iter);
if (count == 1)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner, iter);
if (count == 2)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner, iter);
count++;
}
}
}
}
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
CRRFile landmarkFile = ReadCRRFile(p_Model.ModelFileFolder + p_Model.ModelFileName + ".crr");
//search for the closest point from the model to the read landmark
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Iterator in the model is not valid!");
Cl3DModel.Cl3DModelPointIterator SavedNoseTip = null;
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeRightCorner = null;
Cl3DModel.Cl3DModelPointIterator SavedRightEyeLeftCorner = null;
Cl3DModel.Cl3DModelPointIterator SavedRightEyeRightCorner = null;
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeLeftCorner = null;
Cl3DModel.Cl3DModelPointIterator SavedLeftCornerOfNose = null;
Cl3DModel.Cl3DModelPointIterator SavedRightCornerOfNose = null;
Cl3DModel.Cl3DModelPointIterator SavedLeftCornerOfLips = null;
Cl3DModel.Cl3DModelPointIterator SavedRightCornerOfLips = null;
float RightCornerOfLipsDistance = 0;
float NoseTipDistance = 0;
float LeftEyeRightCornerDistance = 0;
float RightEyeLeftCornerDistance = 0;
float RightEyeRightCornerDistance = 0;
float LeftEyeLeftCornerDistance = 0;
float LeftCornerOfNoseDistance = 0;
float RightCornerOfNoseDistance = 0;
float LeftCornerOfLipsDistance = 0;
if (landmarkFile.NostTip != null)
{
SavedNoseTip = iter.CopyIterator();
NoseTipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.NostTip.X - SavedNoseTip.X, 2) + Math.Pow(landmarkFile.NostTip.Y - SavedNoseTip.Y, 2) + Math.Pow(landmarkFile.NostTip.Z - SavedNoseTip.Z, 2));
}
if (landmarkFile.LeftEyeRightCorner != null)
{
SavedLeftEyeRightCorner = iter.CopyIterator();
LeftEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeRightCorner.X - SavedLeftEyeRightCorner.X, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Y - SavedLeftEyeRightCorner.Y, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Z - SavedLeftEyeRightCorner.Z, 2));
}
if (landmarkFile.LeftCornerOfRightEye != null)
{
SavedRightEyeLeftCorner = iter.CopyIterator();
RightEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfRightEye.X - SavedRightEyeLeftCorner.X, 2) + Math.Pow(landmarkFile.LeftCornerOfRightEye.Y - SavedRightEyeLeftCorner.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfRightEye.Z - SavedRightEyeLeftCorner.Z, 2));
}
if (landmarkFile.RightCornerOfRightEye != null)
{
SavedRightEyeRightCorner = iter.CopyIterator();
RightEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfRightEye.X - SavedRightEyeRightCorner.X, 2) + Math.Pow(landmarkFile.RightCornerOfRightEye.Y - SavedRightEyeRightCorner.Y, 2) + Math.Pow(landmarkFile.RightCornerOfRightEye.Z - SavedRightEyeRightCorner.Z, 2));
}
if (landmarkFile.LeftCornerOfLeftEye != null)
{
SavedLeftEyeLeftCorner = iter.CopyIterator();
LeftEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLeftEye.X - SavedLeftEyeLeftCorner.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLeftEye.Y - SavedLeftEyeLeftCorner.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLeftEye.Z - SavedLeftEyeLeftCorner.Z, 2));
}
if (landmarkFile.LeftCornerOfNose != null)
{
SavedLeftCornerOfNose = iter.CopyIterator();
LeftCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfNose.X - SavedLeftCornerOfNose.X, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Y - SavedLeftCornerOfNose.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Z - SavedLeftCornerOfNose.Z, 2));
}
if (landmarkFile.RightCornerOfNose != null)
{
SavedRightCornerOfNose = iter.CopyIterator();
RightCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfNose.X - SavedRightCornerOfNose.X, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Y - SavedRightCornerOfNose.Y, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Z - SavedRightCornerOfNose.Z, 2));
}
if (landmarkFile.LeftCornerOfLips != null)
{
SavedLeftCornerOfLips = iter.CopyIterator();
LeftCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLips.X - SavedLeftCornerOfLips.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Y - SavedLeftCornerOfLips.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Z - SavedLeftCornerOfLips.Z, 2));
}
if (landmarkFile.RightCornerOfLips != null)
{
SavedRightCornerOfLips = iter.CopyIterator();
RightCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfLips.X - SavedRightCornerOfLips.X, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Y - SavedRightCornerOfLips.Y, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Z - SavedRightCornerOfLips.Z, 2));
}
do
{
if (SavedNoseTip != null)
{
float NewNoseTipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.NostTip.X - iter.X, 2) + Math.Pow(landmarkFile.NostTip.Y - iter.Y, 2) + Math.Pow(landmarkFile.NostTip.Z - iter.Z, 2));
if (NewNoseTipDistance < NoseTipDistance)
{
NoseTipDistance = NewNoseTipDistance;
SavedNoseTip = iter.CopyIterator();
}
}
if (SavedLeftEyeRightCorner != null)
{
float NewLeftEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeRightCorner.X - iter.X, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Z - iter.Z, 2));
if (NewLeftEyeRightCornerDistance < LeftEyeRightCornerDistance)
{
LeftEyeRightCornerDistance = NewLeftEyeRightCornerDistance;
SavedLeftEyeRightCorner = iter.CopyIterator();
}
}
if (SavedRightEyeLeftCorner != null)
{
float NewRightEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfRightEye.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfRightEye.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfRightEye.Z - iter.Z, 2));
if (NewRightEyeLeftCornerDistance < RightEyeLeftCornerDistance)
{
RightEyeLeftCornerDistance = NewRightEyeLeftCornerDistance;
SavedRightEyeLeftCorner = iter.CopyIterator();
}
}
if (SavedRightEyeRightCorner != null)
{
float NewRightEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfRightEye.X - iter.X, 2) + Math.Pow(landmarkFile.RightCornerOfRightEye.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightCornerOfRightEye.Z - iter.Z, 2));
if (NewRightEyeRightCornerDistance < RightEyeRightCornerDistance)
{
RightEyeRightCornerDistance = NewRightEyeRightCornerDistance;
SavedRightEyeRightCorner = iter.CopyIterator();
}
}
if (SavedLeftEyeLeftCorner != null)
{
float NewLeftEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLeftEye.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLeftEye.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLeftEye.Z - iter.Z, 2));
if (NewLeftEyeLeftCornerDistance < LeftEyeLeftCornerDistance)
{
LeftEyeLeftCornerDistance = NewLeftEyeLeftCornerDistance;
SavedLeftEyeLeftCorner = iter.CopyIterator();
}
}
if (SavedLeftCornerOfNose != null)
{
float NewLeftCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfNose.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Z - iter.Z, 2));
if (NewLeftCornerOfNoseDistance < LeftCornerOfNoseDistance)
{
LeftCornerOfNoseDistance = NewLeftCornerOfNoseDistance;
SavedLeftCornerOfNose = iter.CopyIterator();
}
}
if (SavedRightCornerOfNose != null)
{
float NewRightCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfNose.X - iter.X, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Z - iter.Z, 2));
if (NewRightCornerOfNoseDistance < RightCornerOfNoseDistance)
{
RightCornerOfNoseDistance = NewRightCornerOfNoseDistance;
SavedRightCornerOfNose = iter.CopyIterator();
}
}
if (SavedLeftCornerOfLips != null)
{
float NewLeftCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLips.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Z - iter.Z, 2));
if (NewLeftCornerOfLipsDistance < LeftCornerOfLipsDistance)
{
LeftCornerOfLipsDistance = NewLeftCornerOfLipsDistance;
SavedLeftCornerOfLips = iter.CopyIterator();
}
}
if (SavedRightCornerOfLips != null)
{
float NewRightCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfLips.X - iter.X, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Z - iter.Z, 2));
if (NewRightCornerOfLipsDistance < RightCornerOfLipsDistance)
{
RightCornerOfLipsDistance = NewRightCornerOfLipsDistance;
SavedRightCornerOfLips = iter.CopyIterator();
}
}
} while (iter.MoveToNext());
if(SavedNoseTip != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip, SavedNoseTip);
if (SavedRightEyeLeftCorner != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner, SavedRightEyeLeftCorner);
if (SavedLeftEyeRightCorner != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner, SavedLeftEyeRightCorner);
if (SavedLeftEyeLeftCorner != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeLeftCorner, SavedLeftEyeLeftCorner);
if (SavedRightEyeRightCorner != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeRightCorner, SavedRightEyeRightCorner);
if (SavedLeftCornerOfNose != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfNose, SavedLeftCornerOfNose);
if (SavedRightCornerOfNose != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfNose, SavedRightCornerOfNose);
if (SavedLeftCornerOfLips != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfLips, SavedLeftCornerOfLips);
if (SavedRightCornerOfLips != null)
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfLips, SavedRightCornerOfLips);
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
CORFile landmarkFile = ReadCORFile(p_Model.ModelFileFolder + p_Model.ModelFileName + ".cor");
//search for the closest point from the model to the read landmark
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Iterator in the model is not valid!");
Cl3DModel.Cl3DModelPointIterator SavedNoseTip = iter.CopyIterator();
float NoseTipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.NoseTip.X - SavedNoseTip.X,2)+Math.Pow(landmarkFile.NoseTip.Y - SavedNoseTip.Y,2)+Math.Pow(landmarkFile.NoseTip.Z - SavedNoseTip.Z,2));
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeRightCorner = iter.CopyIterator();
float LeftEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeRightCorner.X - SavedLeftEyeRightCorner.X, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Y - SavedLeftEyeRightCorner.Y, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Z - SavedLeftEyeRightCorner.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightEyeLeftCorner = iter.CopyIterator();
float RightEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeLeftCorner.X - SavedRightEyeLeftCorner.X, 2) + Math.Pow(landmarkFile.RightEyeLeftCorner.Y - SavedRightEyeLeftCorner.Y, 2) + Math.Pow(landmarkFile.RightEyeLeftCorner.Z - SavedRightEyeLeftCorner.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightEyeRightCorner = iter.CopyIterator();
float RightEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeRightCorner.X - SavedRightEyeRightCorner.X, 2) + Math.Pow(landmarkFile.RightEyeRightCorner.Y - SavedRightEyeRightCorner.Y, 2) + Math.Pow(landmarkFile.RightEyeRightCorner.Z - SavedRightEyeRightCorner.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeLeftCorner = iter.CopyIterator();
float LeftEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeLeftCorner.X - SavedLeftEyeLeftCorner.X, 2) + Math.Pow(landmarkFile.LeftEyeLeftCorner.Y - SavedLeftEyeLeftCorner.Y, 2) + Math.Pow(landmarkFile.LeftEyeLeftCorner.Z - SavedLeftEyeLeftCorner.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedLeftCornerOfNose = iter.CopyIterator();
float LeftCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfNose.X - SavedLeftCornerOfNose.X, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Y - SavedLeftCornerOfNose.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Z - SavedLeftCornerOfNose.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightCornerOfNose = iter.CopyIterator();
float RightCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfNose.X - SavedRightCornerOfNose.X, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Y - SavedRightCornerOfNose.Y, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Z - SavedRightCornerOfNose.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedLeftCornerOfLips = iter.CopyIterator();
float LeftCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLips.X - SavedLeftCornerOfLips.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Y - SavedLeftCornerOfLips.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Z - SavedLeftCornerOfLips.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightCornerOfLips = iter.CopyIterator();
float RightCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfLips.X - SavedRightCornerOfLips.X, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Y - SavedRightCornerOfLips.Y, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Z - SavedRightCornerOfLips.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedUpperLip = iter.CopyIterator();
float UpperLipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.UpperLip.X - SavedUpperLip.X, 2) + Math.Pow(landmarkFile.UpperLip.Y - SavedUpperLip.Y, 2) + Math.Pow(landmarkFile.UpperLip.Z - SavedUpperLip.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedBottomLip = iter.CopyIterator();
float BottomLipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.BottomLip.X - SavedBottomLip.X, 2) + Math.Pow(landmarkFile.BottomLip.Y - SavedBottomLip.Y, 2) + Math.Pow(landmarkFile.BottomLip.Z - SavedBottomLip.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeUpperEyelid = iter.CopyIterator();
float LeftEyeUpperEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeUpperEyelid.X - SavedLeftEyeUpperEyelid.X, 2) + Math.Pow(landmarkFile.LeftEyeUpperEyelid.Y - SavedLeftEyeUpperEyelid.Y, 2) + Math.Pow(landmarkFile.LeftEyeUpperEyelid.Z - SavedLeftEyeUpperEyelid.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedLeftEyeBottomEyelid = iter.CopyIterator();
float LeftEyeBottomEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeBottomEyelid.X - SavedLeftEyeBottomEyelid.X, 2) + Math.Pow(landmarkFile.LeftEyeBottomEyelid.Y - SavedLeftEyeBottomEyelid.Y, 2) + Math.Pow(landmarkFile.LeftEyeBottomEyelid.Z - SavedLeftEyeBottomEyelid.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightEyeUpperEyelid = iter.CopyIterator();
float RightEyeUpperEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeUpperEyelid.X - SavedRightEyeUpperEyelid.X, 2) + Math.Pow(landmarkFile.RightEyeUpperEyelid.Y - SavedRightEyeUpperEyelid.Y, 2) + Math.Pow(landmarkFile.RightEyeUpperEyelid.Z - SavedRightEyeUpperEyelid.Z, 2));
Cl3DModel.Cl3DModelPointIterator SavedRightEyeBottomEyelid = iter.CopyIterator();
float RightEyeBottomEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeBottomEyelid.X - SavedRightEyeBottomEyelid.X, 2) + Math.Pow(landmarkFile.RightEyeBottomEyelid.Y - SavedRightEyeBottomEyelid.Y, 2) + Math.Pow(landmarkFile.RightEyeBottomEyelid.Z - SavedRightEyeBottomEyelid.Z, 2));
do
{
float NewNoseTipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.NoseTip.X - iter.X, 2) + Math.Pow(landmarkFile.NoseTip.Y - iter.Y, 2) + Math.Pow(landmarkFile.NoseTip.Z - iter.Z, 2));
float NewLeftEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeRightCorner.X - iter.X, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftEyeRightCorner.Z - iter.Z, 2));
float NewRightEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeLeftCorner.X - iter.X, 2) + Math.Pow(landmarkFile.RightEyeLeftCorner.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightEyeLeftCorner.Z - iter.Z, 2));
float NewLeftEyeLeftCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeLeftCorner.X - iter.X, 2) + Math.Pow(landmarkFile.LeftEyeLeftCorner.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftEyeLeftCorner.Z - iter.Z, 2));
float NewRightEyeRightCornerDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeRightCorner.X - iter.X, 2) + Math.Pow(landmarkFile.RightEyeRightCorner.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightEyeRightCorner.Z - iter.Z, 2));
float NewLeftCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfNose.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfNose.Z - iter.Z, 2));
float NewRightCornerOfNoseDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfNose.X - iter.X, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightCornerOfNose.Z - iter.Z, 2));
float NewLeftCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftCornerOfLips.X - iter.X, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftCornerOfLips.Z - iter.Z, 2));
float NewRightCornerOfLipsDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightCornerOfLips.X - iter.X, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightCornerOfLips.Z - iter.Z, 2));
float NewUpperLipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.UpperLip.X - iter.X, 2) + Math.Pow(landmarkFile.UpperLip.Y - iter.Y, 2) + Math.Pow(landmarkFile.UpperLip.Z - iter.Z, 2));
float NewBottomLipDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.BottomLip.X - iter.X, 2) + Math.Pow(landmarkFile.BottomLip.Y - iter.Y, 2) + Math.Pow(landmarkFile.BottomLip.Z - iter.Z, 2));
float NewLeftEyeUpperEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeUpperEyelid.X - iter.X, 2) + Math.Pow(landmarkFile.LeftEyeUpperEyelid.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftEyeUpperEyelid.Z - iter.Z, 2));
float NewLeftEyeBottomEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.LeftEyeBottomEyelid.X - iter.X, 2) + Math.Pow(landmarkFile.LeftEyeBottomEyelid.Y - iter.Y, 2) + Math.Pow(landmarkFile.LeftEyeBottomEyelid.Z - iter.Z, 2));
float NewRightEyeUpperEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeUpperEyelid.X - iter.X, 2) + Math.Pow(landmarkFile.RightEyeUpperEyelid.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightEyeUpperEyelid.Z - iter.Z, 2));
float NewRightEyeBottomEyelidDistance = (float)Math.Sqrt(Math.Pow(landmarkFile.RightEyeBottomEyelid.X - iter.X, 2) + Math.Pow(landmarkFile.RightEyeBottomEyelid.Y - iter.Y, 2) + Math.Pow(landmarkFile.RightEyeBottomEyelid.Z - iter.Z, 2));
if (NewNoseTipDistance < NoseTipDistance)
{
NoseTipDistance = NewNoseTipDistance;
SavedNoseTip = iter.CopyIterator();
}
if (NewLeftEyeRightCornerDistance < LeftEyeRightCornerDistance)
{
LeftEyeRightCornerDistance = NewLeftEyeRightCornerDistance;
SavedLeftEyeRightCorner = iter.CopyIterator();
}
if (NewRightEyeLeftCornerDistance < RightEyeLeftCornerDistance)
{
RightEyeLeftCornerDistance = NewRightEyeLeftCornerDistance;
SavedRightEyeLeftCorner = iter.CopyIterator();
}
if (NewRightEyeRightCornerDistance < RightEyeRightCornerDistance)
{
RightEyeRightCornerDistance = NewRightEyeRightCornerDistance;
SavedRightEyeRightCorner = iter.CopyIterator();
}
if (NewLeftEyeLeftCornerDistance < LeftEyeLeftCornerDistance)
{
LeftEyeLeftCornerDistance = NewLeftEyeLeftCornerDistance;
SavedLeftEyeLeftCorner = iter.CopyIterator();
}
if (NewLeftCornerOfNoseDistance < LeftCornerOfNoseDistance)
{
LeftCornerOfNoseDistance = NewLeftCornerOfNoseDistance;
SavedLeftCornerOfNose = iter.CopyIterator();
}
if (NewRightCornerOfNoseDistance < RightCornerOfNoseDistance)
{
RightCornerOfNoseDistance = NewRightCornerOfNoseDistance;
SavedRightCornerOfNose = iter.CopyIterator();
}
if (NewLeftCornerOfLipsDistance < LeftCornerOfLipsDistance)
{
LeftCornerOfLipsDistance = NewLeftCornerOfLipsDistance;
SavedLeftCornerOfLips = iter.CopyIterator();
}
if (NewRightCornerOfLipsDistance < RightCornerOfLipsDistance)
{
RightCornerOfLipsDistance = NewRightCornerOfLipsDistance;
SavedRightCornerOfLips = iter.CopyIterator();
}
if (NewUpperLipDistance < UpperLipDistance)
{
UpperLipDistance = NewUpperLipDistance;
SavedUpperLip = iter.CopyIterator();
}
if (NewBottomLipDistance < BottomLipDistance)
{
BottomLipDistance = NewBottomLipDistance;
SavedBottomLip = iter.CopyIterator();
}
if (NewLeftEyeUpperEyelidDistance < LeftEyeUpperEyelidDistance)
{
LeftEyeUpperEyelidDistance = NewLeftEyeUpperEyelidDistance;
SavedLeftEyeUpperEyelid = iter.CopyIterator();
}
if (NewLeftEyeBottomEyelidDistance < LeftEyeBottomEyelidDistance)
{
LeftEyeBottomEyelidDistance = NewLeftEyeBottomEyelidDistance;
SavedLeftEyeBottomEyelid = iter.CopyIterator();
}
if (NewRightEyeUpperEyelidDistance < RightEyeUpperEyelidDistance)
{
RightEyeUpperEyelidDistance = NewRightEyeUpperEyelidDistance;
SavedRightEyeUpperEyelid = iter.CopyIterator();
}
if (NewRightEyeBottomEyelidDistance < RightEyeBottomEyelidDistance)
{
RightEyeBottomEyelidDistance = NewRightEyeBottomEyelidDistance;
SavedRightEyeBottomEyelid = iter.CopyIterator();
}
} while (iter.MoveToNext());
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip, SavedNoseTip);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner, SavedRightEyeLeftCorner);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner, SavedLeftEyeRightCorner);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeLeftCorner, SavedLeftEyeLeftCorner);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeRightCorner, SavedRightEyeRightCorner);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfNose, SavedLeftCornerOfNose);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfNose, SavedRightCornerOfNose);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftCornerOfLips, SavedLeftCornerOfLips);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightCornerOfLips, SavedRightCornerOfLips);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.UpperLip, SavedUpperLip);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.BottomLip, SavedBottomLip);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeUpperEyelid, SavedLeftEyeUpperEyelid);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeBottomEyelid, SavedLeftEyeBottomEyelid);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeUpperEyelid, SavedRightEyeUpperEyelid);
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeBottomEyelid, SavedRightEyeBottomEyelid);
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
SaveModel(p_Model);
string ParameterString = "\"" + p_Model.ModelFileFolder+paramFileName + ".tmp\" " + "0";
if(m_bAreaWeight)
{
ParameterString = "\"" + p_Model.ModelFileFolder + paramFileName + ".tmp\" " + "1";
}
ProcessStartInfo proces = new ProcessStartInfo(m_sRunApplication, ParameterString);
proces.WindowStyle = ProcessWindowStyle.Minimized;
Process ConformalProcess = System.Diagnostics.Process.Start(proces);
ConformalProcess.WaitForExit();
string FileName = p_Model.ModelFileFolder + paramFileName + "_AreaWeight.UV";
if(m_bAreaWeight)
{
FileName = p_Model.ModelFileFolder + paramFileName + "_NoAreaWeight.UV";
}
if (!File.Exists(FileName))
{
throw new Exception("Something goes wrong with calculation of UV parametrization for the file:" + p_Model.ModelFileName);
}
using (StreamReader FileStream = File.OpenText(FileName))
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
string line = "";
while ((line = FileStream.ReadLine()) != null)
{
if (line.Length == 0)
continue;
// UV verID=41496 U=0.0854425 V=0.224188
char[] delimiters = new char[] { ' ', '=' };
string[] parts = line.Split(delimiters, StringSplitOptions.RemoveEmptyEntries);
uint ID = UInt32.Parse(parts[2], System.Globalization.CultureInfo.InvariantCulture);
float U = Single.Parse(parts[4], System.Globalization.CultureInfo.InvariantCulture);
float V = Single.Parse(parts[6], System.Globalization.CultureInfo.InvariantCulture);
if (!iter.MoveToPoint(ID))
throw new Exception("Cannot move iterator to the position: " + ID.ToString());
iter.U = U;
iter.V = V;
}
FileStream.Close();
}
File.Delete(FileName);
File.Delete(p_Model.ModelFileFolder + paramFileName + ".tmp");
}
public static void MoveReferenceModelBasedOnCorelatedPoints(List<Cl3DModel.Cl3DModelPointIterator> ReferenceModel, List<Cl3DModel.Cl3DModelPointIterator> MovingModel, Cl3DModel Model)
{
Matrix RotationMatrix = null;
Matrix TranslationMatrix = null;
ClTools.CalculateRotationAndTranslation(ReferenceModel, MovingModel, out RotationMatrix, out TranslationMatrix);
Cl3DModel.Cl3DModelPointIterator iterToMove = Model.GetIterator();
do
{
Matrix V = RotationMatrix * iterToMove + TranslationMatrix;
iterToMove.X = (float)V[0, 0];
iterToMove.Y = (float)V[1, 0];
iterToMove.Z = (float)V[2, 0];
} while (iterToMove.MoveToNext());
}
public static void FindClosestPointInModel(Cl3DModel p_Model, float p_X, float p_Y, float p_Z, out Cl3DModel.Cl3DModelPointIterator p_ClosestPoint, out float p_Distance)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Iterator in model is not valid");
p_ClosestPoint = iter.CopyIterator();
p_Distance = (float)Math.Sqrt(Math.Pow(iter.X - p_X, 2) + Math.Pow(iter.Y - p_Y, 2) + Math.Pow(iter.Z - p_Z, 2));
Cl3DModel.Cl3DModelPointIterator closest = iter.CopyIterator();
do
{
float distance = (float)Math.Sqrt(Math.Pow(iter.X - p_X, 2) + Math.Pow(iter.Y - p_Y, 2) + Math.Pow(iter.Z - p_Z, 2));
if (distance < p_Distance)
{
p_Distance = distance;
p_ClosestPoint = iter.CopyIterator();
}
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator RightEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner.ToString());
Cl3DModel.Cl3DModelPointIterator LeftEye = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner.ToString());
Cl3DModel.Cl3DModelPointIterator[,] Map = null;
int Width, Height, Xoffset, Yoffset;
ClTools.CreateGridBasedOnRangeValues(p_Model, out Map, out Width, out Height, out Xoffset, out Yoffset);
int RightEyeX = RightEye.RangeImageX - Xoffset;
int RightEyeY = RightEye.RangeImageY - Yoffset;
int LeftEyeX = LeftEye.RangeImageX - Xoffset;
int LeftEyeY = LeftEye.RangeImageY - Yoffset;
int MiddleX = (RightEyeX + LeftEyeX) / 2;
int MiddleY = (RightEyeY + LeftEyeY) / 2;
Cl3DModel.Cl3DModelPointIterator MiddlePoint = Map[MiddleX, MiddleY];
p_Model.AddSpecificPoint(Cl3DModel.eSpecificPoints.UnspecifiedPoint, MiddlePoint);
List<Cl3DModel.Cl3DModelPointIterator> Neighborhood = null;
ClTools.GetNeighborhoodWithGeodesicDistance(out Neighborhood, MiddlePoint, m_fEyesPartRadious);
foreach (Cl3DModel.Cl3DModelPointIterator point in Neighborhood)
point.AlreadyVisited = true;
MiddlePoint.AlreadyVisited = true;
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
if (!iter.IsValid())
break;
if (!iter.AlreadyVisited)
iter = p_Model.RemovePointFromModel(iter);
else
if (!iter.MoveToNext())
break;
} while (true);
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator EyePoint1 = null;
Cl3DModel.Cl3DModelPointIterator EyePoint2 = null;
Cl3DModel.Cl3DModelPointIterator NosePoint = null;
if (!p_Model.GetSpecificPoint(LeftEyeLabel, ref EyePoint1))
throw new Exception("Cannot find specific point labeled as: " + LeftEyeLabel);
if (!p_Model.GetSpecificPoint(RightEyeLabel, ref EyePoint2))
throw new Exception("Cannot find specific point labeled as: " + RightEyeLabel);
if (!p_Model.GetSpecificPoint(NoseLabel, ref NosePoint))
throw new Exception("Cannot find specific point labeled as: " + NoseLabel);
Matrix GenModel = new Matrix(3, 3);
GenModel[0, 0] = 0;
GenModel[1, 0] = 0; // nose
GenModel[2, 0] = 0;
GenModel[0, 1] = -22f;
GenModel[1, 1] = 34f; // left eye
GenModel[2, 1] = -35f;
GenModel[0, 2] = 22f;
GenModel[1, 2] = 34f; // right eye
GenModel[2, 2] = -35f;
Matrix Model = new Matrix(3, 3);
Cl3DModel.Cl3DModelPointIterator point = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
Model[0, 0] = point.X;
Model[1, 0] = point.Y; // nose
Model[2, 0] = point.Z;
point = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.LeftEyeRightCorner);
Model[0, 1] = point.X;
Model[1, 1] = point.Y; // left eye
Model[2, 1] = point.Z;
point = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.RightEyeLeftCorner);
Model[0, 2] = point.X;
Model[1, 2] = point.Y; // right eye
Model[2, 2] = point.Z;
Matrix rotationMatrix = null;
Matrix translationMatrix = null;
ClTools.CalculateRotationAndTranslation(GenModel, Model, out rotationMatrix, out translationMatrix);
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
Matrix V = rotationMatrix * iter + translationMatrix;
iter.X = (float)V[0, 0];
iter.Y = (float)V[1, 0];
iter.Z = (float)V[2, 0];
} while (iter.MoveToNext());
point = p_Model.GetSpecificPoint(Cl3DModel.eSpecificPoints.NoseTip);
float x = point.X;
float y = point.Y;
float z = point.Z;
iter = p_Model.GetIterator();
do
{
iter.X = iter.X - x;
iter.Y = iter.Y - y;
iter.Z = iter.Z - z;
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
// if (!(p_Model.ModelType == "abs" || p_Model.ModelType == "binaryModel" || p_Model.ModelType == "model"))
// throw new Exception("Remove Holes based on Range Image works only for abs, binaryModel and model files");
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Model iterator not Valid");
int MinX;
int MaxX;
int MinY;
int MaxY;
MinX = iter.RangeImageX;
MaxX = iter.RangeImageX;
MinY = iter.RangeImageY;
MaxY = iter.RangeImageY;
do
{
if (MinX > iter.RangeImageX)
MinX = iter.RangeImageX;
if (MaxX < iter.RangeImageX)
MaxX = iter.RangeImageX;
if (MinY > iter.RangeImageY)
MinY = iter.RangeImageY;
if (MaxY < iter.RangeImageY)
MaxY = iter.RangeImageY;
} while (iter.MoveToNext());
int Width = (MaxX - MinX)+1;
int Height = (MaxY - MinY) +1;
Cl3DModel.Cl3DModelPointIterator[,] map = new Cl3DModel.Cl3DModelPointIterator[Width, Height];
iter = p_Model.GetIterator();
do
{
map[iter.RangeImageX-MinX, iter.RangeImageY-MinY] = iter.CopyIterator();
foreach (Cl3DModel.Cl3DModelPointIterator pts in iter.GetListOfNeighbors())
iter.RemoveNeighbor(pts);
} while (iter.MoveToNext());
for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
if (map[x, y] == null)
continue;
if (x - 1 >= 0 && y + 1 < Height && map[x - 1, y + 1] != null)
map[x, y].AddNeighbor(map[x - 1, y + 1]);
if (x - 1 >= 0 && map[x - 1, y] != null)
map[x, y].AddNeighbor(map[x - 1, y]);
if (x - 1 >= 0 && y - 1 >= 0 && map[x - 1, y - 1] != null)
map[x, y].AddNeighbor(map[x - 1, y - 1]);
if (y - 1 >= 0 && map[x, y - 1] != null)
map[x, y].AddNeighbor(map[x, y - 1]);
if (x + 1 < Width && y - 1 >= 0 && map[x + 1, y - 1] != null)
map[x, y].AddNeighbor(map[x + 1, y - 1]);
if (x + 1 < Width && map[x + 1, y] != null)
map[x, y].AddNeighbor(map[x + 1, y]);
if (x + 1 < Width && y + 1 < Height && map[x + 1, y + 1] != null)
map[x, y].AddNeighbor(map[x + 1, y + 1]);
if (y + 1 < Height && map[x, y + 1] != null)
map[x, y].AddNeighbor(map[x, y + 1]);
}
}
}
public static void sortModelPoints(Cl3DModel p_model, out Dictionary<int, List<Cl3DModel.Cl3DModelPointIterator>> p_SortedDictionary)
{
p_SortedDictionary = new Dictionary<int, List<Cl3DModel.Cl3DModelPointIterator>>();
Cl3DModel.Cl3DModelPointIterator iter = p_model.GetIterator();
do
{
List<Cl3DModel.Cl3DModelPointIterator> SameDistancePoints = null;
int dist = (int)Math.Sqrt(Math.Pow(iter.X, 2) + Math.Pow(iter.Y, 2) + Math.Pow(iter.Z, 2));
if (!p_SortedDictionary.TryGetValue(dist, out SameDistancePoints))
{
SameDistancePoints = new List<Cl3DModel.Cl3DModelPointIterator>();
p_SortedDictionary.Add(dist, SameDistancePoints);
}
SameDistancePoints.Add(iter.CopyIterator());
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
do
{
double value = 0;
if (!iter.GetSpecificValue(m_SpecificValue, out value))
{
iter.MoveToNext();
continue;
}
if (value > m_ThresholdValue)
{
iter = p_Model.RemovePointFromModel(iter);
}
else
{
iter.MoveToNext();
}
} while (iter.IsValid());
}
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 void CreateGridBasedOnRangeValues(Cl3DModel p_Model, out Cl3DModel.Cl3DModelPointIterator[,] p_Map, out int p_Width, out int p_Height, out int p_OffsetX, out int p_OffsetY)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Model iterator is not Valid");
int MinX;
int MaxX;
int MinY;
int MaxY;
MinX = iter.RangeImageX;
MaxX = iter.RangeImageX;
MinY = iter.RangeImageY;
MaxY = iter.RangeImageY;
do
{
if (MinX > iter.RangeImageX)
MinX = iter.RangeImageX;
if (MaxX < iter.RangeImageX)
MaxX = iter.RangeImageX;
if (MinY > iter.RangeImageY)
MinY = iter.RangeImageY;
if (MaxY < iter.RangeImageY)
MaxY = iter.RangeImageY;
} while (iter.MoveToNext());
p_Width = (MaxX - MinX) + 1;
p_Height = (MaxY - MinY) + 1;
p_Map = new Cl3DModel.Cl3DModelPointIterator[p_Width, p_Height];
p_OffsetX = MinX;
p_OffsetY = MinY;
iter = p_Model.GetIterator();
do
{
p_Map[iter.RangeImageX - MinX, iter.RangeImageY - MinY] = iter.CopyIterator();
} while (iter.MoveToNext());
}
void SaveModel(Cl3DModel p_Model)
{
string name = p_Model.ModelFileFolder + paramFileName+".tmp";
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
using (TextWriter tw = new StreamWriter(name, false))
{
uint vertexNO = 1;
Dictionary<uint, uint> MapVertexNo = new Dictionary<uint, uint>();
do
{
string line = "v " + iter.X.ToString(System.Globalization.CultureInfo.InvariantCulture) +
" " + iter.Y.ToString(System.Globalization.CultureInfo.InvariantCulture) +
" " + iter.Z.ToString(System.Globalization.CultureInfo.InvariantCulture) +
" " + iter.PointID.ToString(System.Globalization.CultureInfo.InvariantCulture);
tw.WriteLine(line);
MapVertexNo.Add(iter.PointID, vertexNO++);
} while (iter.MoveToNext());
// --- create faces
iter = p_Model.GetIterator();
do
{
iter.AlreadyVisited = true;
List<Cl3DModel.Cl3DModelPointIterator> neighbors = iter.GetListOfNeighbors();
uint MainPointID;
if (!MapVertexNo.TryGetValue(iter.PointID, out MainPointID))
throw new Exception("Cannot find point ID in the ID dictionary: " + iter.PointID);
uint point1 = 0;
uint point2 = 0;
uint point3 = 0;
foreach (Cl3DModel.Cl3DModelPointIterator point in neighbors)
{
//if (point.AlreadyVisited)
// continue;
if (point.RangeImageY == iter.RangeImageY && point.RangeImageX > iter.RangeImageX) // first point
{
if (!MapVertexNo.TryGetValue(point.PointID, out point1))
throw new Exception("Cannot find point ID in the ID dictionary: " + iter.PointID);
}
if (point.RangeImageY > iter.RangeImageY && point.RangeImageX > iter.RangeImageX) // second point
{
if (!MapVertexNo.TryGetValue(point.PointID, out point2))
throw new Exception("Cannot find point ID in the ID dictionary: " + iter.PointID);
}
if (point.RangeImageY > iter.RangeImageY && point.RangeImageX == iter.RangeImageX) // third point
{
if (!MapVertexNo.TryGetValue(point.PointID, out point3))
throw new Exception("Cannot find point ID in the ID dictionary: " + iter.PointID);
}
}
string line = "";
if (point1 != 0 && point2 != 0)
{
line += "f " + MainPointID.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point2.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point1.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
if (point3 != 0)
{
line += "f " + MainPointID.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point3.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point2.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
}
}
else if (point1 != 0 && point3 != 0)
{
line += "f " + MainPointID.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point3.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point1.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
}
else if (point2 != 0 && point3 != 0)
{
line += "f " + MainPointID.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point3.ToString(System.Globalization.CultureInfo.InvariantCulture) + " " +
point2.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n";
}
tw.Write(line);
} while (iter.MoveToNext());
tw.Close();
}
}
public static void CreateGridBasedOnRealXY(Cl3DModel p_Model, out List<Cl3DModel.Cl3DModelPointIterator>[,] p_Map, out int p_Width, out int p_Height, out float p_MinusOffsetX, out float p_MinusOffsetY)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if (!iter.IsValid())
throw new Exception("Model iterator not Valid");
float MinX;
float MaxX;
float MinY;
float MaxY;
MinX = iter.X;
MaxX = iter.X;
MinY = iter.Y;
MaxY = iter.Y;
do
{
if (MinX > iter.X)
MinX = iter.X;
if (MaxX < iter.X)
MaxX = iter.X;
if (MinY > iter.Y)
MinY = iter.Y;
if (MaxY < iter.Y)
MaxY = iter.Y;
} while (iter.MoveToNext());
p_Width = (int)(MaxX - MinX) + 1;
p_Height = (int)(MaxY - MinY) + 1;
p_Map = new List<Cl3DModel.Cl3DModelPointIterator>[p_Width, p_Height];
p_MinusOffsetX = MinX;
p_MinusOffsetY = MinY;
iter = p_Model.GetIterator();
do
{
int x = (int)(iter.X - MinX);
int y = (int)(iter.Y - MinY);
if (p_Map[x, y] == null)
p_Map[x, y] = new List<Cl3DModel.Cl3DModelPointIterator>();
p_Map[x, y].Add(iter.CopyIterator());
} while (iter.MoveToNext());
}
protected override void Algorithm(ref Cl3DModel p_Model)
{
Cl3DModel.Cl3DModelPointIterator iter = p_Model.GetIterator();
if(iter.IsValid())
{
do
{
double H;
double K;
double ShapeIndex;
if (!iter.GetSpecificValue("Gaussian_25", out K))
continue;
if (!iter.GetSpecificValue("Mean_25", out H))
continue;
if (H < 0 && K > m_ThresholdK) // Nose
{
iter.Color = Color.Green;
}
}
while (iter.MoveToNext());
}
}
public static float doICP(Cl3DModel p_referenceModel, Cl3DModel p_modelToMove)
{
// utworzenie skorelowanych punktow wszystko do reference model dopasowane
List<Cl3DModel.Cl3DModelPointIterator> ReferenceModelPoints = new List<Cl3DModel.Cl3DModelPointIterator>();
List<Cl3DModel.Cl3DModelPointIterator> ConnectedPointsWithreference = new List<Cl3DModel.Cl3DModelPointIterator>();
Cl3DModel.Cl3DModelPointIterator RefModelIter = p_referenceModel.GetIterator();
List<Cl3DModel.Cl3DModelPointIterator>[,] MapOfMovingModel = null;
int width = 0;
int height = 0;
float MinusOffsetX = 0;
float MinusOffsetY = 0;
ClTools.CreateGridBasedOnRealXY(p_modelToMove, out MapOfMovingModel, out width, out height, out MinusOffsetX, out MinusOffsetY);
do
{
//Search for the closes point in next model
int RefX = (int)(RefModelIter.X - MinusOffsetX);
int RefY = (int)(RefModelIter.Y - MinusOffsetY);
if (RefX >= width)
RefX = width - 1;
if (RefY >= height)
RefY = height - 1;
if (RefX < 0)
RefX = 0;
if (RefY < 0)
RefY = 0;
List<Cl3DModel.Cl3DModelPointIterator> ListToCheck = new List<Cl3DModel.Cl3DModelPointIterator>();
if (MapOfMovingModel[RefX, RefY] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX, RefY])
ListToCheck.Add(iter);
}
if (RefX - 1 >= 0 && RefY - 1 >= 0)
if (MapOfMovingModel[RefX - 1, RefY - 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX - 1, RefY - 1])
ListToCheck.Add(iter);
}
if (RefX - 1 >= 0)
if (MapOfMovingModel[RefX - 1, RefY] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX - 1, RefY])
ListToCheck.Add(iter);
}
if (RefX - 1 >= 0 && RefY + 1 < height)
if (MapOfMovingModel[RefX - 1, RefY + 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX - 1, RefY + 1])
ListToCheck.Add(iter);
}
if (RefY - 1 >= 0)
if (MapOfMovingModel[RefX, RefY - 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX, RefY - 1])
ListToCheck.Add(iter);
}
if (RefY + 1 < height)
if (MapOfMovingModel[RefX, RefY + 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX, RefY + 1])
ListToCheck.Add(iter);
}
if (RefY - 1 >= 0 && RefX + 1 < width)
if (MapOfMovingModel[RefX + 1, RefY - 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX + 1, RefY - 1])
ListToCheck.Add(iter);
}
if (RefX + 1 < width)
if (MapOfMovingModel[RefX + 1, RefY] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX + 1, RefY])
ListToCheck.Add(iter);
}
if (RefY + 1 < height && RefX + 1 < width)
if (MapOfMovingModel[RefX + 1, RefY + 1] != null)
{
foreach (Cl3DModel.Cl3DModelPointIterator iter in MapOfMovingModel[RefX + 1, RefY + 1])
ListToCheck.Add(iter);
}
float minValue = 0;
Cl3DModel.Cl3DModelPointIterator ClosesPoint = null;
foreach (Cl3DModel.Cl3DModelPointIterator iterToCheck in ListToCheck)
{
if (ClosesPoint == null)
{
minValue = RefModelIter - iterToCheck;
ClosesPoint = iterToCheck.CopyIterator();
}
else
{
float currDistance = RefModelIter - iterToCheck;
if (currDistance < minValue)
{
minValue = currDistance;
ClosesPoint = iterToCheck.CopyIterator();
}
}
}
if (ClosesPoint == null)
continue;
ConnectedPointsWithreference.Add(ClosesPoint);
ReferenceModelPoints.Add(RefModelIter.CopyIterator());
} while (RefModelIter.MoveToNext() && RefModelIter.MoveToNext() && RefModelIter.MoveToNext() && RefModelIter.MoveToNext());
Matrix RotationMatrix = null;
Matrix TranslationMatrix = null;
ClTools.CalculateRotationAndTranslation(ReferenceModelPoints, ConnectedPointsWithreference, out RotationMatrix, out TranslationMatrix);
Cl3DModel.Cl3DModelPointIterator iterToMove = p_modelToMove.GetIterator();
do
{
Matrix V = RotationMatrix * iterToMove + TranslationMatrix;
iterToMove.X = (float)V[0, 0];
iterToMove.Y = (float)V[1, 0];
iterToMove.Z = (float)V[2, 0];
} while (iterToMove.MoveToNext());
float Distance = 0;
for (int i = 0; i < ConnectedPointsWithreference.Count; i++)
{
Distance += ConnectedPointsWithreference[i] - ReferenceModelPoints[i];
}
return Distance;
}