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); }