private void LoadTwoClouds()
{
string fileName1 = GLSettings.FileNamePointCloudLast1;
string fileName2 = GLSettings.FileNamePointCloudLast2;
//tabControlImages.SelectTab(0);
pSource = new PointCloud(GLSettings.Path + GLSettings.PathPointClouds, fileName1);
pointCloudFirstAfterLoad = pSource.Clone();
//pSource.SetColor(new OpenTK.Vector3(0, 1, 0));
pSource.Name = GLSettings.FileNamePointCloudLast1;
pSource.Path = GLSettings.Path + GLSettings.PathPointClouds;
pSource.FileNameLong = pSource.Path + "\\" + pSource.Name;
pTarget = new PointCloud(GLSettings.Path + GLSettings.PathPointClouds, fileName2);
//pTarget.SetColor(new OpenTK.Vector3(1, 1, 1));
pTarget.Name = GLSettings.FileNamePointCloudLast2;
pTarget.Path = GLSettings.Path + GLSettings.PathPointClouds;
pTarget.FileNameLong = pTarget.Path + "\\" + pTarget.Name;
}
/// <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);
}
public bool CutUndo()
{
if (this.PointCloud == null)
{
return(false);
}
RenderableObject o = this.RenderableObjects[0];
o.PointCloud = PointCloud.Clone(PointCloud);
AlignCameraToObject = true;
//ReplaceRenderableObject(objectOld, true);
return(true);
}
public PointCloud CalculatePCA(PointCloud pointsSource, int eigenvectorUsed)
{
List <Vector3> Vector3Source = pointsSource.ListVectors;
Centroid = pointsSource.CentroidVector;
List <Vector3> listTranslated = pointsSource.Clone().ListVectors;
Vector3Source.SubtractVector(Centroid);
SVD_ForListVectorsMassCentered(listTranslated, false);
List <Vector3> vectors = GetResultOfEigenvector(eigenvectorUsed, listTranslated);
return(PointCloud.FromListVector3(Vector3Source));
}
public static bool CheckCloudAbs(PointCloud myPCLTarget, PointCloud myPCLResult, double threshold)
{
PointCloud pt = myPCLTarget.Clone();
PointCloud pr = myPCLResult.Clone();
for (int i = 0; i < myPCLTarget.Count; i++)
{
pt.Vectors[i].X = Math.Abs(pt.Vectors[i].X);
pt.Vectors[i].Y = Math.Abs(pt.Vectors[i].Y);
pt.Vectors[i].Z = Math.Abs(pt.Vectors[i].Z);
pr.Vectors[i].X = Math.Abs(pr.Vectors[i].X);
pr.Vectors[i].Y = Math.Abs(pr.Vectors[i].Y);
pr.Vectors[i].Z = Math.Abs(pr.Vectors[i].Z);
}
return(PointCloud.CheckClouds(pt, pr, threshold));
}
public static Matrix4 FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3 R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3 T = SVD_Float.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4 myMatrix = new Matrix4();
myMatrix = myMatrix.PutTheMatrix4together(T, R);
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
//Vector3d centroidTarget_Double = new Vector3d(pointsTarget.CentroidVector.X, pointsTarget.CentroidVector.Y, pointsTarget.CentroidVector.Z);
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);
return(myMatrix);
}
public PointCloud Clone()
{
return(PointCloud.Clone(this));
}
public static PointCloud CloneAll(PointCloud pc)
{
return(pc.Clone());
}
/// <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);
}
