public PointCloud AlignPointClouds_SVD_WithShuflleEffect(bool axesRotateEffect, PointCloud pointCloudSource, PointCloud 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 = Matrix4.Identity; pcSourceCentered = CalculatePCA_Internal(pointCloudSource); PrepareTargetTree(pointCloudTarget); PointCloud myPointCloudIteration = PointCloud.CloneAll(pointCloudSource); for (int i = 0; i < MaxmimumIterations; i++) { float meanDistance = SVD_Iteration(myPointCloudIteration); System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString()); if (meanDistance < ThresholdConvergence) { break; } myPointCloudIteration = pcResultBest; } //final check: pcResultCentered = CalculatePCA_Internal(pcResult); //"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 = PointCloud.MeanDistance(pcResultBest, pcTreeResult); System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); //MeanDistance = PointCloud.MeanDistance(pointCloudResult, pointCloudTarget); //System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget); pcResult = Matrix.TransformPoints(pointCloudSource); pcResultCentered = CalculatePCA_Internal(pcResult); //MeanDistance = PointCloud.MeanDistance(pointCloudResult, pointCloudTarget); //System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); //for display later: } catch { System.Windows.Forms.MessageBox.Show("Error aligning point cloud"); } return(pcResult); }
public PointCloud AlignPointClouds_OneVector(PointCloud pointCloudSource, PointCloud pointCloudTarget, int vectorNumberSource, int vectorNumberTarget) { //------------------- pcSourceCentered = CalculatePCA_Internal(pointCloudSource); //second object: //----------- pcTargetCentered = CalculatePCA_Internal(pointCloudTarget); //Vector3 v = TargetPCVectors[vectorNumberTarget]; //v.X = -v.X; //v.Y = -v.Y; //v.Z = -v.Z; //TargetPCVectors[vectorNumberTarget] = v; Matrix3 R = new Matrix3(); //R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]); R = R.RotationOneVectorToAnother(pointCloudSource.PCAAxes[vectorNumberSource].Vector, pointCloudTarget.PCAAxes[vectorNumberTarget].Vector); //R.CheckRotationMatrix(); // //test: //Vector3 testV = R.MultiplyVector(sourceV); PointCloud pointCloudResult = PointCloud.CloneAll(pointCloudSource); PointCloud.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector); PointCloud.Rotate(pointCloudResult, R); PointCloud.AddVectorToAll(pointCloudResult, pointCloudTarget.CentroidVector); pcResultCentered = CalculatePCA_Internal(pointCloudResult); MeanDistance = PointCloud.MeanDistance(pointCloudResult, pointCloudTarget); System.Diagnostics.Debug.WriteLine("-->> PCA (V) - Mean Distance : " + MeanDistance.ToString("0.000000")); return(pointCloudResult); }