public static Matrix4d FindTransformationMatrix_WithoutCentroids(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
Matrix3d R = FindRotationMatrix(pointsSource, pointsTarget, icpVersionUsed);
//Vector3d T = new Vector3d();
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.FromMatrix3d(R);
return(myMatrix);
}
public static Matrix4d PutTheMatrix4dtogether(this Matrix4d mat4d, Vector3d T, Matrix3d Rotation)
{
//put the 4d matrix together
Matrix3d r3D = Rotation.Clone();
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.FromMatrix3d(r3D);
myMatrix[0, 3] = T.X;
myMatrix[1, 3] = T.Y;
myMatrix[2, 3] = T.Z;
myMatrix[3, 3] = 1f;
return(myMatrix);
}
