%Function takes in A and B clusters, registers A to B using ICP, returns resultant transformation matrix and error calculation
% - Params：
%   - A:initial point cloud [n,3]
%   - B:target point cloud [m,3]
% - Return：
%   - TR:solve rotation matrix [1,3]
%   - RO:solve translation matrix [3,3]
%   - e:registers error
function [ R,T,e ] = solveicp( A,B,Mdl,R_0,T_0 )
 
%1.initialize variable parameters
k = 1;
error = 0.0001;
Iterator = 50;
R = R_0;
T = T_0;
%2.initial:kdtree algorithm to find the nearest point pair of two point clouds
trans = transform(A,R,T);
e = calculateCost(trans,B,Mdl);
%3.ICP loop
while e > error && k <= Iterator
    %find the nearest point
    idx = knnsearch(Mdl,trans);
    NN = B(idx,:);
    %svd solve: optimal matching matrix of two point clouds in current state
    [T_k, R_k] = solvesvd(trans,NN);
    %Cumulative transformation T=[1,3] R=[3,3]
    T = (R_k*T')'+T_k;
    R = R_k*R;
    trans = transform(A,R,T);
    %Update error E
    e = sum(sqrt(sum((NN-trans).^2,2)))/length(trans);
    k = k+1;
    
end

end

