正交迭代求位姿

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;