Matla + SVD 求解变换矩阵

% svd分解迭代求取R,T
% 要求inputA和inputB点集没有尺度变换，只有旋转和平移
function [RR,TT,msen]=findhomobySVD(inputA,inputB) 

N=size(inputA,1);            
%2计算目标点集P的重心和参考点集X的重心
up=mean(inputA); %1*3的矩阵
ux=mean(inputB); %1*3的矩阵

%3_由点集P和X构造协方差矩阵
E=((inputA-repmat(up,N,1))'*(inputB-repmat(ux,N,1)));
%svd分解
[U,A,D]=svd(E);
%求旋转矩阵
RR=D*U';
%求平移矩阵
TT=ux'-RR*up';
inpB=(RR*inputA'+repmat(TT,1,N))';
su=0;
for i=1:N
    for j=1:3
        su=su+(inputB(i,j)-inpB(i,j))^2;
    end
end
msen=sqrt(su/N);
end