%
% This code is used to display LIDAR scans in the corresponding image frame
% Scans colors follow the Ibeo standard.
%     
% By: CPremebida
%-------------------------------
format longG
clear; clc;
path_ima = './ISRtest_frames/';     %Extracted from ISRtest_frames.zip
path_lidar = './ISRtest_LIDARlog/'; %Extracted from ISRtest_LIDARlog.zip
% % %-------------------------------
ima = dir([path_ima,'*.jpg']);
lidar = dir([path_lidar,'*.txt']);

SCAN(1:4) = struct('x', {[]}, 'y', {[]}, 'z', {[]},'r', {[]});
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
     
    fd_lidar = fopen([path_lidar,lidar(k).name],'r');
     if fd_lidar > 0
        Scan=fscanf(fd_lidar,'%c');
        fclose(fd_lidar);
    else
        fprintf('WARNING::: file not opened: %s \n',lidar(k).name);
        keyboard
     end
     
% % % LIDAR scans | 
Tetha = [-1.6 -0.8 0.8 1.6]*pi/180;
is = find(Scan(1:40) == 10);
scan_header = str2num(Scan(1:is(1))); %time_stamp, ini_angle, end_angle, n_points
Scan = str2num(Scan(is(2):end));
% Note: size(Scan,1) == scan_header(4)
for r=1:4
is = Scan(:,1) == r-1;   
SCAN(r).x = Scan(is,4); SCAN(r).y = Scan(is,5); 
SCAN(r).r = sqrt(SCAN(r).x .^2 + SCAN(r).y .^2); SCAN(r).z = SCAN(r).r*tan(Tetha(r));    
end
% % % 
cla;
imshow(Ima); hold on;
%//===========================================
fc = [ 622.06 ; 622.56 ];   cc = [ 366.08 ; 258.92];
K = [fc(1) -0.05 cc(1); 0 fc(2) cc(2); 0 0 1];
Delta = [-18.592; -259.84; -8.2989];
Phi = [0.999 0.05 0.0001
       -0.005  0.998 0.001
       0.008  -0.018  0.984]; %Approx. Phi
   
% % L = [1 0 0, Delta(1);0 0 1, Delta(3);0 1 0, Delta(2);0 0 0 1];   
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*10, SCAN(r).x*10, -SCAN(r).z*10]';
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    
axis([0 640 0 480]);
pause(0.1); %for display purpose
%----------------------------------------
%----------------------------------------  
fprintf('---> Frame %d of %d\n',k,size(ima,1));
end

