private static double CalculateScale_Zinsser(PointCloud pointsSourceShift, PointCloud pointsTargetShift, ref Matrix3d R)
{
double sum1 = 0;
double sum2 = 0;
Matrix3d RT = Matrix3d.Transpose(R);
Matrix3d checkT = Matrix3d.Mult(RT, R);
Matrix4d R4D = new Matrix4d();
R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R);
for (int i = 0; i < pointsSourceShift.Count; i++)
{
Vector3d vSource = new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z);
Vector3d vTarget = new Vector3d(pointsTargetShift.Vectors[i].X, pointsTargetShift.Vectors[i].Y, pointsTargetShift.Vectors[i].Z);
Vector3d vd = Vector3d.TransformNormalInverse(new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z), R4D);
vd = R.MultiplyVector(vSource);
//Vector3 v = new Vector3(vd.X, vd.Y, vd.Z);
//sum1 += Vector3.Dot(v, pointsTargetShift.Vectors[i]);
//sum2 += Vector3.Dot(pointsSourceShift.Vectors[i], pointsSourceShift.Vectors[i]);
sum1 += Vector3d.Dot(vd, vTarget);
sum2 += Vector3d.Dot(vSource, vSource);
}
double c = sum1 / sum2;
R = R.MultiplyScalar(c);
return(c);
}
private static double CalculateScale_Zinsser(List <Vector3d> pointsSourceShift, List <Vector3d> pointsTargetShift, ref Matrix3d R)
{
double sum1 = 0;
double sum2 = 0;
Matrix3d RT = Matrix3d.Transpose(R);
Matrix3d checkT = Matrix3d.Mult(RT, R);
Matrix4d R4D = new Matrix4d();
R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R);
for (int i = 0; i < pointsSourceShift.Count; i++)
{
Vector3d v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D);
v = R.MultiplyVector(pointsSourceShift[i]);
sum1 += Vector3d.Dot(v, pointsTargetShift[i]);
sum2 += Vector3d.Dot(pointsSourceShift[i], pointsSourceShift[i]);
}
double c = sum1 / sum2;
R = R.MultiplyScalar(c);
return(c);
}
public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3d centroidTarget = pointsTarget.CalculateCentroid();
List <Vector3d> pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3d centroidSource = pointsSource.CalculateCentroid();
List <Vector3d> pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = SVD.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(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);
}