function valid
close all;
clc;
clear all;
cd ('C:\Users\xiahaa\Videos\img_6th_1440_30_depth');

%img_2nd_27K_60
%img_3rd_1440_30
%img_4th_1440_30_depth
%img_5th_2.7k_30_depth
%img_6th_1440_30_depth

vdatam = load('vdata_s.mat');
vdatam = vdatam.position;
vdatam = vdatam(:,2:end);

gdtruth = [[0,0,0];...
    [0.214 0 0];...
    [0.214*2 0 0];...
    [0.214*2+0.205 0 0];...
    [0 0.240 0];...
    [0.214 0.240 0];...
    [0.214*2 0.240 0];...
    [0.214*2+0.205 0.240 0];...
    [-0.305 0.240 -0.245]];

% meas1 = vdatam(:,6*4+2:6*4+4);
% meas2 = vdatam(:,4*4+2:4*4+4);
% meas3 = vdatam(:,2*4+2:2*4+4);
% meas4 = vdatam(:,0*4+2:0*4+4);
% meas5 = vdatam(:,7*4+2:7*4+4);
% meas6 = vdatam(:,5*4+2:5*4+4);
% meas7 = vdatam(:,3*4+2:3*4+4);
% meas8 = vdatam(:,1*4+2:1*4+4);
% % meas9 = vdatam(:,8*4+2:8*4+4);

meas1 = vdatam(:,6*4+2:6*4+4);
meas2 = vdatam(:,4*4+2:4*4+4);
meas3 = vdatam(:,2*4+2:2*4+4);
meas4 = vdatam(:,0*4+2:0*4+4);
meas5 = vdatam(:,7*4+2:7*4+4);
meas6 = vdatam(:,5*4+2:5*4+4);
meas7 = vdatam(:,3*4+2:3*4+4);
meas8 = vdatam(:,1*4+2:1*4+4);
meas9 = vdatam(:,8*4+2:8*4+4);


err = zeros(size(vdatam,1),1);
for i=1:1:size(vdatam,1)
    dst = gdtruth';
    
%     z = [meas1(i,3);meas2(i,3);meas3(i,3);meas4(i,3);meas5(i,3);meas6(i,3);meas7(i,3);meas8(i,3);meas9(i,3)];
%     [minval, minid] = min(z);
%     if minid ~= 9
%         continue;
%     end
%     id = 1:1:size(z,2);
%     id(id == minid) = 0;
%     zothers = z(id~=0);
%     zmean = mean(zothers);
%     err1 = (minval - zmean) - dst(3,end);
%     err(i) = norm(err1);
    
%     src = [meas1(i,:);meas2(i,:);meas3(i,:);meas4(i,:);meas5(i,:);meas6(i,:);meas7(i,:);meas8(i,:)]';%;meas9(i,:)]';
    src = [meas1(i,:);meas2(i,:);meas3(i,:);meas4(i,:);meas5(i,:);meas6(i,:);meas7(i,:);meas8(i,:);meas9(i,:)]';

    [R,t] = estimate_rigid_body_transformation_Horn(src,dst);
    dstt = R*src + repmat(t,1,size(src,2));
    err1 = dst - dstt;
    sum_error_norm = sum(sqrt(diag(err1'*err1)))/8;
%     if sum_error_norm > 0.2
%         continue;
%     end
    
    err(i) = sum_error_norm;
end
mean(err)
std(err)

outlier = abs(err - mean(err)) > 3*std(err);
err = err(~outlier);
mean(err)
std(err)

plot(err,'b--o');
title('Error Norm');
grid on;
end

function [Ropt,topt] = estimate_rigid_body_transformation_Horn(ptsrc, ptdst)

ptsrcmean = mean(ptsrc')';
ptdstmean = mean(ptdst')';

ptsrcrefine = ptsrc - repmat(ptsrcmean, 1, size(ptsrc,2));
ptdstrefine = ptdst - repmat(ptdstmean, 1, size(ptsrc,2));
M = [ptsrcrefine(1,:)*ptdstrefine(1,:)' ptsrcrefine(1,:)*ptdstrefine(2,:)' ptsrcrefine(1,:)*ptdstrefine(3,:)';...
     ptsrcrefine(2,:)*ptdstrefine(1,:)' ptsrcrefine(2,:)*ptdstrefine(2,:)' ptsrcrefine(2,:)*ptdstrefine(3,:)';...
     ptsrcrefine(3,:)*ptdstrefine(1,:)' ptsrcrefine(3,:)*ptdstrefine(2,:)' ptsrcrefine(3,:)*ptdstrefine(3,:)'];

% 3 N
N = [M(1,1)+M(2,2)+M(3,3) M(2,3)-M(3,2) M(3,1)-M(1,3) M(1,2)-M(2,1);...
     M(2,3)-M(3,2) M(1,1)-M(2,2)-M(3,3) M(1,2)+M(2,1) M(3,1)+M(1,3);...
     M(3,1)-M(1,3) M(1,2)+M(2,1) -M(1,1)+M(2,2)-M(3,3) M(2,3)+M(3,2);...
     M(1,2)-M(2,1) M(3,1)+M(1,3) M(2,3)+M(3,2) -M(1,1)-M(2,2)+M(3,3)];
 
[evec, eval] = eig(N);
evalvec = [eval(1,1);eval(2,2);eval(3,3);eval(4,4)];
[maxeval, maxid] = max(evalvec);
q = evec(:,maxid);

Ropt = q2r(q);

topt = ptdstmean - Ropt * ptsrcmean; 
 
end

function R = q2r(q)

R = [q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4) 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));...
     2*(q(2)*q(3)+q(1)*q(4)) q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4) 2*(q(3)*q(4)-q(1)*q(2));...
     2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4)];

end



