
wd='/Users/jpc/Dropbox/PIV/piv2014-15/projecto/codigoaquisicao/data_rgb/';%working Directory
dtag=dir([wd 'tag*.png']);%get file names for tag images
dd=dir([wd 'depth*.mat']);%get file names for depth images
drgb=dir([wd 'rgb_image*']);%get file names for rgb images
[aux1,aux2]=sort(rand(480*640,1));%Select randomly a few points from the total of 480*640
inds_to_plot=aux2(1:48*64);%pick 1/100 of the total number of points
load /Users/jpc/Dropbox/PIV/piv2014-15/webprojecto/stuff/camParams; %kinect parameters
alpha=a;beta=b;
load /Users/jpc/Dropbox/PIV/piv2014-15/webprojecto/stuff/arucobox;%definitions of the box
load /Users/jpc/Dropbox/PIV/piv2014-15/webprojecto/stuff/camara-aruco-intrinsics.mat %Intrinsics of camera watching the box
load /Users/jpc/Dropbox/PIV/piv2014-15/webprojecto/stuff/camToBox % Calibration parameter between kinect and arucobox
colors=[1 0 0;0 1 0;0 0 1;1 1 0; 1 0 1; 0 1 1];
figure(4);clf;
ArucoSize=0.088;
%transfms={};%store all data (for debug and future use!
load transformations %This is the solution :-)...this data is the outcome of your project!This line out
for inew=1:length(dd),
%%% Display the rgb image
    figure(1);
    imagesc(imread([wd drgb(inew).name]));
%%% image of the box - tags
    figure(2);
    im=imread([wd dtag(inew).name]);
    %You should check this on your own computer. I have processed it in
    %advance -
    %[im_mk_id, ind, xy_mkrs, ~, ~] = mex_aruco(im, CamMatrix, DistCoef, length(im(1, :,1)), length(im(:, 1,1)), ArucoSize);
    load(['mattag_' dtag(inew).name(1:end-4)]);% this line out!
    imagesc(im_mk_id);%Show the markers detection
    %Forget about this->save(['mattag_' dtag(inew).name(1:end-4)],'im_mk_id','ind','xy_mkrs');
    if length(ind)>=2,% at least 2 markers detected otherwise too imprecise.
        load([wd dd(inew).name]);% load depth map - 
        xyz=get_xyz_asus(depth_array(:),[480 640], depth_array(:)~=0,K_ir,1,0);%Compute 3D coordinates
        figure(3);%let's see the depth value
        imagesc(depth_array)
        %reduce number of points - use the sample defined earlier
        xyz=xyz(inds_to_plot,:);
        %%%%% Compute kinect orientation and position
        %This is stuff you should do[R_temp, T_temp, ~, ~, ~,xyzbase] = estima_posicao_globalbeatriz(im, CamMatrix, DistCoef, ArucoSize, ids, mapa_RT);
        %This is stuff you should do R=R_temp';T=-R_temp'*T_temp; 
        R=transfms{inew}.R;%I computed it before
        T=transfms{inew}.T;% I computed it before
        xyzbase=transfms{inew}.xyzbase;%I computed it before
        xyz_tag2=R*(Rcalib*xyz'+repmat(Tcalib,1,size(xyz,1)))+repmat(T,1,size(xyz,1));
        fprintf(' Ploting image %s \n',dtag(inew).name);
        %%%%%cross check. Just project back the corner points and the origin to see if they plot
        %%%%%in the right place
        ll=CamMatrix*[R T]*[[xyzbase';ones(1,size(xyzbase',2))] [0 0 0 1]'];
        figure(2);hold on; plot(ll(1,:)./ll(3,:),ll(2,:)./ll(3,:),'r*');hold off;
        figure(4);hold on;%plot the points in the camera reference frame
        plot3(xyz_tag2(1,:),xyz_tag2(2,:),xyz_tag2(3,:),'.','Color', colors(fix(rem(inew,6)+1),:));axis equal
        axis([ -5.4944    5.5534   -0.1847    3.4259   -4.5455    8.6894]); view(2.0091,-3.5574);
        %transfms{inew}.R=R;
        %transfms{inew}.T=T;
        %transfms{inew}.xyz=xyz;
        %transfms{inew}.xy_mkrs=xy_mkrs;
        %transfms{inew}.xyzbase=xyzbase;
        %transfms{inew}.xyzcamera=xyz_tag2;
        
    end
   
    hold off
end

