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;

> Centroid := <<0,0>>;

> for i from 1 to nops(l) do:

> Centroid := l[i] + Centroid;

> end do:

> Centroid := Centroid/nops(l);

> end proc:

> stdev := proc(l::list)

> local i,vmc,StDev,n;

> StDev := 0;

> n := nops(l);

> for i from 1 to n do:

> vmc[i] := convert(l[i]-centroid(l),Vector);

> StDev := StDev + linalg[dotprod](vmc[i],vmc[i]);

> end do:

> StDev := StDev/n;

> end proc:

scalrottrans := proc(Actual::list,Ideal::list)

> local i, centroidx, centroidy, n, stdevx, stdevy, xvmc, covariance, Spos, R,c,t,mserror, U::Matrix, S::Matrix, Vt::Matrix,DD;

>

> use LinearAlgebra in

> centroidx := centroid(Actual);

> centroidy := centroid(Ideal);

> stdevx := stdev(Actual);

> stdevy := stdev(Ideal);

>

> n := nops(Actual);

> covariance := <<0,0>|<0,0>>;

> for i from 1 to n do:

> covariance := covariance + (Ideal[i]-centroidy).(Transpose(Actual[i]-centroidx));

> end do:

> covariance := covariance/n;

>

> if (linalg[det](covariance) >= 0) then

> Spos := <<1,0>|<0,1>> else Spos := <<1,0>|<0,-1>> end if;

>

> U,S,Vt := LinearAlgebra[SingularValues](covariance, output=[':-U',':-S',':-Vt']);

>

> DD := DiagonalMatrix( [S[1],S[2]],2,2);

> R:= U.Spos.Vt;

> t := centroidy - R.centroidx;

> mserror := stdevy - linalg[trace]((DD.Spos))^2/stdevx;

> return([R,t,mserror]);

> end use;

> end proc:

It seems to work for some easy cases, where the mean squared error is zero after the rotation and translation...

The method was adapted from a paper by Shinji Umeyama (1991), IEEE Transactions on Pattern Analysis and Machine Intelligence. Except that in his paper you were allowed to scale the points as well. So the mean squared error becomes:

e = 1/n (Summation from i = 1 to n) ||Ideal[i] - R.Actual[i]||^2

where c is the scaling factor

I've made a proc of Umeyama's solution in the original form, with scaling allowed, and I think it works:

scalrottrans := proc(Actual::list,Ideal::list)

> local i, centroidx, centroidy, n, stdevx, stdevy, xvmc, covariance, Spos, R,c,t,mserror, U::Matrix, S::Matrix, Vt::Matrix,DD;

>

> use LinearAlgebra in

> centroidx := centroid(Actual);

> centroidy := centroid(Ideal);

> stdevx := stdev(Actual);

> stdevy := stdev(Ideal);

>

> n := nops(Actual);

> covariance := <<0,0>|<0,0>>;

> for i from 1 to n do:

> covariance := covariance + (Ideal[i]-centroidy).(Transpose(Actual[i]-centroidx));

> end do:

> covariance := covariance/n;

>

> if (linalg[det](covariance) >= 0) then

> Spos := <<1,0>|<0,1>> else Spos := <<1,0>|<0,-1>> end if;

>

> U,S,Vt := LinearAlgebra[SingularValues](covariance, output=[':-U',':-S',':-Vt']);

>

> DD := DiagonalMatrix( [S[1],S[2]],2,2);

> R:= U.Spos.Vt;

> c := 1/stdevx * linalg[trace](DD.Spos);

> t := centroidy - c*R.centroidx;

> mserror := stdevy - linalg[trace]((DD.Spos))^2/stdevx;

> return([R,c,t,mserror]);

> end use;

> end proc:

I'm just not sure if its ok to just take out the scaling factor...

Can anyone think of two sets of point patterns where if you could scale, rotate and translate to minimise e you would want to use one rotation matrix R,

but if you could only rotate and translate it, you would want to use another rotation matrix, R'? (Thus showing that the first proc is incorrect?)

Thanks in advance,

Wei-Yan