initial commit

ov_data/kaist/matlab/kaist_calculate_imu2cam.m

@@ -0,0 +1,71 @@
+
+clear all;
+close all;
+
+
+% load their stereo parameters
+% looks like they used the matlab calibration tool to get it.
+% https://www.mathworks.com/help/vision/ref/stereoparameters.html#d117e94134
+load ../raw/stereoParams.mat
+%stereoParams = toStruct(stereoParams);
+
+% transform between the left (stereo pair base) and the right camera
+% the units of the stereo parameters are millimeters
+% need to get R_1to2 and p_1in2
+R_2to1 = stereoParams.RotationOfCamera2;
+p_2in1 = stereoParams.TranslationOfCamera2'/1000;
+T_LtoR = [...
+    R_2to1' p_2in1;
+    0 0 0 1
+];
+
+
+% T_LtoR = zeros(4,4);
+% T_LtoR(1,1) = 1;
+% T_LtoR(1,4) = -0.4751;
+% T_LtoR(2,2) = 1;
+% T_LtoR(3,3) = 1;
+% T_LtoR(4,4) = 1;
+
+
+% Vehicle2Stereo.txt
+% Stereo camera (based on left camera) extrinsic calibration parameter from vehicle
+% WHAT????? THEIR POSITION IS INCORRECTLY THE OPOSITE
+% DIRECTION????!@#!@#?!@#!@#$!@$%!#@%!#%!~@%!@#%@!#^%#@%^!@#%@#%
+p_LinV = [1.66944; 0.278027; 1.61215];
+R_LtoV = [-0.00413442 -0.0196634 0.999798; -0.999931 -0.0109505 -0.00435034; 0.0110338 -0.999747 -0.0196168];
+T_VtoL = [...
+    R_LtoV' -R_LtoV'*p_LinV;
+    0 0 0 1
+];
+
+
+% Vehicle2IMU.txt
+% IMU extrinsic calibration parameter from vehicle
+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
+];
+
+
+
+% calculate the transform between the camneras and the IMU
+T_LtoI = T_VtoI*inv(T_VtoL);
+T_RtoI = T_VtoI*inv(T_LtoR*T_VtoL);
+
+
+% print it out
+fprintf('T_C0toI = \n');
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_LtoI(1,1),T_LtoI(1,2),T_LtoI(1,3),T_LtoI(1,4));
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_LtoI(2,1),T_LtoI(2,2),T_LtoI(2,3),T_LtoI(2,4));
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_LtoI(3,1),T_LtoI(3,2),T_LtoI(3,3),T_LtoI(3,4));
+fprintf('%.5f,%.5f,%.5f,%.5f\n',T_LtoI(4,1),T_LtoI(4,2),T_LtoI(4,3),T_LtoI(4,4));
+
+fprintf('T_C1toI = \n');
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_RtoI(1,1),T_RtoI(1,2),T_RtoI(1,3),T_RtoI(1,4));
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_RtoI(2,1),T_RtoI(2,2),T_RtoI(2,3),T_RtoI(2,4));
+fprintf('%.5f,%.5f,%.5f,%.5f,\n',T_RtoI(3,1),T_RtoI(3,2),T_RtoI(3,3),T_RtoI(3,4));
+fprintf('%.5f,%.5f,%.5f,%.5f\n',T_RtoI(4,1),T_RtoI(4,2),T_RtoI(4,3),T_RtoI(4,4));
+

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)
+

ov_data/kaist/matlab/rot2quat.m

@@ -0,0 +1,45 @@
+function q = rot_matrix_to_quaternion(R)
+% converts a rotational matrix to a unit quaternion, according to JPL
+% procedure (Breckenridge Memo)
+
+T = trace(R);
+
+[dummy maxpivot] = max([R(1,1) R(2,2) R(3,3) T]);
+
+switch maxpivot
+    case 1
+        q(1) = sqrt((1+2*R(1,1)-T)/4);
+        q(2:4) = 1/(4*q(1)) * [R(1,2)+R(2,1);
+                               R(1,3)+R(3,1);
+                               R(2,3)-R(3,2) ];
+        
+    case 2
+        q(2) = sqrt((1+2*R(2,2)-T)/4);
+        q([1 3 4]) = 1/(4*q(2)) * [R(1,2)+R(2,1);
+                                   R(2,3)+R(3,2);
+                                   R(3,1)-R(1,3) ];
+                               
+    case 3
+        q(3) = sqrt((1+2*R(3,3)-T)/4);
+        q([1 2 4]) = 1/(4*q(3)) * [R(1,3)+R(3,1);
+                                   R(2,3)+R(3,2);
+                                   R(1,2)-R(2,1) ];
+                               
+    case 4
+        q(4) = sqrt((1+T)/4);
+        q(1:3) = 1/(4*q(4)) * [R(2,3)-R(3,2);
+                               R(3,1)-R(1,3);
+                               R(1,2)-R(2,1) ];
+                           
+end % switch
+
+% make column vector
+q = q(:); 
+
+% 4th element is always positive
+if q(4)<0
+    q = -q;
+end
+
+% quaternion normalization
+q = q/sqrt(q'*q);