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);
}
private void RemovePointsByAngle(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, List <List <Neighbours> > listNeighbours)
{
if (Normals_RemovePoints)
{
//adjust normals - because of Search, the number of PointTarget my be different
int pointsRemoved = 0;
for (int i = pointsTarget.Count - 1; i >= 0; i--)
{
Vector3d vT = pointsTarget[i].Vector;
Vector3d vS = pointsSource[i].Vector;
//float angle = Vector3d.CalculateAngle(vT, vS);
int indexVec = pointsTarget[i].IndexInModel;
Vector3 vTNormal = NormalsTarget[pointsTarget[i].IndexInModel];
//Vector3d vTNormal = NormalsTarget[i];
Vector3 vSNormal = NormalsSource[i];
float angle = vTNormal.AngleInDegrees(vSNormal);
//float angle = vT.AngleInDegrees(vS);
float angleToCheck = 30;// (this.startAngleForNormalsCheck - 5) * (1 - this.NumberOfIterations / this.ICPSettings.MaximumNumberOfIterations) + 5;
Debug.WriteLine("Angle: " + i.ToString() + " : " + angle.ToString("0.00"));
if (Math.Abs(angle) > angleToCheck)
{
pointsTarget.RemoveAt(i);
pointsSource.RemoveAt(i);
pointsRemoved++;
}
}
Debug.Assert(pointsTarget.Count > 2, "Error after kdtree - checking normals : Should have at least 3 points found");
Debug.WriteLine("--NormalCheck: Removed a total of: " + pointsRemoved.ToString());
}
}
private void RemovePointsWithDistanceGreaterThanMedian(List <float> listDistances, PointCloudVertices pointsSource, PointCloudVertices pointsTarget)
{
float median = GetMedian(listDistances);
for (int i = listDistances.Count - 1; i >= 0; i--)
{
if (listDistances[i] > median)
{
pointsSource.RemoveAt(i);
pointsTarget.RemoveAt(i);
}
}
}
private void RemovePointsWithDistanceGreaterThanAverage(PointCloudVertices pointsSource, PointCloudVertices pointsTarget, List <List <Neighbours> > listNeighbours)
{
float median = GetAverage(listNeighbours);
for (int i = listNeighbours.Count - 1; i >= 0; i--)
{
if (listNeighbours[i][0].Distance > median)
{
pointsSource.RemoveAt(i);
pointsTarget.RemoveAt(i);
}
}
}
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 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);
}
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 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);
}
}
public PointCloudVertices PerformICP_Stitching()
{
int iPoint = 0;
try
{
PointCloudVertices pointsTarget = null;
PointCloudVertices pointsSource = null;
ICPSolution res = CalculateStartSolution(ref pointsSource, ref pointsTarget, ICPSettings.NumberOfStartTrialPoints, this.LandmarkTransform, this.PTarget, this.PSource, ICPSettings.MaximumNumberOfIterations);
if (res == null)
{
return(null);
}
Matrix4d myMatrix = res.Matrix;
double oldMeanDistance = 0;
//now try all points and check if outlier
for (iPoint = (pointsTarget.Count - 1); iPoint >= 0; iPoint--)
{
double distanceOfNewPoint = CheckNewPointDistance(iPoint, myMatrix, pointsTarget, pointsSource);
////experimental
////--compare this distance to:
//pointsTargetTrial.Add[pointsTargetTrial.Count, p1[0], p1[1], p1[2]);
//pointsSourceTrial.Add[pointsSourceTrial.Count, p2[0], p2[1], p2[2]);
//PointCloud tempPointRotateAll = TransformPoints(pointsSourceTrial, myMatrix, pointsSourceTrial.Count);
//dist = CalculateTotalDistance(pointsTargetTrial, tempPointRotateAll);
//DebugWriteUtils.WriteTestOutput(myMatrix, pointsSourceTrial, tempPointRotateAll, pointsTargetTrial, pointsTargetTrial.Count);
Debug.WriteLine("------>ICP Iteration Trial: " + iPoint.ToString() + " : Mean Distance: " + distanceOfNewPoint.ToString());
if (Math.Abs(distanceOfNewPoint - res.MeanDistance) < ICPSettings.ThresholdOutlier)
{
PointCloudVertices pointsTargetTrial = PointCloudVertices.CopyVertices(res.PointsTarget);
PointCloudVertices pointsSourceTrial = PointCloudVertices.CopyVertices(res.PointsSource);
myMatrix = TryoutNewPoint(iPoint, pointsTarget, pointsSource, pointsTargetTrial, pointsSourceTrial, this.LandmarkTransform);
PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSourceTrial, myMatrix);
this.MeanDistance = PointCloudVertices.MeanDistance(pointsTargetTrial, myPointsTransformed);
// this.MeanDistance = totaldist / Convert.ToSingle(pointsTargetTrial.Count);
DebugWriteUtils.WriteTestOutputVertex("Iteration " + iPoint.ToString(), myMatrix, pointsSourceTrial, myPointsTransformed, pointsTargetTrial);
//could also remove this check...
if (Math.Abs(oldMeanDistance - this.MeanDistance) < ICPSettings.ThresholdOutlier)
{
res.PointsTarget = pointsTargetTrial;
res.PointsSource = pointsSourceTrial;
res.Matrix = myMatrix;
res.PointsTransformed = myPointsTransformed;
oldMeanDistance = this.MeanDistance;
//Debug.WriteLine("************* Point OK : ");
DebugWriteUtils.WriteTestOutputVertex("************* Point OK :", myMatrix, res.PointsSource, myPointsTransformed, res.PointsTarget);
}
//remove point from point list
pointsTarget.RemoveAt(iPoint);
pointsSource.RemoveAt(iPoint);
}
}
this.Matrix = res.Matrix;
//System.Diagnostics.Debug.WriteLine("Solution of ICP is : ");
DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, res.PointsSource, res.PointsTransformed, res.PointsTarget);
pointsTransformed = res.PointsTransformed;
return(pointsTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at point: " + iPoint.ToString() + " : " + err.Message);
return(null);
}
//Matrix4d newMatrix = accumulate.GetMatrix();
//this.Matrix = newMatrix;
}
