function convertPoseData(folder,outfile)
%convertPoseData Load pose data and convert to Raul format
%
% convertPoseData(folder,outfile)
%
% Read the all files in folder, and convert the Stereo Pedestrian Pose
% Estimator results to Raul desired format.
% Read from the folder and write as a single file into outfile

files = dir(fullfile(folder,'*.txt'));

rot = rotation_angle_axis(-pi/2,[0 0 1])*rotation_angle_axis(-pi/2,[1 0 0]);

fid = fopen(outfile,'w');
% fprintf(fid,'# Stereo Pedestrian Pose Estimator v0.1\n');
fprintf(fid,['Field Time head:x head:y head:z neck_base:x neck_base:y neck_base:z ' ...
            'left_shoulder:x left_shoulder:y left_shoulder:z right_shoulder:x right_shoulder:y right_shoulder:z ' ...
            'center_torso:x center_torso:y center_torso:z left_torso:x left_torso:y left_torso:z ' ...
            'right_torso:x right_torso:y right_torso:z waist:x waist:y waist:z left_hip:x left_hip:y left_hip:z ' ...
            'right_hip:x right_hip:y right_hip:z left_knee:x left_knee:y left_knee:z right_knee:x right_knee:y right_knee:z ' ...
            'left_foot:x left_foot:y left_foot:z right_foot:x right_foot:y right_foot:z\n']);

for i=1:length(files)
    file_name = fullfile(folder,files(i).name);
    
    try
    pose = dlmread(file_name)';
    catch
        break
    end
    
    %Rotate to correct switch axis
    pose = rot*pose;
    
    %Convert to milimeters
    pose = pose*1000;

    %Extract individual elements
    head = pose(:,1);
    neck_base = pose(:,2);
    left_shoulder = pose(:,3);
    right_shoulder = pose(:,4);
    center_torso = pose(:,5);
    left_torso = pose(:,6);
    right_torso = pose(:,7);
    waist = pose(:,8);
    left_hip = pose(:,9);
    right_hip = pose(:,10);
    left_knee = pose(:,11);
    right_knee = pose(:,12);
    left_foot = pose(:,13);
    right_foot = pose(:,14);
    
    if ~isempty(find(isnan(pose), 1))
        continue
    end
    
    fprintf(fid,'1 %.3f ',0);
    fprintf(fid,'%.3f %.3f %.3f ',head(1),head(2),head(3));
    fprintf(fid,'%.3f %.3f %.3f ',neck_base(1),neck_base(2),neck_base(3));
    fprintf(fid,'%.3f %.3f %.3f ',left_shoulder(1),left_shoulder(2),left_shoulder(3));
    fprintf(fid,'%.3f %.3f %.3f ',right_shoulder(1),right_shoulder(2),right_shoulder(3));
    fprintf(fid,'%.3f %.3f %.3f ',center_torso(1),center_torso(2),center_torso(3));
    fprintf(fid,'%.3f %.3f %.3f ',left_torso(1),left_torso(2),left_torso(3));
    fprintf(fid,'%.3f %.3f %.3f ',right_torso(1),right_torso(2),right_torso(3));
    fprintf(fid,'%.3f %.3f %.3f ',waist(1),waist(2),waist(3));
    fprintf(fid,'%.3f %.3f %.3f ',left_hip(1),left_hip(2),left_hip(3));
    fprintf(fid,'%.3f %.3f %.3f ',right_hip(1),right_hip(2),right_hip(3));
    fprintf(fid,'%.3f %.3f %.3f ',left_knee(1),left_knee(2),left_knee(3));
    fprintf(fid,'%.3f %.3f %.3f ',right_knee(1),right_knee(2),right_knee(3));
    fprintf(fid,'%.3f %.3f %.3f ',left_foot(1),left_foot(2),left_foot(3));
    fprintf(fid,'%.3f %.3f %.3f ',right_foot(1),right_foot(2),right_foot(3));
    fprintf(fid,'\n');
end