I've made this proc to solve the absolute orientation problem, but I'm not sure if its correct...

I've got two sets of points, one called 'Actual' and one called 'Ideal'. Each set has n points.
I want to find rotation matrix R and translation matrix t which I can apply to the points in Actual so that the mean squared error, e, between the points in Actual and Ideal would be minimum.
e = 1/n (Summation from i = 1 to n) ||Ideal[i] - R.Actual[i]||^2


centroid := proc(l::list)
> local i,Centroid;