Upload New File

SPR.m

+% SPR Main logic function:transform A sparse point cloud into B model point cloud coordinate system for registration
+% - Params：
+%   - A:sparse point cloud [n,3]
+%   - B:model point cloud [m,3]
+% - Return：
+%   - R:optimal rotation matrix [3,3]
+%   - T:optimal translation matrix [1,3]
+%   - trans_res:transform point cloud [n,3]
+function [ R,T,trans_res ] = SPR(A,B)
+
+%1.kdtree model for B(euclidean distance) and size of the model
+Mdl = KDTreeSearcher(B);
+Size = max(range(B));
+%2.Initialize R0,T0,e0
+R = eye(3);
+%Align the centroids of two point clouds A->B
+cen_A = sum(A)/length(A);
+cen_B = sum(B)/length(B);
+T = cen_B - cen_A;
+trans_res = transform(A,R,T);
+e = calculateCost(trans_res,B,Mdl);
+%3.Initialize the number of disturbed pose P, the maximum number of iterations I, and the minimum error threshold RMS
+P = 10;
+I = 30;
+RMS = 1e-6;
+%4.Initialize loop variable
+k = 1;
+%record data
+x_loop = 1:I;
+y_error = zeros(1,I);
+
+%5.SPR Loop
+while e > RMS && k <= I
+    cost_best = inf;
+    R_k = R;
+    T_k = T;
+    %(1). Perturbations for Sparse point cloud,and then the perturbation transformation matrix is obtained
+    % - R_p: [3,3,P+1]
+    % - T_p: [p+1,3]
+    [R_p, T_p] = perturb(Size,P,k,I);
+    %(2). Calculate the optimal pose
+    for j = 1:P+1
+        %Obtain the transformation matrix parameters rp_j(3,3), tp_j(1,3) of the current p-th disturbance pose
+        rp_j = R_p(:,:,j)*R;
+        tp_j = (R_p(:,:,j)*T')'+T_p(j,:);
+        %Calculate current optimal cost(from ICP)
+        [R_j,T_j,cost_j] = solveicp(A,B,Mdl,rp_j,tp_j);
+        %judge optimal cost
+        if cost_j < cost_best
+            cost_best = cost_j;
+            R_k = R_j;
+            T_k = T_j;
+        end
+    end
+    %(3). Error calculation and judge for update
+    trans_k = transform(A,R_k,T_k);
+    e_k = calculateCost(trans_k,B,Mdl);
+    if e_k < e
+        e = e_k;
+        R = R_k;
+        T = T_k;
+    end
+    y_error(k) = e;
+    k = k+1;
+end
+trans_res = transform(A,R,T);
+
+figure
+plot(x_loop,y_error,marker='o', MarkerFaceColor='r');
+xlabel('loop');
+ylabel('ems error');
+
+end
\ No newline at end of file