public PointCloud FindClosestPointCloud_Parallel(PointCloud source)
{
this.source = source;
this.ResetTaken();
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
System.Threading.Tasks.Parallel.For(0, source.Count, i =>
//for (int i = 0; i < source.Count; i++)
{
VertexKDTree vSource = new VertexKDTree(source.Vectors[i], i);
int nearest_index = 0;
float nearest_distance = 0f;
VertexKDTree vTargetFound = FindClosestPoint(vSource, ref nearest_distance, ref nearest_index);
//resultArray[i] = vTargetFound.Clone();
resultArray[i] = vTargetFound;
});
List <VertexKDTree> resultList = new List <VertexKDTree>(resultArray);
result = PointCloud.FromListVertexKDTree(resultList);
return(result);
}
/// <summary>
/// Return the KDTreeNode that 'vertex' belongs to according to the split axis.
/// </summary>
/// <param name="vertex">3D vertex to bin</param>
/// <returns>KDTreeNode containing 'vertex'</returns>
public KDNodeJeremyC GetSplitNode(VertexKDTree vertex)
{
if (ChildLeft == null || ChildRight == null)
{
ChildLeft = new KDNodeJeremyC {
Parent = this
};
ChildRight = new KDNodeJeremyC {
Parent = this
};
ChildLeft.Sibling = ChildRight;
ChildRight.Sibling = ChildLeft;
}
float mid_value = Leaf.Vector[(int)SplitAxis];
float pt_value = vertex.Vector[(int)SplitAxis];
//float diff = pt_value - mid_value;
//float diff2 = Math.Abs(diff / pt_value);
//if (diff > 0)
// return ChildLeft;
//else if (diff < 0 && diff2 < 1e-7)
// return ChildLeft;
//else
// return ChildRight;
KDNodeJeremyC child = (pt_value >= mid_value) ? ChildLeft : ChildRight;
return(child);
}
public bool SearchNode(VertexKDTree v, KDNode node, float bestDistance)
{
// float bestDistance = float.MaxValue;
float dist = node.Leaf.Vector.Distance(v.Vector);
if (dist < bestDistance && !node.Taken)
{
bestDistance = dist;
node.Taken = true;
v.Index = node.Leaf.Index;
}
if (node.NodeLeft != null)
{
//float dist = node.Leaf.Vector.Distance(v.Vector);
if (dist < bestDistance && !node.Taken)
{
bestDistance = dist;
node.Taken = true;
v.Index = node.Leaf.Index;
}
}
return(true);
}
/// <summary>
/// Based upon the info of the nearest vertex (of each vertex), the triangles are created
/// </summary>
/// <param name="myModel"></param>
//private static List<Triangle> CreateTrianglesByNearestVertices(PointCloud pointCloud)
//{
// List<Triangle> listTriangles = new List<Triangle>();
// //create triangles
// //for (int i = pointCloud.Count - 1; i >= 0; i--)
// for (int i = 0; i < pointCloud.Count; i++)
// {
// Vertex v = pointCloud[i];
// if (v.KDTreeSearch.Count >= 2)
// {
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[0].Key, v.KDTreeSearch[1].Key, listTriangles, v);
// }
// if (v.KDTreeSearch.Count >= 3)
// {
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[0].Key, v.KDTreeSearch[2].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[1].Key, v.KDTreeSearch[2].Key, listTriangles, v);
// }
// if (v.KDTreeSearch.Count >= 4)
// {
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[0].Key, v.KDTreeSearch[3].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[1].Key, v.KDTreeSearch[3].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[2].Key, v.KDTreeSearch[3].Key, listTriangles, v);
// }
// if (v.KDTreeSearch.Count >= 5)
// {
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[0].Key, v.KDTreeSearch[4].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[1].Key, v.KDTreeSearch[4].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[2].Key, v.KDTreeSearch[4].Key, listTriangles, v);
// Triangle.AddTriangleToList(v.Index, v.KDTreeSearch[3].Key, v.KDTreeSearch[4].Key, listTriangles, v);
// }
// }
// //RemoveDuplicateTriangles(listTriangles);
// listTriangles.Sort(new TriangleComparer());
// return listTriangles;
//}
public void Triangulate_KDTree(int numberNeighbours)
{
KDTreeKennell kdTree = new KDTreeKennell();
kdTree.Build(this);
List <Triangle> listTriangles = new List <Triangle>();
for (int i = 0; i < this.Vectors.Length; i++)
{
VertexKDTree vSource = new VertexKDTree(this.Vectors[i], this.Colors[i], i);
uint indexI = Convert.ToUInt32(i);
ListKDTreeResultVectors listResult = kdTree.Find_N_Nearest(vSource.Vector, numberNeighbours);
for (int j = 1; j < listResult.Count; j++)
{
for (int k = j + 1; k < listResult.Count; k++)
{
Triangle t = new Triangle(indexI, listResult[j].IndexNeighbour, listResult[k].IndexNeighbour);
listTriangles.Add(t);
}
}
}
this.Triangles = listTriangles;
CreateIndicesFromTriangles();
}
/// <summary>
/// Recursively split node across its pivot axis.
/// </summary>
/// <param name="node">KDTreeNode to split</param>
/// <param name="depth">Current recursion depth, set lower if you get stack overflow</param>
/// <returns>True if split was a success or max_depth/max_node_size criterion met</returns>
bool BuildNode(KDNodeJeremyC node, int depth)
{
if (depth >= MaxNodeDepth)
{
return(true);
}
if (node.Indices.Count <= MaxNodeSize)
{
return(true);
}
foreach (var index in node.Indices)
{
VertexKDTree vertex = TreeVectors[index];
if (!node.IsBuilt)
{
node.Build();
}
KDNodeJeremyC child = node.GetSplitNode(vertex);
child.AddVertex(index, vertex);
}
// TODO: Do we need to check if either child is empty? Since we're calculating the split axis
// using raw data it's unlikely.
node.Clear();
node.ChildLeft.Build();
node.ChildRight.Build();
BuildNode(node.ChildLeft, depth + 1);
BuildNode(node.ChildRight, depth + 1);
return(true);
}
public KDNode(VertexKDTree v, List <VertexKDTree> myListLeft, List <VertexKDTree> myListRight)
{
Leaf = v;
NodeLeft = KDTree.BuildNode(myListLeft);
NodeRight = KDTree.BuildNode(myListRight);
//ListLeft = myListLeft;
//ListRight= myListRight;
}
/// <summary>
/// returns the target (tree) points found for the input (source) points
/// </summary>
/// <param name="source"></param>
/// <returns></returns>
public PointCloud FindClosestPointCloud_Parallel(PointCloud source)
{
this.source = source;
this.ResetTaken();
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
//shuffle points for the "Taken" algorithm
PointCloud sourceShuffled;
if (this.TakenAlgorithm)
{
sourceShuffled = source.Clone();
sourceShuffled.SetDefaultIndices();
sourceShuffled = PointCloud.Shuffle(sourceShuffled);
}
else
{
sourceShuffled = source;
}
int nearest_index = 0;
float nearest_distance = 0f;
System.Threading.Tasks.Parallel.For(0, source.Count, i =>
//for (int i = 0; i < sourceShuffled.Count; i++)
{
VertexKDTree vSource = new VertexKDTree(sourceShuffled.Vectors[i], i);
VertexKDTree vTargetFound = FindClosestPoint(vSource, ref nearest_distance, ref nearest_index);
//resultArray[i] = vTargetFound.Clone();
resultArray[i] = vTargetFound;
});
List <VertexKDTree> resultList = new List <VertexKDTree>(resultArray);
result = PointCloud.FromListVertexKDTree(resultList);
//shuffle back
PointCloud pcResultShuffledBack;
if (this.TakenAlgorithm)
{
pcResultShuffledBack = result.Clone();
for (int i = 0; i < pcResultShuffledBack.Count; i++)
{
//pcResultShuffled.Vectors[i] = pcResult.Vectors[Convert.ToInt32(sourceShuffled.Indices[i])];
pcResultShuffledBack.Vectors[Convert.ToInt32(sourceShuffled.Indices[i])] = result.Vectors[i];
}
}
else
{
pcResultShuffledBack = result;
}
//this.MeanDistance = PointCloud.MeanDistance(source, pcResultShuffledBack);
return(pcResultShuffledBack);
}
/// <summary>
/// Find closest point using an axis aligned search boundary
/// </summary>
/// <param name="node"></param>
/// <param name="point"></param>
/// <returns></returns>
VertexKDTree FindClosestPoint_Recursive(KDNodeJeremyC node, VertexKDTree vertex, BoundingBoxAxisAligned search_bounds, ref int nearest_index)
{
int tmp_index = -1;
if (node.IsLeaf)
{
tmp_index = -1;
VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
if (result != null)
{
nearest_index = tmp_index;
return(result);
}
else
{
return(null);
}
}
tmp_index = -1;
KDNodeJeremyC near_child = node.GetSplitNode(vertex);
VertexKDTree retV = FindClosestPoint_Recursive(near_child, vertex, search_bounds, ref tmp_index);
//Edgar - TakenVector implementation - have to go one level up if vector is taken
float near_distance = float.MaxValue;
if (retV != null)
{
//near_distance = retV.Vector.Distance(vertex.Vector);
near_distance = retV.Vector.DistanceSquared(vertex.Vector);
nearest_index = tmp_index;
}
KDNodeJeremyC far_child = near_child.Sibling;
if (search_bounds != null && far_child.BoundingBox.Intersects(search_bounds))
{
VertexKDTree far_result = FindClosestPoint_Recursive(far_child, vertex, search_bounds, ref tmp_index);
//Edgar - TakenVector implementation - have to go one level up if vector is taken
if (far_result != null)
{
//float far_distance = far_result.Vector.Distance(vertex.Vector);
float far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
if (far_distance < near_distance)
{
nearest_index = tmp_index;
retV = far_result;
}
}
}
if (retV == null)
{
}
return(retV);
}
public void Search(PointCloud pcl)
{
List <VertexKDTree> list = new List <VertexKDTree>(pcl.VectorsWithIndex);
for (int i = 0; i < list.Count; i++)
{
VertexKDTree v = list[i];
//if(v.Vector.Distance())
//for(int j = 0; j < this.RootNode.)
}
}
/// <summary>
/// Utility method to build KDTreeNode variables, as long as min and max
/// </summary>
public void Build()
{
Leaf = new VertexKDTree(new Vector3(maxV + minV) / 2.0f, -1);
rangeV = maxV - minV;
SplitAxis = rangeV.LargestAxis();
BoundingBox = new BoundingBoxAxisAligned(minV, maxV);
IsBuilt = true;
}
/// <summary>
/// Add a new vertex, and its index, to node.
/// </summary>
/// <param name="index">Index in to KDTree's vertices List</param>
/// <param name="vertex">Actual vertex from KDTree's vertex list</param>
public void AddVertex(int index, VertexKDTree vertex)
{
if (Indices.Count == 0)
{
minV = maxV = vertex.Vector;
}
minV = Vector3.Min(minV, vertex.Vector);
maxV = Vector3.Max(maxV, vertex.Vector);
Indices.Add(index);
IsBuilt = false;
}
/// <summary>
/// FInd the closest matching point using a full For-loop search: O(n)
/// </summary>
/// <param name="vertex">Vertex to match</param>
/// <param name="nearest_index">Index of matching vertex in the KDTree vertex array</param>
/// <returns>Nearest matching vertex</returns>
public VertexKDTree FindClosestPoint(VertexKDTree vertex, ref float nearestDistance, ref int nearest_index)
{
float[] array = new float[] { vertex.Vector.X, vertex.Vector.Y, vertex.Vector.Z };
Tuple <float[], string>[] treeNearest = tree.NearestNeighbors(array, 1);
Tuple <float[], string> p = treeNearest[0];
VertexKDTree v = new VertexKDTree();
v.Vector = new Vector3(p.Item1[0], p.Item1[1], p.Item1[2]);
return(v);
}
/// <summary>
/// FInd the closest matching point using a full For-loop search: O(n)
/// </summary>
/// <param name="vertex">Vertex to match</param>
/// <param name="nearest_index">Index of matching vertex in the KDTree vertex array</param>
/// <returns>Nearest matching vertex</returns>
public VertexKDTree FindClosestPoint(VertexKDTree vertex, ref float nearestDistance, ref int nearest_index)
{
VertexKDTree v = new VertexKDTree();
ListKDTreeResultVectors listResult = Find_N_Nearest(vertex.Vector, 1);
if (listResult != null && listResult.Count > 0)
{
nearest_index = Convert.ToInt32(listResult[0].IndexNeighbour);
nearestDistance = listResult[0].Distance;
v = this.TreeVectors[Convert.ToInt32(listResult[0].IndexNeighbour)];
}
return(v);
}
/// <summary>
/// Algorithm based on ignoring points with less neighbours int thresholdNeighboursCount = 10; float thresholdDistance = 15e-5f;
/// at given distance.
/// </summary>
/// <param name="source"></param>
/// <param name="threshold"></param>
/// <returns></returns>
public static PointCloud ByLessNeighbours(PointCloud source, float thresholdDistance, int thresholdNeighboursCount)
{
PointCloud pcResult = new PointCloud();
KDTreeKennell kdTree = new KDTreeKennell();
kdTree.Build(source);
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
try
{
List <Vector3> listV = new List <Vector3>();
List <Vector3> listC = new List <Vector3>();
System.Threading.Tasks.Parallel.For(0, source.Count, i =>
{
VertexKDTree vSource = new VertexKDTree(source.Vectors[i], source.Colors[i], i);
int neighboursCount = 0;
kdTree.FindClosestPoints_Radius(vSource, thresholdDistance, ref neighboursCount);
if (neighboursCount >= thresholdNeighboursCount)
{
resultArray[i] = vSource;
}
});
for (int i = 0; i < source.Count; i++)
{
if (resultArray[i] != null)
{
listV.Add(resultArray[i].Vector);
listC.Add(resultArray[i].Color);
}
}
pcResult.Vectors = listV.ToArray();
pcResult.Colors = listC.ToArray();
pcResult.SetDefaultIndices();
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in KDTreeKennnellRemoveDuplicates: " + err.Message);
}
return(pcResult);
}
private List <VertexKDTree> FindClosestPoint_List_Parallel(List <VertexKDTree> source)
{
this.ResetTaken();
VertexKDTree[] vArray = source.ToArray();
System.Threading.Tasks.Parallel.For(0, source.Count, i =>
{
int nearest_index = 0;
float nearest_distance = 0f;
VertexKDTree vTargetFound = FindClosestPoint(vArray[i], ref nearest_distance, ref nearest_index);
vArray[i] = vTargetFound.Clone();
});
return(new List <VertexKDTree>(vArray));
}
public void FindClosestPoints_Radius(VertexKDTree vertex, float radius, ref int neighboursCount)
{
// search for all within a ball of a certain radius
//ListKDTreeResultVectors result = new ListKDTreeResultVectors();
SearchRecord sr = new SearchRecord(vertex.Vector);
// Vector3 vdiff = new Vector3();
sr.Radius = radius;
root.search(sr);
ListKDTreeResultVectors listResult = sr.SearchResult;
neighboursCount = listResult.Count;
}
/// <summary>
/// Find the closest matching vertex
/// </summary>
/// <param name="vertex">Vertex to find closest match.</param>
/// <param name="distance">Search distance.</param>
/// <param name="nearest_index">Index of matching vertex in the KDTree vertex array</param>
/// <returns>Nearest matching vertex</returns>
public VertexKDTree FindClosestPoint(VertexKDTree vertex, ref float distance, ref int nearest_index)
{
if (root == null)
{
Console.WriteLine("Null root, Build() must be called before using the KDTree.");
return(new VertexKDTree(new Vector3(0, 0, 0), -1));
}
BoundingBoxAxisAligned search_bounds = null;
if (distance != 0)
{
Vector3 distance_vector = new Vector3(distance, distance, distance);
search_bounds = new BoundingBoxAxisAligned(vertex.Vector - distance_vector, vertex.Vector + distance_vector);
}
return(FindClosestPoint_Recursive(Root, vertex, search_bounds, ref nearest_index));
}
public static KDNode BuildNode(List <VertexKDTree> list)
{
if (list.Count <= 1)
{
if (list.Count == 0)
{
return(null);
}
else
{
return(new KDNode(list[0]));
}
}
//check length even or odd
int medianIndex = list.Count / 2;
if (list.Count % 2 == 0)
{
medianIndex = list.Count / 2;
}
else
{
medianIndex = list.Count / 2;
}
VertexKDTree leaf = list[medianIndex];
List <VertexKDTree> listLeft = new List <VertexKDTree>();
listLeft.AddRange(list);
listLeft.RemoveRange(medianIndex, list.Count - medianIndex);
// if(listLeft.Count > 0)
List <VertexKDTree> listRight = new List <VertexKDTree>();
listRight.AddRange(list);
listRight.RemoveRange(0, medianIndex + 1);
return(new KDNode(leaf, listLeft, listRight));
}
public PointCloud RemoveDuplicates(PointCloud source, float threshold)
{
PointCloud pcResult = new PointCloud();
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
try
{
List <Vector3> listV = new List <Vector3>();
List <Vector3> listC = new List <Vector3>();
System.Threading.Tasks.Parallel.For(0, source.Count, i =>
{
VertexKDTree vSource = new VertexKDTree(source.Vectors[i], i);
int nearest_index = 0;
float nearest_distance = 0f;
VertexKDTree vTargetFound = FindClosestPoint(vSource, ref nearest_distance, ref nearest_index);
if (nearest_distance > threshold)
{
resultArray[i] = vSource;
}
});
//add only the non null list items
for (int i = 0; i < source.Count; i++)
{
if (resultArray[i] != null)
{
listV.Add(resultArray[i].Vector);
listC.Add(resultArray[i].Color);
}
}
pcResult.Vectors = listV.ToArray();
pcResult.Colors = listC.ToArray();
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in KDTreeKennnellRemoveDuplicates: " + err.Message);
}
return(pcResult);
}
/// <summary>
/// FInd the closest matching point using a full For-loop search: O(n)
/// </summary>
/// <param name="vertex">Vertex to match</param>
/// <param name="nearest_index">Index of matching vertex in the KDTree vertex array</param>
/// <returns>Nearest matching vertex</returns>
public VertexKDTree FindClosestPoint(VertexKDTree vertex, ref float nearestDistance, ref int nearest_index)
{
float min_dist = 0.0f;
int min_index = -1;
for (int j = 0; j < TreeVectors.Count; j++)
{
VertexKDTree tmp_vertex = TreeVectors[j];
//float distance = tmp_vertex.Vector.Distance(vertex.Vector);
float distance = tmp_vertex.Vector.DistanceSquared(vertex.Vector);
if (min_index == -1)
{
min_dist = distance;
min_index = j;
}
else
{
if (distance < min_dist)
{
if (!this.TakenAlgorithm)
{
min_dist = distance;
min_index = j;
}
else
{
if (!tmp_vertex.TakenInTree)
{
tmp_vertex.TakenInTree = true;
min_dist = distance;
min_index = j;
}
}
}
}
}
nearest_index = min_index;
return(TreeVectors[min_index]);
}
//assumes a 2.5 D point cloud (i.e. for a given x,y there is only ONE z value
public void Triangulate25D(float distMin)
{
float meanDistance, standardDeviation;
float[] distances;
//1. get standard deviation, meanDistance and array of distances
Outliers.StandardDeviation(this, 10, out meanDistance, out standardDeviation, out distances);
distMin = meanDistance * 100;
this.Triangles = new List <Triangle>();
List <List <VertexKDTree> > listNew = SortVectorsWithIndex();
for (int i = listNew.Count - 1; i > 0; i--)
{
List <VertexKDTree> columnx = listNew[i];
List <VertexKDTree> columny = listNew[i - 1];
for (int j = 1; j < columnx.Count; j++)
{
VertexKDTree vx = columnx[j];
float dist_x = columnx[j - 1].Vector.Distance(vx.Vector);
if (dist_x < distMin)
{
foreach (VertexKDTree vy in columny)
{
float dist_xy = vx.Vector.Distance(vy.Vector);
if (dist_xy < distMin)
{
Triangles.Add(new Triangle(columnx[j - 1].Index, vx.Index, vy.Index));
}
}
}
}
}
CreateIndicesFromTriangles();
}
/// <summary>
/// Iterate through System.IO.Collections.Generic.List<int> to find closest matching vertex
/// </summary>
/// <param name="indices">List of integer indices that index vertices</param>
/// <param name="vertex">Vertex to match</param>
/// <returns>Closest stored 'vertex' to given 'vertex'</returns>
VertexKDTree GetClosestVertexFromIndices(List <int> indices, VertexKDTree vertex, ref int nearest_index)
{
int min_index = -1;
float min_dist = 0.0f;
foreach (var index in indices)
{
VertexKDTree tmp_vertex = TreeVectors[index];
if (!(TakenAlgorithm && tmp_vertex.TakenInTree))
{
if (min_index == -1)
{
//min_dist = tmp_vertex.Vector.Distance(vertex.Vector);
min_dist = tmp_vertex.Vector.DeltaSquared(vertex.Vector);
min_index = index;
}
else
{
//float tmp_dist = tmp_vertex.Vector.Distance(vertex.Vector);
float tmp_dist = tmp_vertex.Vector.DistanceSquared(vertex.Vector);
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_index = index;
}
}
}
}
nearest_index = min_index;
if (min_index != -1)
{
TreeVectors[min_index].TakenInTree = true;
return(TreeVectors[min_index]);
}
return(null);
}
/// <summary>
/// Find closest point using a centered bounding sphere
/// </summary>
/// <param name="node"></param>
/// <param name="vertex"></param>
/// <param name="search_bounds"></param>
/// <param name="nearest_index"></param>
/// <returns></returns>
VertexKDTree FindClosestPoint(KDNodeJeremyC node, VertexKDTree vertex, BoundingSphere search_bounds, ref int nearest_index)
{
int tmp_index = -1;
if (node.IsLeaf)
{
tmp_index = -1;
VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
nearest_index = tmp_index;
return(result);
}
tmp_index = -1;
KDNodeJeremyC near_child = node.GetSplitNode(vertex);
VertexKDTree near_result = FindClosestPoint(near_child, vertex, search_bounds, ref tmp_index);
VertexKDTree ret = near_result;
//float near_distance = near_result.Vector.Distance(vertex.Vector);
float near_distance = near_result.Vector.DistanceSquared(vertex.Vector);
nearest_index = tmp_index;
KDNodeJeremyC far_child = near_child.Sibling;
if (far_child.BoundingBox.Intersects(search_bounds))
{
VertexKDTree far_result = FindClosestPoint(far_child, vertex, search_bounds, ref tmp_index);
float far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
//float far_distance = far_result.Vector.Distance(vertex.Vector);
if (far_distance < near_distance)
{
nearest_index = tmp_index;
ret = far_result;
}
}
return(ret);
}
//public void CreateIndicesFromNewTriangles(List<OpenTKExtension.TriangleVectors> listTriangles)
//{
// if (listTriangles != null)
// {
// this.Indices = new uint[listTriangles.Count * 3];
// int ind = 0;
// Triangles = new List<Triangle>();
// foreach (OpenTKExtension.TriangleVectors t in listTriangles)
// {
// this.Indices[ind++] = Convert.ToUInt32(t.A_Index);
// this.Indices[ind++] = Convert.ToUInt32(t.B_Index);
// this.Indices[ind++] = Convert.ToUInt32(t.C_Index);
// Triangles.Add(new Triangle(Convert.ToInt32(t.A_Index), Convert.ToInt32(t.B_Index), Convert.ToInt32(t.C_Index)));
// }
// }
//}
private List <List <VertexKDTree> > SortVectorsWithIndex()
{
List <List <VertexKDTree> > listNew = new List <List <VertexKDTree> >();
List <VertexKDTree> listOld = new List <VertexKDTree>(this.VectorsWithIndex);
List <VertexKDTree> column = new List <VertexKDTree>();
bool positiveValues = false;
//for (int i = listOld.Count - 1; i >= 0; i--)
for (int i = 0; i < listOld.Count; i++)
{
VertexKDTree v = listOld[i];
if (v.Vector.Y < 0 && !positiveValues)
{
column.Add(v);
}
else if (v.Vector.Y > 0 && !positiveValues)
{
positiveValues = true;
column.Add(v);
}
else if (v.Vector.Y > 0)
{
column.Add(v);
}
else if (v.Vector.Y < 0 && positiveValues)
{
positiveValues = false;
listNew.Add(column);
column = new List <VertexKDTree>();
}
}
return(listNew);
}
public static void StandardDeviation(PointCloud source, int numberOfNeighbours, out float meanDistance, out float standardDeviation, out float[] distances)
{
meanDistance = 0;
standardDeviation = 0f;
distances = new float[source.Count];
KDTreeKennell kdTree = new KDTreeKennell();
kdTree.Build(source);
PointCloud pcResult = new PointCloud();
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
VertexKDTree[] outliers = new VertexKDTree[source.Count];
try
{
List <Vector3> listV = new List <Vector3>();
List <Vector3> listC = new List <Vector3>();
//1. mean distance of one point to his next "numberOfNeighbours" neighbours - stored in the "distances" array
for (int i = 0; i < source.Count; i++)
{
VertexKDTree vSource = new VertexKDTree(source.Vectors[i], source.Colors[i], i);
ListKDTreeResultVectors listResult = kdTree.Find_N_Nearest(vSource.Vector, numberOfNeighbours);
float distSum = 0f;
for (int k = 1; k < listResult.Count; ++k) // k = 0 is the query point
{
distSum += listResult[k].Distance;
}
distances[i] = (distSum / (listResult.Count - 1));
}
//2. calculate the mean distance of ALL points
for (int i = 0; i < distances.Length; ++i)
{
meanDistance += distances[i];
}
meanDistance /= distances.Length;
//3. calculate the deviation of each data point from the mean, and square the result of each
for (int i = 0; i < distances.Length; i++)
{
float dev = distances[i] - meanDistance;
dev *= dev;
standardDeviation += dev;
}
standardDeviation /= distances.Length;
standardDeviation = Convert.ToSingle(Math.Sqrt(standardDeviation));
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in KDTreeKennnellRemoveDuplicates: " + err.Message);
}
}
public VertexKDTree Clone()
{
VertexKDTree v = new VertexKDTree(this.Vector, this.Index);
return(v);
}
/// <summary>
/// tree.RemoveOutliersByDeviation(PointCloudDirty, 10, 1.3f);
/// </summary>
/// <param name="source"></param>
/// <param name="numberOfNeighbours"></param>
/// <param name="stdMultiplier"></param>
/// <returns></returns>
public static PointCloud ByStandardDeviation(PointCloud source, int numberOfNeighbours, float stdDeviationMultiplier, out PointCloud pcOutliersMarkedRed)
{
PointCloud pcResult = new PointCloud();
//the outliers are marked red
pcOutliersMarkedRed = source.Clone();
float meanDistance, standardDeviation;
float[] distances;
//1. get standard deviation, meanDistance and array of distances
StandardDeviation(source, numberOfNeighbours, out meanDistance, out standardDeviation, out distances);
int numberRemoved = 0;
VertexKDTree[] resultArray = new VertexKDTree[source.Count];
VertexKDTree[] outliers = new VertexKDTree[source.Count];
try
{
List <Vector3> listV = new List <Vector3>();
List <Vector3> listC = new List <Vector3>();
//2. distance threshold: deviation plus the overall mean distance
float distanceThreshold = meanDistance + standardDeviation;
//3. remove all points according to the distance threshold
for (int i = 0; i < source.Count; i++)
{
VertexKDTree vSource = new VertexKDTree(source.Vectors[i], source.Colors[i], i);
if (distances[i] > distanceThreshold)
{
pcOutliersMarkedRed.Colors[i] = new Vector3(1, 0, 0);
numberRemoved++;
continue;
}
else
{
resultArray[i] = vSource;
}
}
List <Vector3> listOutliers = new List <Vector3>();
List <Vector3> listOutliersColors = new List <Vector3>();
//build resulting cloud and outliers
for (int i = 0; i < source.Count; i++)
{
if (resultArray[i] != null)
{
listV.Add(resultArray[i].Vector);
listC.Add(resultArray[i].Color);
}
}
pcResult.Vectors = listV.ToArray();
pcResult.Colors = listC.ToArray();
pcResult.SetDefaultIndices();
System.Diagnostics.Debug.WriteLine("Outliers: Mean distance---" + meanDistance.ToString("G") + " ---- standard deviation ---" + standardDeviation.ToString("G") + "---Number of outliers: " + numberRemoved.ToString());
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in KDTreeKennnellRemoveDuplicates: " + err.Message);
}
return(pcResult);
}
public KDNode(VertexKDTree v)
{
Leaf = v;
}