close all;
clear;
clc;

%Input
bun000 = pcread('bunny/data/bun000.ply');
bun090 = pcread('bunny/data/bun045.ply');

%Ground Truth
b045 = [-0.0520211 -0.000383981 -0.0109223 0.00548449 -0.294635 -0.0038555 0.955586];
b090 = [2.20761e-05 -3.34606e-05 -7.20881e-05 0.000335889 -0.708202 0.000602459 0.706009];
b180 = [0.000116991 2.47732e-05 -4.6283e-05 -0.00215148 0.999996 -0.0015001 0.000892527];
b270 = [0.000130273 1.58623e-05 0.000406764 0.000462632 0.707006 -0.0033301 0.7072];
b315 = [-0.00646017 -1.36122e-05 -0.0129064 0.00449209 0.38422 -0.00976512 0.923179];

b = b045;

figure(1);
pcshow(bun000);
title('Bunny 1');
xlabel('x');
ylabel('y');
zlabel('z');

figure(2);
pcshow(bun090);
title('Bunny 2');
xlabel('x');
ylabel('y');
zlabel('z');

quat = [b(7) b(4) b(5) b(6)];
%these are in the form of rot(data - tran) = model
%we want them in form: rot*data + tran = model.. therefore tran = -rot*tran
rot = quat2rotm(quat);
tran = b(1:3);
rot1 = inv(rot);
tran1 = (-rot*tran')';

tform1 = affine3d([rot', [0 0 0]'; tran1 1]); %for some reason the matlab functions expect inverse of the rotation...

%Perform ICP using ground truth as initial guess
[icp_tform, moving_reg, rmse] = pcregrigid(bun000, bun090, 'Extrapolate', false, 'InitialTransform', tform1, 'MaxIterations', 1000);

disp(tform1.T)
disp(icp_tform.T)
% The rms error of the Euclidean distance between the aligned point clouds
% rmse

bun000_icp_tform = pctransform(bun000, icp_tform);
%bun000_icp_tform = pointCloud((rot*bun000.Location' + repmat(tran1', 1, length(bun000.Location)))');

%Display the overlap of transformed bunny
figure(3);
hold on;
title('ICP Bunny');
pcshow(bun000_icp_tform);
pcshow(bun090);
xlabel('x');
ylabel('y');
zlabel('z');
hold off;

% Using standard Levenberg-Marquardt
rot = rot;% + 0.01*randn(size(rot)) + 0.01;
a0 = [rotm2eul(eye(3)) tran1];
a = lmicp_our_3d(bun090.Location, bun000.Location, a0, 1);

rot = eul2rotm(a(1:3));
tran = a(4:6);

bun000_lmicp_tform = pointCloud((rot*bun000.Location' + repmat(tran', 1, length(bun000.Location)))');

%Display the overlap of transformed bunny
figure(5);
hold on;
title('LM-ICP Bunny');
pcshow(bun000_lmicp_tform);
pcshow(bun090);
xlabel('x');
ylabel('y');
zlabel('z');
hold off;
