%
% This code is used to display laser-scans in the corresponding image frame
% Scans colors follow the Ibeo standard.
%     
% By: CPremebida
% %-------------------------------
function yout = Demo()
format longG
clear; clc;

path_ima = './Full_frames/full_images_pos/'; %Extracted from Full_frames.zip
path_lidar = './Raw_scans/raw_scans_pos/'; %Extracted from Raw_scans.zip
% % Alternatively, you can use data from:
% % path_ima = './Full_frames/full_images_neg/';
% % path_lidar = './Raw_scans/raw_scans_neg/';
% % %-------------------------------
ima = dir([path_ima,'*.jpg']);
lidar = dir([path_lidar,'*.txt']);

figure(1); scolor = ['r', 'b', 'g', 'y'];
for k=1:1:size(ima,1)
% % %----------------------------------------
    fd_ima = fopen([path_ima,ima(k).name],'r');
     if fd_ima > 0
        Ima=imread([path_ima,ima(k).name]);
        fclose(fd_ima);
    else
        fprintf('WARNING::: file not opened: %s \n',ima(k).name);
        keyboard
     end
    
	Scan = load([path_lidar,lidar(k).name]);
     if size(Scan,2) < 10
        fprintf('WARNING::: file not ok: %s \n',lidar(k).name);
        keyboard
     end
     
% % % Laser scans: 4 channels
SCAN = make_array_lidar(Scan);

cla; imshow(Ima); hold on;
axis([0 640 0 480]);
%//===========================================
% -- Focal length:
fc = [ 623.730957401930030 ; 624.594742948671980 ];
%-- Principal point:
cc = [ 360.876661071841230 ; 256.895182562749140 ];
K = [fc(1) 0 cc(1); 0 fc(2) cc(2); 0 0 1];
Phi = [0.99986    -0.014149   -0.0093947
     0.014395      0.99954     0.026672
     0.009013    -0.026804       0.9996];
Delta = [11.917; -161.26*2;  0.77955];

L = [Phi(1,:), Delta(1); Phi(3,:), Delta(3); Phi(2,:), Delta(2);0 0 0 1];
% % % 
for r=1:size(SCAN,2)
x_laser = [SCAN(r).y*1000; SCAN(r).x*1000; -SCAN(r).z*1000];
x_img = eye(3,4)*(L\[x_laser; ones(1,size(x_laser,2))]);    
x_img = x_img ./ (ones(3,1)*x_img(3,:));
x_img = K * x_img;
plot(x_img(1,:),x_img(2,:),'+','Color',scolor(r),'MarkerSize',2);

end  
pause(0.1); %for display purpose
% %----------------------------------------
% %----------------------------------------  
fprintf('---> Frame %d of %d\n',k,size(ima,1));

end

yout = true;
end %Main function
% % % =====================================================================

function SCAN = make_array_lidar(scan)
SCAN(1:4) = struct('x', {[]}, 'y', {[]}, 'z', {[]});

npts = size(scan,2); %Number of measurement points
i = 1:3:npts;   scan_y = scan(i);
i = 2:3:npts;   scan_x = scan(i);
i = 3:3:npts;   scan_z = scan(i);
%//------------------------------------------------------------------------
pos_z = scan_z > 0;
neg_z = scan_z < 0;

pos_scan_x = scan_x(pos_z); pos_scan_y = scan_y(pos_z); pos_scan_z = scan_z(pos_z);
neg_scan_x = scan_x(neg_z); neg_scan_y = scan_y(neg_z); neg_scan_z = scan_z(neg_z);

theta_e = 1.2*pi/180;
theta_i = 0.4*pi/180;
sinTheta = sin(theta_i);
n = sum(pos_z);
pt_i = false(1,n);
pt_e = false(1,n);
for k=1:n
    thr_i = pos_scan_x(k)*sinTheta;
    if pos_scan_z(k) > 1.5*thr_i
        pt_e(k) = true;
    else
        pt_i(k) = true;
    end
end

scan_x3 = pos_scan_x(pt_i);  scan_y3 = pos_scan_y(pt_i);  scan_z3 = pos_scan_z(pt_i);
scan_x4 = pos_scan_x(pt_e);  scan_y4 = pos_scan_y(pt_e);  scan_z4 = pos_scan_z(pt_e);

%=====
scan_y3 = scan_y3 * cos(theta_i);
scan_y4 = scan_y4 * cos(theta_e);


n = sum(neg_z);
pt_i = false(1,n);
pt_e = false(1,n);
for k=1:n
    thr_i = neg_scan_x(k)*sinTheta;
    if abs(neg_scan_z(k)) > 1.5*thr_i
        pt_e(k) = true;
    else
        pt_i(k) = true;
    end
end
scan_x1 = neg_scan_x(pt_e);  scan_y1 = neg_scan_y(pt_e);  scan_z1 = neg_scan_z(pt_e);
scan_x2 = neg_scan_x(pt_i);  scan_y2 = neg_scan_y(pt_i);  scan_z2 = neg_scan_z(pt_i);

%NEW =====
scan_y2 = scan_y2 * cos(theta_i);
scan_y1 = scan_y1 * cos(theta_e);

SCAN(1).x = scan_x1;
SCAN(1).y = scan_y1;
SCAN(1).z = scan_z1;

SCAN(2).x = scan_x2;
SCAN(2).y = scan_y2;
SCAN(2).z = scan_z2;

SCAN(3).x = scan_x3;
SCAN(3).y = scan_y3;
SCAN(3).z = scan_z3;

SCAN(4).x = scan_x4;
SCAN(4).y = scan_y4;
SCAN(4).z = scan_z4;
end