initial commit

ov_data/kaist/matlab/kaist_convert_groundtruth.m

@@ -0,0 +1,60 @@
+
+% clear and close old
+close all;
+clear all;
+
+% our data file
+path_main = '../';
+filename_in = 'raw/urban39_global_pose.csv';
+filename_out = 'urban39';
+
+%% Vehicle2IMU.txt
+% IMU extrinsic calibration parameter from vehicle
+% The groundtruth poses are in the vehicle frame while we need the imu
+p_IinV = [-0.07; 0; 1.7];
+R_ItoV = [1 0 0; 0 1 0; 0 0 1];
+T_VtoI = [...
+    R_ItoV' -R_ItoV'*p_IinV;
+    0 0 0 1
+];
+
+
+
+%% Load the groundtruth file
+disp('Loading groundtruth...')
+data_e = {};
+data_e{length(data_e)+1} = importdata([path_main,filename_in],',',1);
+temp_data = data_e{end}.data;
+data_e{length(data_e)}.data = zeros(size(temp_data,1),8);
+for jj=1:size(temp_data,1)
+    % timestamp is in nanoseconds so convert to our seconds
+    data_e{length(data_e)}.data(jj,1) = 1e-9*temp_data(jj,1);
+    % next get our transform
+    T_VtoUTM = [temp_data(jj,2:5); temp_data(jj,6:9); temp_data(jj,10:13); 0 0 0 1];
+    T_ItoUTM = T_VtoUTM*inv(T_VtoI);
+    % store in our vector
+    data_e{length(data_e)}.data(jj,2:4) = T_ItoUTM(1:3,4)';
+    data_e{length(data_e)}.data(jj,5:8) = rot2quat(T_ItoUTM(1:3,1:3)');
+end
+
+
+
+% finally write groundtruth data
+fprintf('Outputing groundtruth (RPE format)...\n')
+filename = [path_main,filename_out,'.txt'];
+file = fopen([filename], 'w');
+for t=1:size(data_e{end}.data(:,1:8),1)
+    fprintf(file, '%0.9f %0.6f %0.6f %0.6f %0.8f %0.8f %0.8f %0.8f\n',data_e{end}.data(t,1:8));
+end
+fprintf('   + saved %s\n',filename)
+
+
+% finally write groundtruth data
+fprintf('Outputing groundtruth (ETH format)...\n')
+filename = [path_main,filename_out,'.csv'];
+file = fopen([filename], 'w');
+for t=1:size(data_e{end}.data(:,1:8),1)
+     fprintf(file, '%0.9f,%0.6f,%0.6f,%0.6f,%0.8f,%0.8f,%0.8f,%0.8f\n',1e9*data_e{end}.data(t,1),data_e{end}.data(t,2:4),data_e{end}.data(t,8),data_e{end}.data(t,5:7));
+end
+fprintf('   + saved %s\n',filename)
+