private Matrix4d AdjustSourceTargetByTranslation(Matrix4d myMatrixFound, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget) { Matrix3d R = myMatrixFound.ExtractMatrix3d(); Vector3d T = SVD.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R); myMatrixFound = myMatrixFound.AddTranslation(T); return(myMatrixFound); }
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); }