public PointCloudVertices AlignPointClouds_SVD_WithShuflleEffect(bool axesRotateEffect, 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 = 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); //MeanDistance = PointCloudVertices.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(pointCloudResult); }
private void CheckPCA(Matrix4d myMatrix4d, PointCloud sourceAxes, PointCloud targetAxes) { //----------------------- //for check - transform sourceAxes - should give targetAxis: i.w. resultList should contains only zeros PointCloud resultAxes = myMatrix4d.TransformPoints(sourceAxes); resultAxes.SubtractCloud(targetAxes); float fMax; if (!resultAxes.CheckCloud(this.ThresholdConvergence, out fMax)) { System.Windows.Forms.MessageBox.Show("SW Error in SVD.FindTransformationMatrix, difference should be zero, is: " + fMax.ToString()); } }
/// <summary> /// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult /// </summary> /// <param name="i"></param> /// <param name="j"></param> /// <param name="bestResultMeanDistance"></param> /// <param name="meanDistance"></param> /// <param name="myMatrixBestResult"></param> /// <param name="sourceAxes"></param> private void SVD_ForTwoPointCloudAlignment(int i, int j, ref double bestResultMeanDistance, ref double meanDistance, ref Matrix4d myMatrixBestResult, PointCloudVertices sourceAxes) { PointCloudVertices targetAxes = InvertAxes(pointCloudTargetCentered, pointCloudTargetCentered.PCAAxes, j); Matrix4d myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama); //Matrix4d myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama); //----------------------- //for check - should give TargetPCVectors List <Vector3d> resultAxes = Matrix4dExtension.TransformPoints(myMatrix, PointCloudVertices.ToVectors(sourceAxes)); resultAxes = resultAxes.Subtract(PointCloudVertices.ToVectors(targetAxes)); List <Vector3d> myPointsResult = myMatrix.TransformPoints(PointCloudVertices.ToVectors(pointCloudSourceCentered)); PointCloudVertices myPointsResultTemp = PointCloudVertices.FromVectors(myPointsResult); PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela_Parallel(ref myPointsResultTemp, pointCloudTargetCentered, -1); //PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1); double trace = myMatrix.Trace; meanDistance = kdtree.MeanDistance; //double trace = kdtree.MeanDistance; //double meanDistance = myMatrix.Trace; //Check: System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G") + " : Trace: " + trace.ToString("G")); if (meanDistance < bestResultMeanDistance) { myMatrixBestResult = myMatrix; bestResultMeanDistance = meanDistance; pointCloudResultBest = PointCloudVertices.FromVectors(myPointsResult); } pointCloudResult = PointCloudVertices.FromVectors(myPointsResult); pointCloudTargetKDTree = myPointCloudTargetTemp; }
/// <summary> /// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult /// </summary> /// <param name="i"></param> /// <param name="j"></param> /// <param name="bestResultMeanDistance"></param> /// <param name="meanDistance"></param> /// <param name="myMatrixBestResult"></param> /// <param name="sourceAxes"></param> private void SVD_ForTwoPointCloudAlignment_Double(int i, int j, ref float meanDistance, ref Matrix4 myMatrixBestResult, PointCloud sourceAxes) { PointCloud targetAxes = InvertAxes(pcTargetCentered, pcTargetCentered.PCAAxes, j); //Matrix4 myMatrix = SVD_Float.FindTransformationMatrix(sourceAxes, targetAxes, ICP_VersionUsed.Scaling_Umeyama); Matrix4d myMatrix4d = SVD.FindTransformationMatrix_WithoutCentroids(sourceAxes, targetAxes, ICP_VersionUsed.Umeyama); //CheckPCA(myMatrix4d, sourceAxes, targetAxes); PointCloud myResultPC = myMatrix4d.TransformPoints(pcSourceCentered); //-------------- pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC); meanDistance = KDTree.MeanDistance; //PointCloud myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1); System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G")); if (meanDistance < bestResultMeanDistance) { myMatrixBestResult = myMatrix4d.ToMatrix4(); bestResultMeanDistance = meanDistance; pcResultBest = myResultPC; } pcResult = myResultPC; }