public PointCloudVertices AlignPointClouds_SVD(PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
{
try
{
if (pointCloudSource == null || pointCloudTarget == null || pointCloudSource.Count == 0 || pointCloudTarget.Count == 0)
{
System.Diagnostics.Debug.WriteLine("PCA - please check point clouds ");
return(null);
}
this.Matrix = Matrix4d.Identity;
// pointCloudSourceCentered = ShiftByCenterOfMass(pointCloudSource);
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
PrepareTargetTree(pointCloudTarget);
PointCloudVertices myPointCloudIteration = PointCloudVertices.CloneVertices(pointCloudSource);
for (int i = 0; i < MaxmimumIterations; i++)
{
double meanDistance = SVD_Iteration(myPointCloudIteration);
System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString());
if (meanDistance < thresholdConvergence)
{
break;
}
myPointCloudIteration = pointCloudResultBest;
}
//final check:
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
//"Shuffle" effect - the target points are in other order after kdtree search:
//The mean distance calculated again, as check (was calculated before in the kdTree routine)
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTargetKDTree);
System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget);
pointCloudResult = Matrix.TransformPoints(pointCloudSource);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
}
catch
{
System.Windows.Forms.MessageBox.Show("Error aligning point cloud");
}
return(pointCloudResult);
}
private void PrepareTargetTree(PointCloudVertices pointCloudTarget)
{
//second object:
//-----------
pointCloudTargetCentered = CalculatePCA_Internal(pointCloudTarget);
//pointCloudTargetCentered = ShiftByCenterOfMass(pointCloudTarget);
kdtree = new KDTreeVertex();
kdtree.BuildKDTree_Rednaxela(pointCloudTargetCentered);
pointCloudResult = null;
pointCloudTargetKDTree = null;
}
//// Make a new tree from a list of points.
//public static KDTreeNew MakeFromVectorArray(params Vertex[] points)
//{
// int[] indices = Iota(points.Length);
// return MakeFromPointsInner(0, 0, points.Length - 1, points, indices);
//}
// Make a new tree from a list of points.
public static KDTree_Stark Build(PointCloudVertices points)
{
try
{
int[] indices = Iota(points.Count);
return(MakeFromVectorsRecursive(0, 0, points.Count - 1, points, indices));
}
catch (System.Exception err)
{
System.Windows.Forms.MessageBox.Show("Error building kd-tree " + err.Message);
return(null);
}
}
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);
}
}
}
/// <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);
}
public static double Test2_RotationX30Degrees(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
//myPCLTarget = Vertices.CreateSomePoints();
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = Matrix3d.CreateRotationX(30);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
/// <summary>
/// assume - vectors are mass - centered!
/// </summary>
/// <param name="pointCloud"></param>
/// <param name="axesVectors"></param>
/// <param name="i"></param>
/// <returns></returns>
private PointCloudVertices InvertAxes(PointCloudVertices pointCloud, PointCloudVertices axesVectors, int i)
{
PointCloudVertices resultList = PointCloudVertices.CopyVertices(axesVectors);
if (i == -1)
{
return(resultList);
}
Vector3d v = resultList[i].Vector.Negate();
resultList[i] = new Vertex(resultList[i].IndexInModel, v);
return(resultList);
}
public static double Test9_Face_Stitch(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "Models\\UnitTests";
Model model3DTarget = new Model(path + "\\KinectFace_1_15000.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
Model model3DSource = new Model(path + "\\KinectFace_2_15000.obj");
myPCLSource = model3DSource.PointCloudVertices;
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public PointCloudVertices AlignPointClouds_OneVector(PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget, int vectorNumberSource, int vectorNumberTarget)
{
//-------------------
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
//second object:
//-----------
pointCloudTargetCentered = CalculatePCA_Internal(pointCloudTarget);
//Vector3d v = TargetPCVectors[vectorNumberTarget];
//v.X = -v.X;
//v.Y = -v.Y;
//v.Z = -v.Z;
//TargetPCVectors[vectorNumberTarget] = v;
Matrix3d R = new Matrix3d();
//R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]);
R = R.RotationOneVertexToAnother(pointCloudSource.PCAAxes[vectorNumberSource], pointCloudTarget.PCAAxes[vectorNumberTarget]);
//R.CheckRotationMatrix();
//
//test:
//Vector3d testV = R.MultiplyVector(sourceV);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
PointCloudVertices.AddVector(pointCloudResult, pointCloudTarget.CentroidVector);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
System.Diagnostics.Debug.WriteLine("-->> PCA (V) - Mean Distance : " + MeanDistance.ToString("0.000000"));
return(pointCloudResult);
}
public static double Test11_Person_TwoScans(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "TestData";
Model model3DTarget = new Model(path + "\\1.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
Model model3DSource = new Model(path + "\\2.obj");
myPCLSource = model3DSource.PointCloudVertices;
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_CubeRotate(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(90, 124, -274);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static Bitmap UpdateFromPointCloud_Color(this Image im, PointCloudVertices pc, int width, int height)
{
//Bitmap bmp = new Bitmap(width, height, PixelFormat.Format32bppRgb);
byte[] data = PointCloudVertices.ToColorInfo(pc, width, height);
LockBitmap lockBitmap = new LockBitmap((Bitmap)im);
lockBitmap.LockBits();
lockBitmap.Pixels = data;
lockBitmap.UnlockBits();
return((Bitmap)im);
}
private Matrix4d Helper_FindTransformationMatrix(PointCloudVertices pointsSource, PointCloudVertices pointsTarget)
{
Matrix4d myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), ICPSettings.ICPVersion);
}
return(myMatrix);
}
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);
}
public static double Test10_Cube26p_RotateShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 2);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleRandom(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test10_CubeRTranslate(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 5);
//myPCLTarget = Vertices.CreateCube_Corners(10);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.Translate(myPCLSource, cubeSize * 1.2f, -cubeSize * 2.5f, cubeSize * 2);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_Cube8RotateShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(cubeSize);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = CreateAndPrintMatrix(45, 45, 45);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
/// <summary>
/// assigns the PC Axes to both pointsSource and mypointCloudSourceCentered
/// </summary>
/// <param name="pointsSource"></param>
/// <param name="mypointCloudSourceCentered"></param>
private void AssignPCAxes(PointCloudVertices pointsSource, PointCloudVertices mypointCloudSourceCentered)
{
pointsSource.CentroidVector = Centroid;
pointsSource.PCAAxes = new PointCloudVertices();
List <Vector3d> vectorList = PointCloudVertices.ToVectors(mypointCloudSourceCentered);
for (int i = 0; i < 3; i++)
{
Vector3d v = VT.ExtractColumn(i);
v = v * Convert.ToSingle(Math.Sqrt(EV[i]));
Vertex ve = new Vertex(i, v);
pointsSource.PCAAxes.Add(ve);
}
mypointCloudSourceCentered.PCAAxes = pointsSource.PCAAxes;
}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
////}
////}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT_NotNormalized.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV_NotNormalized[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
//}
private static PointCloudVertices CalculateResults(Matrix3d Ub, Matrix3d Ua, PointCloudVertices pointCloudSource, Vector3d centroidB, Vector3d centroidA)
{
Matrix3d R;
Matrix3d.Mult(ref Ub, ref Ua, out R);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.Rotate(pointCloudResult, R);
Vector3d t = centroidB - R.MultiplyVector(centroidA);
//Vertices.AddVector(pointCloudResult, t);
return(pointCloudResult);
}
public static List <Vector3> CalculateNormals_PCA(PointCloudVertices pointCloud, int numberOfNeighbours, bool centerOfMassMethod, bool flipNormalWithOriginVector)
{
KDTreeVertex kv = new KDTreeVertex();
kv.NumberOfNeighboursToSearch = numberOfNeighbours;
kv.BuildKDTree_Rednaxela(pointCloud);
kv.ResetVerticesLists(pointCloud);
kv.FindNearest_NormalsCheck_Rednaxela(pointCloud, false);
PCA pca = new PCA();
List <Vector3> normals = pca.Normals(pointCloud, centerOfMassMethod, flipNormalWithOriginVector);
return(normals);
}
public static double Test10_Cube8pRotateTranslateScaleShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 1);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.Translate(myPCLSource, cubeSize * 1.2f, -cubeSize * 2.5f, cubeSize * 2);
PointCloudVertices.ScaleByFactor(myPCLSource, 0.2f);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_Cube8TranslateRotateShuffleNew(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(cubeSize);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
//Model3D myModel = Example3DModels.Cuboid("Cuboid", 20f, 40f, 100, System.Drawing.Color.White, null);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.Translate(myPCLSource, cubeSize * 1.2f, -cubeSize * 2.5f, cubeSize * 2);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
/// <summary>
/// compute normals for a all vectors of pointSource
/// </summary>
/// <param name="pointsSource"></param>
/// <returns></returns>
public List <Vector3> Normals(PointCloudVertices pointsSource, bool centerOfMassMethod, bool flipNormalWithOriginVector)
{
Vector3 normalPrevious = new Vector3();
List <Vector3> normals = new List <Vector3>();
for (int i = 0; i < pointsSource.Count; i++)
{
Vertex v = pointsSource[i];
List <Vector3d> sublist = new List <Vector3d>();
for (int j = 0; j < v.KDTreeSearch.Count; j++)
{
Vertex vNearest = pointsSource[v.KDTreeSearch[j].Key];
sublist.Add(pointsSource[v.KDTreeSearch[j].Key].Vector);
}
if (centerOfMassMethod)
{
Vector3d centroid = sublist.CalculateCentroid();
sublist.SubtractVector(centroid);
}
else
{
sublist.SubtractVector(v.Vector);
}
SVD_ForListVectorsMassCentered(sublist, true);
Vector3 normal = V.ExtractRow(2).ToVector();
if (flipNormalWithOriginVector)
{
AdjustOrientationWithVector(ref normal, v.Vector.ToVector());
}
//if (i > 0)
// AdjustOrientation(ref normal, normalPrevious);
normalPrevious = normal;
normal.Normalize();
normals.Add(normal);
v.IndexNormals.Add(normals.Count - 1);
// to show ALL vectors (including the 2 vectors on the plane:
//AddAllPlaneVectors(v, normals);
}
return(normals);
}
/// <summary>
/// sets the result point cloud - resp. pointCloudResultBest
/// </summary>
/// <param name="mypointCloudSource"></param>
/// <returns></returns>
private double SVD_Iteration(PointCloudVertices mypointCloudSource)
{
double bestResultMeanDistance = double.MaxValue;
double meanDistance = double.MaxValue;
pointCloudSourceCentered = CalculatePCA_Internal(mypointCloudSource);
Matrix4d myMatrixBestResult = Matrix4d.Identity;
// int i = -1;
//SVD_ForTwoPointCloudAlignment(-1, -1, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, pointCloudSourceCentered.PCAAxes);
SVD_ForTwoPointCloudAlignment(-1, -1, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, pointCloudSourceCentered.PCAAxes);
//leads to a lot of iterations
if (axesRotateEffect)
{
//additionally try other solutions: Invert all axes
if (meanDistance > thresholdConvergence)
{
for (int i = -1; i < 3; i++)
{
PointCloudVertices sourceAxes = InvertAxes(pointCloudSourceCentered, pointCloudSourceCentered.PCAAxes, i);
for (int j = -1; j < i; j++)
{
SVD_ForTwoPointCloudAlignment(i, j, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, sourceAxes);
if (meanDistance < thresholdConvergence)
{
break;
}
}
if (meanDistance < thresholdConvergence)
{
break;
}
}
}
}
Matrix4d.Mult(ref myMatrixBestResult, ref this.Matrix, out this.Matrix);
//pointCloudResultBest
return(bestResultMeanDistance);
}
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);
}
private void RemoveAllVerticesBasedOnRadius(PointCloudVertices pointCloud)
{
//remove all pointCloud beyound minimal radius
//(distance list is automatically sorted due to KDTree)
for (int i = pointCloud.Count - 1; i >= 0; i--)
{
Vertex v = pointCloud[i];
//if(NeighboursDistance.Count < NumberOfNeighboursToSearch)
float distanceMax = v.KDTreeSearch[NumberOfNeighboursToSearch - 1].Value;
for (int j = v.KDTreeSearch.Count - 1; j >= 2; j--)
{
if (v.KDTreeSearch[j].Value > distanceMax)
{
v.KDTreeSearch.RemoveAt(j);
}
}
}
}
private void SaveResultCloudAndShow(PointCloudVertices pointCloudResult)
{
//-------------------------------------
//save
if (pointCloudResult != null)
{
string path = string.Empty;
string fileNameShort = string.Empty;
IOUtils.ExtractDirectoryAndNameFromFileName(pSource.FileNameLong, ref fileNameShort, ref path);
pointCloudResult.Save(this.pSource.Path, "Result.obj");
this.pResult = pointCloudResult.ToPointCloud();
//pResult.SetColor(new OpenTK.Vector3(1, 0, 0));
DisplayResultPointCloud();
}
}
private static bool CheckSourceTarget(PointCloudVertices myPointsTarget, PointCloudVertices mypointsSource)
{
// Check source, target
if (mypointsSource == null || mypointsSource.Count == 0)
{
MessageBox.Show("Source point set is empty");
System.Diagnostics.Debug.WriteLine("Can't execute with null or empty input");
return(false);
}
if (myPointsTarget == null || myPointsTarget.Count == 0)
{
MessageBox.Show("Target point set is empty");
System.Diagnostics.Debug.WriteLine("Can't execute with null or empty target");
return(false);
}
return(true);
}
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);
}
