% 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