%
% Author: Chieh-Chih (Bob) Wang [bobwang@cs.cmu.edu]
% Created: Oct. 25, 2002.
% Modified: May 10, 2004

clear all

SICK_filename = 'Loop1\SICKFront.txt';
Pose_filename = 'Loop1\VehState.txt';

% Loading SICK data
disp('Loading SICK data')
tempdata = load(SICK_filename);
N_scan = size(tempdata,1)/363;
SICK.data = zeros(N_scan,361);
SICK.time = zeros(N_scan,1);
for i=1:N_scan
    SICK.data(i,:) = tempdata(1+(i-1)*363:361+(i-1)*363)';    
    SICK.time(i,1) = tempdata(362+(i-1)*363) +tempdata(363+(i-1)*363)/1000000;
end
clear tempdata;

% Loading Navlab11 Pose data
% -- data format --
%  1. vehstate.secs 
%  2. vehstate.usecs
%  3. vehstate.x 
%  4. vehstate.y 
%  5. vehstate.z 
%  6. vehstate.roll 
%  7. vehstate.pitch
%  8. vehstate.yaw 
%  9. vehstate.speed
% 10. vehstate.radius 
% 11. vehstate.dist 
% 12. vehstate.gps_status 	
% 13. vehstate.x_origin 
% 14. vehstate.y_origin 
% 15. vehstate.z_origin 
% 16. vehstate.cm 
% 17. vehstate.error 
disp('Loading Pose data')
tempdata = load(Pose_filename);
N_pose = size(tempdata,1);
Pose.time = zeros(N_pose,1); 
Pose.x = zeros(N_pose,1);
Pose.y = zeros(N_pose,1);
Pose.yaw = zeros(N_pose,1);
for i=1:N_pose        
    Pose.time(i,1)= tempdata(i,1) +tempdata(i,2)/1000000;
    Pose.x(i,1) = tempdata(i,3);
    Pose.y(i,1) = tempdata(i,4);
    Pose.yaw(i,1) = tempdata(i,8);    
end
clear tempdata;

% Show Pose Data
figure(1)
clf
plot(Pose.y,Pose.x,'k')
axis equal
xlabel('meter')
ylabel('meter')
title('Pose Data')


% Displaying SICK data
disp('Displaying SICK data')
angle = 90:-0.5:-90;
angle = angle*pi/180; 
for i=1:N_scan
    x = SICK.data(i,:).*cos(angle);
    y = SICK.data(i,:).*sin(angle);    
    figure(2)
    clf
    hold on    
    plot(y,x,'.')
    axis([-4000 4000 0 6000])
    xlabel('cm')
    ylabel('cm')
    title(sprintf('SICK Data: scan %d',i))
    pause(0.01)
end