public static PointCloudVertices ReadXYZFile_ToVertices(string fileNameLong, bool rotatePoints)
{
List <Vector3d> listVector3d = ReadXYZFile(fileNameLong, rotatePoints);
PointCloudVertices listVertices = PointCloudVertices.FromVectors(listVector3d);
return(listVertices);
}
public PointCloudVertices CalculatePCA(PointCloudVertices pointsSource, int eigenvectorUsed)
{
List <Vector3d> vector3dSource = PointCloudVertices.ToVectors(pointsSource);
vector3dSource = CalculatePCA(vector3dSource, eigenvectorUsed);
return(PointCloudVertices.FromVectors(vector3dSource));
}
/// <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;
}
private PointCloudVertices ICPOnPoints_WithSubset(PointCloudVertices myPCLTarget, PointCloudVertices myPCLToBeMatched, PointCloudVertices myPointsTargetSubset, PointCloudVertices mypointsSourceSubset)
{
List <Vector3d> myVectorsTransformed = null;
PointCloudVertices myPCLTransformed = null;
try
{
Matrix4d m;
PerformICP(mypointsSourceSubset, myPointsTargetSubset);
myVectorsTransformed = PointCloudVertices.ToVectors(PTransformed);
m = Matrix;
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//extend points:
//myPointsTransformed = icpSharp.TransformPointsToPointsData(mypointsSourceSubset, m);
//-----------------------------
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//now with all other points as well...
myVectorsTransformed = new List <Vector3d>();
myVectorsTransformed = m.TransformPoints(PointCloudVertices.ToVectors(myPCLToBeMatched));
myPCLTransformed = PointCloudVertices.FromVectors(myVectorsTransformed);
//write all results in debug output
DebugWriteUtils.WriteTestOutputVertex("Soluation of Points With Subset", m, myPCLToBeMatched, myPCLTransformed, myPCLTarget);
}
catch (Exception err)
{
System.Diagnostics.Debug.WriteLine("Error in ICP : " + err.Message);
return(null);
}
//for output:
return(myPCLTransformed);
}