icp

% ICP - Bunny Example
% -----------------------------------
clear
load_vars = 'on';
clf




% TODO
% SVD - Singular Value Deposition
% ICP Algorithm

% Links
if(load_vars == 'on')
    ptCloudRef = pcread('A2_PointClouds/Pose1/1453.268000000.pcd');
    ptCloudCurrent = pcread('A2_PointClouds/Pose2/1622.950000000.pcd');

    points = {ptCloudRef, ptCloudCurrent};
    %gridStep = 0.1;
    %     ptCloudRef = pcdenoise(ptCloudRef);
    %     ptCloudCurrent = pcdenoise(ptCloudCurrent);
    %     ptCloudRef = pcdownsample(ptCloudRef,'nonuniformGridSample', 50);
    %     ptCloudCurrent = pcdownsample(ptCloudCurrent,'nonuniformGridSample', 50);
    ptCloudRef = pcnormals(ptCloudRef, 10);
    ptCloudCurrent = pcnormals(ptCloudCurrent, 10);
end


i = 1;
tf = 1;
for j = 1:size(points, 2)-1
    %selecting});
    %Add in a Z column
    %ICP Algorithm
	tf_new = pcregistericp(pointCloud(ptCloudRef),pointCloud(ptCloudCurrent),'InlierRatio', .5, 'Tolerance',[0.0001, 0.00011], 'MaxIterations', 100);
    %tf_new = pcregistericp(pointCloud(ptCloudCurrent),pointCloud(ptCloudRef),'Tolerance',[0.0001, 0.0001], 'MaxIterations', 500, 'InlierRatio', 0.97
    %returns a transformation
    tf = tf * tf_new.T';
    translation = [tf(1:3,4)];
    rotation = [tf(1:3, 1:3)];
    i = i+1;
end
% Create the transformed bunny from the current result
tform = rigid3d(rotation, translation');
ptCloudNew =  pctransform(points{2}, tform);
ptCloudNew = ptCloudNew.Location;

ptCloudCurrent = points{1}.Location;
ptCloudRef = points{2}.Location;


figure(1)
scatter3(ptCloudCurrent(:, 1), ptCloudCurrent(:, 2), ptCloudCurrent(:, 3), 'b')
hold on
scatter3(ptCloudRef(:, 1), ptCloudRef(:, 2), ptCloudRef(:, 3), 'g');
hold on
scatter3(ptCloudNew(:, 1), ptCloudNew(:, 2), ptCloudNew(:, 3), 'r');