%算法理論根據(jù)?https://blog.csdn.net/u012836279/article/details/80203170?的博主提供
%輸入pointAs?和?pointBs??要求兩個點云的長度是一致，不一致將較長的截取一下即可
%輸出R旋轉(zhuǎn)矩陣，T平移矩陣
function?[RT]?=?RTbySVD（pointAspointBs）
centerPointA=[0
????0
????0];
centerPointB=centerPointA;
for?i=1:4
????centerPointA=centerPointA+pointAs（:i）;
????centerPointB=centerPointB+pointBs（:i）;
end
centerPointA=centerPointA/4;%求得了A的中心點
centerPointB=centerPointB/4;%求得了B的中心點
%step?02?消除T的影響，將點集合中心，生成A2和B2，
pointA2s=pointAs;
for?i=1:4
????pointA2s（:i）=pointAs（:i）-centerPointA;
end
pointB2s=pointBs;
for?i=1:4
????pointB2s（:i）=pointBs（:i）-centerPointB;
end
%step?03計算A2?B2的協(xié)方差矩陣H
%H=cov（[pointA2spointB2s]）;直接用這個生成的是16*16?不行，生成3*3
H=zeros（33）;
for?i=1:4
????H=H+pointA2s（:i）*pointB2s（:i）‘;
end
[USV]=svd（H）;
U=U‘;
R=V*U;%A點集合不受Zc收縮影響
%Great!!!?you?got?it
R=R*-1
T=R*centerPointA+centerPointB;




end