%function: svd solve yo find T and R between point set B and it nearest neighbour set
% - Params：
%   - B:initial point clouds [n,3]
%   - NN: target nearest neighbour point clouds [n,3]
% - Return：
%   - T:solve optimal translation matrix [1,3]
%   - R:solve optimal rotation matrix [3,3]
function [ T, R ] = solvesvd( B, NN )

num = length(B);
%1.Seeking centroid
cen_B = sum(B)/num;
cen_NN = sum(NN)/num;
%2.Decentralization:Set centroids as reference origin for both clusters
B2 = B-repmat(cen_B,num,1);
NN2 = NN-repmat(cen_NN,num,1);
%3.svd decompose:[U,S,V] = svd(A) ==> A = U*S*V'
H = B2'*NN2;
[U,~,V] = svd(H);
R = V*diag([1 1 sign(det(V*U'))])*U';
T = cen_NN' - R*cen_B';
T = T';

end

