Upload New File

solvesvd.m

+%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
+