    
clear all
% close all
clc
 
% data robotu   
data_stroj = xlsread('body_csys_align_robot.csv', 'A2:C9');

% data trackeru
data_Leica_RHR = xlsread('csys_align_03.csv','B2:D9');

%prepocet ze RHR souradneho systemu Leicy do souradneho systemu stroje
%       souradnice_ss_srtoj = R*e_souradnice_ssRHR_Leica + T
%
%soubor...string cesta a nazev textoveho souboru s namerenymi daty
%format souboru:
%         Hlavicka:
%         1. radek v souboru: jakykoliv text, napr popis pro identifikaci merení…
%         2. radek v souboru: #    x             y             z              fiH          fiV          D             
% 
%         Vlastni data zacinaji na 3. rádku (tecka jako oddelovac desetinnych mist): 
%         Poradi bodu       souradnice_stroj_X       souradnice_stroj_Y   souradnice_stroj_Z   souradnice_Leica_H    souradnice_Leica_V   souradnice_Leica_D
% 
%         Priklad
% 
%         140226 poloha trackeru 1, roh stolu
%         #             x             y             z              fiH          fiV          D
%         1             2000.500         758.590              28.589                   95.897895           32.57896              2056.7321
%         2             356.500           790.590              359.6615              78.897895           45.57896              30048.732
% 
%
%souradny system trackeru SCW, jednotky [rad,m]
%jednotky stroj [mm]
%
%vystupni arg.: R...matice rotace 3x3
%               T...vektor posunuti

R=[];
T=[];

%prepocet souradnic z trackeru do kartezskeho systemu
%data_Leica_RHR =  Leica_SCW2RHR(data_Leica_SCW);
%prepocet dat z leicy na [mm];
data_Leica_RHR = data_Leica_RHR*1000;

[s R T error] = absoluteOrientationQuaternion( data_Leica_RHR', data_stroj', 0);

%% vykresleni chyby rekonstrukce
% rozdil rekonstrukovanych dat a nominalnich
% diffRecon = LeicaSCW2StrojXYZ(R,T,data_Leica_SCW) - data_stroj;
% for i=1:size(diffRecon,1)
%     diffReconNorm(i) = norm(diffRecon(i,:));
% end
% figure
% plot(diffReconNorm,'--oc')
% 
% leicaInRobotCsys = LeicaSCW2StrojXYZ(R,T,data_Leica_SCW);
% figure
% plot3(data_stroj(:,1),data_stroj(:,2),data_stroj(:,3),'xk'), hold on
% plot3(leicaInRobotCsys(:,1),leicaInRobotCsys(:,2),leicaInRobotCsys(:,3),'or')
% grid


