private void AssignPCVectorsOld(PointCloudVertices pointsSource, PointCloudVertices mypointCloudSourceCentered)
{
//pointsSource.PCAAxesNew = new PointCloud();
pointsSource.CentroidVector = Centroid;
pointsSource.PCAAxes = new PointCloudVertices();
List <Vector3d> vectorList = PointCloudVertices.ToVectors(mypointCloudSourceCentered);
for (int i = 0; i < 3; i++)
{
List <Vector3d> vList = GetResultOfEigenvector(i, vectorList);
vList.SubtractVector(Centroid);
Vector3d v = vList.GetMax();
if (EV[i] == 0)
{
v = new Vector3d();
}
pointsSource.PCAAxes.Add(new Vertex(v));
}
for (int i = 0; i < 3; i++)
{
pointsSource.PCAAxes[i].Vector += Centroid;
}
mypointCloudSourceCentered.PCAAxes = pointsSource.PCAAxes;
}
public PointCloudVertices CalculatePCA(PointCloudVertices pointsSource, int eigenvectorUsed)
{
List <Vector3d> vector3dSource = PointCloudVertices.ToVectors(pointsSource);
vector3dSource = CalculatePCA(vector3dSource, eigenvectorUsed);
return(PointCloudVertices.FromVectors(vector3dSource));
}
private List <Vector3d> ShiftByCenterOfMassVectorList(PointCloudVertices pointsSource)
{
Centroid = pointsSource.CentroidVectorGet;
List <Vector3d> listSource = PointCloudVertices.ToVectors(pointsSource);
listSource.SubtractVector(this.Centroid);
return(listSource);
}
public static Matrix4d TryoutPointsSA(PointCloudVertices pointsTarget, PointCloudVertices pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
//transform:
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(res.PointsSource), PointCloudVertices.ToVectors(res.PointsTarget), myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
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);
}
/// <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;
}
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);
}
/// <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;
}
public PointCloudVertices AlignToCenter(PointCloudVertices pointCloudSource)
{
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
Matrix3d R = new Matrix3d();
R = R.RotationChangeBasis(PointCloudVertices.ToVectors(pointCloudSource.PCAAxes));
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
return(pointCloudResult);
}
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);
}
