public static Matrix3d Rotation_ToOriginAxes(this Matrix3d mat, PointCloudVertices cloudOrigin)
{
PointCloudVertices target = new PointCloudVertices();
target.Add(new Vertex(1, 0, 0));
target.Add(new Vertex(0, 1, 0));
target.Add(new Vertex(0, 0, 1));
return(mat.RotationCoordinateChange(cloudOrigin, target));
}
private PointCloudVertices CalculateMergedPoints(PointCloudVertices pResult, PointCloudVertices pTarget)
{
PointCloudVertices pMerged = PointCloudVertices.CloneVertices(pResult);
for (int i = 0; i < pTarget.Count; i++)
{
pMerged.Add(pTarget[i]);
}
//for(int i = 0; i < pMerged.Count; i++)
//{
// Vertex vMerged = pMerged[i];
// Vertex vSource = pTarget[i];
// if(vMerged.Vector.X == vSource.Vector.X && vMerged.Vector.Y == vSource.Vector.Y)
// {
// if(vMerged.Vector.Z == 0)
// {
// vMerged.Vector.Z = vSource.Vector.Z;
// }
// if (vMerged.Color == System.Drawing.Color.Black)
// {
// vMerged.Color = vSource.Color;
// }
// }
//}
return(pMerged);
}
public PointCloudVertices FindNearest_BruteForceOld(PointCloudVertices vSource, PointCloudVertices vTarget)
{
PointCloudVertices nearestNeighbours = new PointCloudVertices();
int iMax = 10;
PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(vTarget);
for (int i = 0; i < vSource.Count; i++)
{
//BuildKDTree_Standard(tempTarget);
Vertex p = vSource[i];
// Perform a nearest neighbour search around that point.
KDTreeRednaxela.NearestNeighbour <EllipseWrapper> pIter = null;
pIter = KdTree_Rednaxela.FindNearest_EuclidDistance(new float[] { Convert.ToSingle(p.Vector.X), Convert.ToSingle(p.Vector.Y), Convert.ToSingle(p.Vector.Z) }, iMax, -1);
while (pIter.MoveNext())
{
// Get the ellipse.
//var pEllipse = pIter.Current;
EllipseWrapper wr = pIter.CurrentPoint;
nearestNeighbours.Add(wr.Vertex);
tempTarget.RemoveAt(pIter.CurrentIndex);
break;
}
}
return(nearestNeighbours);
}
public PointCloudVertices FindNearest_Stark(PointCloudVertices source, PointCloudVertices target)
{
KdTree_Stark.ResetSearch();
PointCloudVertices result = new PointCloudVertices();
List <int> pointsFound = new List <int>();
List <List <int> > searchdoubleList = new List <List <int> >(3);
for (int i = source.Count - 1; i >= 0; i--)
//for (int i = 0; i < source.Count ; i ++)
{
//int indexNearest = KdTree_Stark.FindNearest(source[i]);
int indexNearest = KdTree_Stark.FindNearestAdapted(source[i], pointsFound);
//result.Add(target[indexNearest]);
if (!pointsFound.Contains(indexNearest))
{
pointsFound.Add(indexNearest);
result.Add(target[indexNearest]);
}
else
{
bool bfound = false;
for (int j = 0; j < KDTree_Stark.LatestSearchResults.Count; j++)
{
int newIndex = KDTree_Stark.LatestSearchResults[j].Key;
if (!pointsFound.Contains(newIndex))
{
bfound = true;
pointsFound.Add(newIndex);
result.Add(target[newIndex]);
break;
}
}
if (!bfound)
{
source.RemoveAt(i);
}
}
}
return(result);
}
/// <summary>
/// Reads only position and color information (No normals, texture, triangles etc. etc)
/// </summary>
/// <param name="fileOBJ"></param>
/// <param name="myNewModel"></param>
public static PointCloudVertices ReadObjFile_ToPointCloud(string fileOBJ)
{
PointCloudVertices myPCL = new PointCloudVertices();
string line = string.Empty;
int indexInModel = -1;
try
{
using (StreamReader streamReader = new StreamReader(fileOBJ))
{
//Part p = new Part();
Vertex vertex = new Vertex();
//myNewModel.Part = new List<Part>();
while (!streamReader.EndOfStream)
{
line = streamReader.ReadLine().Trim();
while (line.EndsWith("\\"))
{
line = line.Substring(0, line.Length - 1) + streamReader.ReadLine().Trim();
}
string str1 = GlobalVariables.TreatLanguageSpecifics(line);
string[] strArrayRead = str1.Split();
if (strArrayRead.Length >= 0)
{
switch (strArrayRead[0].ToLower())
{
//case "mtllib":
// if (strArrayRead.Length < 2)
// {
// System.Windows.Forms.MessageBox.Show("Error reading obj file in line : " + line);
// }
// myNewModel.GetTexture(strArrayRead[1], fileOBJ);
// break;
case "v": //Vertex
vertex = HelperReadVertex(strArrayRead);
indexInModel++;
vertex.IndexInModel = indexInModel;
myPCL.Add(vertex);
break;
}
}
}
}
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error reading obj file - Vertices: " + line + " ; " + err.Message);
}
return(myPCL);
}
private double CheckNewPointDistance(int iPoint, Matrix4d myMatrix, PointCloudVertices pointsTarget, PointCloudVertices pointsSource)
{
Vertex p1 = pointsTarget[iPoint];
Vertex p2 = pointsSource[iPoint];
PointCloudVertices tempPointReference = new PointCloudVertices();
PointCloudVertices tempPointToBeMatched = new PointCloudVertices();
tempPointReference.Add(p1);
tempPointToBeMatched.Add(p2);
PointCloudVertices tempPointRotate = MathUtilsVTK.TransformPoints(tempPointToBeMatched, myMatrix);
double dist = PointCloudVertices.MeanDistance(tempPointReference, tempPointRotate);
return(dist);
}
private static Matrix4d TryoutNewPoint(int iPoint, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, PointCloudVertices pointsTargetTrial, PointCloudVertices pointsSourceTrial, LandmarkTransform myLandmarkTransform)
{
Vertex p1 = pointsTarget[iPoint];
Vertex p2 = pointsSource[iPoint];
pointsTargetTrial.Add(p1);
pointsSourceTrial.Add(p2);
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSourceTrial), PointCloudVertices.ToVectors(pointsTargetTrial), myLandmarkTransform);//, accumulate);
Matrix4d myMatrix = myLandmarkTransform.Matrix;
return(myMatrix);
}
public PointCloudVertices FindNearest_BruteForce(PointCloudVertices source, PointCloudVertices target)
{
PointCloudVertices result = new PointCloudVertices();
List <int> indicesTargetFound = new List <int>();
PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(target);
for (int i = source.Count - 1; i >= 0; i--)
{
BuildKDTree_Stark(tempTarget);
int indexNearest = KdTree_Stark.FindNearest(source[i]);
result.Add(target[indexNearest]);
tempTarget.RemoveAt(indexNearest);
}
return(result);
}
public static PointCloudVertices TransformPoints(this Matrix4d matrix, PointCloudVertices a)
{
if (a == null || a.Count == 0)
{
return(null);
}
PointCloudVertices b = new PointCloudVertices();
for (int i = 0; i < a.Count; i++)
{
Vector3d p1 = a[i].Vector;
Vector3d pointReturn = matrix.TransformVector3d(p1);
b.Add(new Vertex(i, pointReturn, a[i].Color));
}
return(b);
}
public static PointCloudVertices ExtractPoints(PointCloudVertices points, List <int> indices)
{
try
{
PointCloudVertices output = new PointCloudVertices();
for (int i = 0; i < indices.Count; i++)
{
int indexPoint = indices[i];
Vertex p = points[indexPoint];
output.Add(p);
}
return(output);
}
catch (Exception err)
{
MessageBox.Show("Error in RandomUtils.ExtractPoints " + err.Message);
return(null);
}
}
public PointCloudVertices FindNearest_Rednaxela_Parallel(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold)
{
this.pointsSource = mypointsSource;
this.pointsTarget = mypointsTarget;
int indexI = -1;
try
{
PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource);
PointCloudVertices pointsResult = new PointCloudVertices();
FindNearest_Rednaxela_HelperParallel();
for (int i = 0; i < pointsSource.Count; i++)
{
indexI = i;
Vertex v = pointsTarget[pointsSource[i].IndexKDTreeTarget];
pointsResult.Add(v);
}
this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource);
pointsSource = pointsSourceNew;
if (pointsSource.Count != pointsResult.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString());
//return false;
}
mypointsSource = this.pointsSource;
return(pointsResult);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message);
return(null);
}
}
public PointCloudVertices FindNearest_Rednaxela_ExcludePoints(PointCloudVertices source, PointCloudVertices target, bool findNormals)
{
NumberOfNeighboursToSearch = source.Count;
FindNearest_NormalsCheck_Rednaxela(source, findNormals);
PointCloudVertices result = new PointCloudVertices();
float totalDistance = 0;
int i = -1;
int loopMax = source.Count;
if (target.Count < loopMax)
{
loopMax = target.Count;
for (i = source.Count - 1; i >= loopMax; i--)
{
source.RemoveAt(i);
}
}
try
{
for (i = 0; i < loopMax; i++)
{
totalDistance += source[i].KDTreeSearch[0].Value;
result.Add(target[source[i].KDTreeSearch[0].Key]);
}
}
catch
{
System.Windows.Forms.MessageBox.Show("Error setting search key at: " + i.ToString());
}
MeanDistance = totalDistance / loopMax;
return(result);
}
public PointCloudVertices ToPointCloudVertices()
{
PointCloudVertices points = new PointCloudVertices();
for (int i = 0; i < this.Vectors.GetLength(0); i++)
{
points.Add(new Vertex(i, this.Vectors[i].X, this.Vectors[i].Y, this.Vectors[i].Z));
}
if (this.Colors != null)
{
for (int i = 0; i < this.Colors.GetLength(0); i++)
{
Vector3 col = this.Colors[i];
points[i].Color = System.Drawing.Color.FromArgb(255, Convert.ToByte(col.X * 255f), Convert.ToByte(col.Y * 255f), Convert.ToByte(col.Z * 255f));
}
}
points.Path = this.Path;
points.FilaName = this.FileNameLong;
return(points);
}
public PointCloudVertices FindNearest_Rednaxela(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold)
{
this.pointsSource = mypointsSource;
this.pointsTarget = mypointsTarget;
int indexI = -1;
try
{
PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource);
PointCloudVertices pointsResult = new PointCloudVertices();
FindNearest_Rednaxela_Helper();
for (int i = 0; i < listNeighbours.Count; i++)
{
indexI = i;
List <Neighbours> mySublist = listNeighbours[i];
mySublist.Sort(new NeighboursComparer());
Vertex v = pointsTarget[mySublist[0].IndexTarget];
pointsResult.Add(v);
}
if (Normals_RemovePoints)
{
listNeighbours.Sort(new NeighboursListComparer());
int minPoints = Math.Min(10, listNeighbours.Count - 1);
for (int i = 0; i < listNeighbours.Count; i++)
{
List <Neighbours> mySublist = listNeighbours[i];
int indexInsert = mySublist[0].IndexSource;
Vertex vSource = pointsSource[mySublist[0].IndexSource];
Vertex vTarget = pointsTarget[mySublist[0].IndexTarget];
pointsSourceNew[indexInsert] = vSource;
pointsResult[indexInsert] = vTarget;
if (i > minPoints)
{
if (mySublist[0].Angle > angleThreshold)
{
pointsSourceNew[indexInsert] = null;
pointsResult[indexInsert] = null;
}
}
}
for (int i = pointsSource.Count - 1; i >= 0; i--)
{
if (pointsSourceNew[i] == null)
{
pointsSourceNew.RemoveAt(i);
pointsResult.RemoveAt(i);
}
}
Debug.WriteLine("Remaining points : " + (pointsResult.Count * 1.0 / pointsTarget.Count * 100).ToString("0.00") + " %");
}
this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource);
pointsSource = pointsSourceNew;
if (pointsSource.Count != pointsResult.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString());
//return false;
}
mypointsSource = this.pointsSource;
return(pointsResult);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message);
return(null);
}
}
/// <summary>
/// reads the OBJ file ONLY with the special format used also in the write_OBJ method
/// </summary>
/// <param name="path"></param>
/// <param name="fileNameShort"></param>
/// <param name="depthData"></param>
/// <returns></returns>
public static PointCloudVertices FromObjFile(string path, string fileNameShort)
{
string fileName = path + "\\" + fileNameShort;
if (!System.IO.File.Exists(fileName))
{
System.Diagnostics.Debug.WriteLine("File does not exist: ");
return(null);
}
double[] p3D = new double[3];
System.Collections.ArrayList lineList = new System.Collections.ArrayList();
PointCloudVertices pointCloud = new PointCloudVertices();
//int numberOfDepthPointsNonZero = -1;
int i = 0;
int startIndex = 0;
try
{
string[] lines = System.IO.File.ReadAllLines(fileName);
//ignore the comment lines
for (i = 0; i < lines.Length; i++)
{
if (!lines[i].Contains('#'))
{
startIndex = i;
break;
}
}
for (i = startIndex; i < lines.GetLength(0); i++)
//for (i = lines.Length -1 ; i >= startIndex + 1; i--)
{
string[] arrStr1 = lines[i].Split(new Char[] { ' ' });
if (lines[i].Contains('#') || arrStr1.Length < 4)
{
//ignore empty and comment lines
continue;
}
if (arrStr1[0] == "vn")
{
//ignore vector normals for now
}
if (arrStr1[0] == "v")
{
if (arrStr1.Length < 7)
{
System.Windows.Forms.MessageBox.Show("Error reading file " + fileNameShort + " in line i");
}
else
{
for (int j = 0; j < 3; j++)
{
p3D[j] = Convert.ToSingle(arrStr1[j + 1], CultureInfo);
}
Vector3d vec = new Vector3d(p3D[0], p3D[1], p3D[2]);
float[] color = new float[4] {
Convert.ToSingle(arrStr1[4], CultureInfo), Convert.ToSingle(arrStr1[5], CultureInfo), Convert.ToSingle(arrStr1[6], CultureInfo), 1f
};
System.Drawing.Color c = System.Drawing.Color.White;
c = c.FromFloatsARGB(color[3], color[0], color[1], color[2]);
Vertex v = new Vertex(vec, c);
pointCloud.Add(v);
//if (vec.Z > 0)
//{
// numberOfDepthPointsNonZero++;
//}
}
}
}
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Read_OBJ: read error in file : " + fileNameShort + " : at line: " + i.ToString() + " ; " + err.Message);
}
return(pointCloud);
}
