Upload New File

solveicp.m

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