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); }