61 lines
1.8 KiB
Matlab
61 lines
1.8 KiB
Matlab
|
|
% 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)
|
|
|