public static PointCloud ShiftByCenterOfMass(PointCloud pointsSource)
{
Vector3 Centroid = pointsSource.CentroidVectorRecalc;
PointCloud pcNew = PointCloud.SubtractVector(pointsSource, Centroid);
return(pcNew);
}
private PointCloud ShiftByCenterOfMass(PointCloud pointsSource)
{
Centroid = pointsSource.CentroidVectorRecalc;
PointCloud pcNew = PointCloud.SubtractVector(pointsSource, this.Centroid);
return(pcNew);
}
public static Matrix4 FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3 R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3 T = SVD_Float.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4 myMatrix = new Matrix4();
myMatrix = myMatrix.PutTheMatrix4together(T, R);
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
//Vector3d centroidTarget_Double = new Vector3d(pointsTarget.CentroidVector.X, pointsTarget.CentroidVector.Y, pointsTarget.CentroidVector.Z);
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);
return(myMatrix);
}