function?[R?t]?=?abspose（P?Q）
%?ABSPOSE?-?Absolute?orientation?
%
%???INPUTS:
%?????P?-?the?reference?point?set?arranged?as?a?3xn?matrix
%?????Q?-?the?point?set?obtained?by?transforming?P?with
%?????????some?pose?estimate?（typically?the?last?estimate）
%
%???OUTPUTS:
%?????R?-?estimated?rotation?matrix
%?????t?-?estimated?translation?vector

n?=?size（P2）;

%?compute?means?of?P?and?Q
pbar?=?sum（P2）/n;
qbar?=?sum（Q2）/n;

for?i?=?1:n
??PP（:i）?=?P（:i）-pbar;
??QQ（:i）?=?Q（:i）-qbar;
end

%?compute?M?matrix
M（1:31:3）?=?0;
for?i?=?1:n
??M?=?M+PP（:i）*QQ（:i）.‘;
end

%?calculate?SVD?of?M
[USV]?=?svd（M）;

%?compose?the?optimal?estimate?of?R
R?=?V*（U.‘）;
t?=?qbar?-?R*pbar;