├── data ├── D1 │ └── groundtruth.txt ├── D10 │ └── groundtruth.txt ├── D11 │ └── groundtruth.txt ├── D12 │ └── groundtruth.txt ├── D13 │ └── groundtruth.txt ├── D14 │ └── groundtruth.txt ├── D15 │ └── groundtruth.txt ├── D2 │ └── groundtruth.txt ├── D3 │ └── groundtruth.txt ├── D4 │ └── groundtruth.txt ├── D5 │ └── groundtruth.txt ├── D6 │ └── groundtruth.txt ├── D7 │ └── groundtruth.txt ├── D8 │ └── groundtruth.txt └── D9 │ └── groundtruth.txt ├── code ├── kalmanRTS.m ├── quaternions │ ├── qt_dot.m │ ├── qt_exp.m │ ├── qt_inv.m │ ├── qt_log.m │ ├── qt_mul.m │ ├── qt_pow.m │ ├── qt_rot.m │ ├── qt_axis.m │ ├── qt_conj.m │ ├── qt_demo1.m │ ├── qt_dot_F.m │ ├── qt_dot_Q.m │ ├── qt_norm.m │ ├── qt_norm2.m │ ├── qt_slerp.m │ ├── qt_unit.m │ ├── qt_dircos.m │ ├── make_fighter.m │ ├── plot_fighter.m │ ├── qt_dircos2quat.m │ ├── qt_dircos2quat_e.m │ └── qt_demo2.m ├── ScaleEstimationMain.m ├── initializeEstimates.m ├── estimateScale.m ├── alignCameraIMU.m ├── readVisual.m ├── readInertial.m └── estimateAlignment.m └── README.md /data/D1/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.025718505971 -------------------------------------------------------------------------------- /data/D10/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 1.028367827447 -------------------------------------------------------------------------------- /data/D11/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.088840189869 -------------------------------------------------------------------------------- /data/D12/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.084936050274 -------------------------------------------------------------------------------- /data/D13/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.087972603246 -------------------------------------------------------------------------------- /data/D14/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.130757053699 -------------------------------------------------------------------------------- /data/D15/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.097850626451 -------------------------------------------------------------------------------- /data/D2/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.073945926837 -------------------------------------------------------------------------------- /data/D3/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.064294475039 -------------------------------------------------------------------------------- /data/D4/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.115462791715 -------------------------------------------------------------------------------- /data/D5/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.033319427983 -------------------------------------------------------------------------------- /data/D6/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 1.025694771929 -------------------------------------------------------------------------------- /data/D7/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.969850946386 -------------------------------------------------------------------------------- /data/D8/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.273347401389 -------------------------------------------------------------------------------- /data/D9/groundtruth.txt: -------------------------------------------------------------------------------- 1 | 0.848984675554 -------------------------------------------------------------------------------- /code/kalmanRTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/kalmanRTS.m -------------------------------------------------------------------------------- /code/quaternions/qt_dot.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_dot.m -------------------------------------------------------------------------------- /code/quaternions/qt_exp.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_exp.m -------------------------------------------------------------------------------- /code/quaternions/qt_inv.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_inv.m -------------------------------------------------------------------------------- /code/quaternions/qt_log.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_log.m -------------------------------------------------------------------------------- /code/quaternions/qt_mul.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_mul.m -------------------------------------------------------------------------------- /code/quaternions/qt_pow.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_pow.m -------------------------------------------------------------------------------- /code/quaternions/qt_rot.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_rot.m -------------------------------------------------------------------------------- /code/ScaleEstimationMain.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/ScaleEstimationMain.m -------------------------------------------------------------------------------- /code/quaternions/qt_axis.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_axis.m -------------------------------------------------------------------------------- /code/quaternions/qt_conj.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_conj.m -------------------------------------------------------------------------------- /code/quaternions/qt_demo1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_demo1.m -------------------------------------------------------------------------------- /code/quaternions/qt_dot_F.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_dot_F.m -------------------------------------------------------------------------------- /code/quaternions/qt_dot_Q.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_dot_Q.m -------------------------------------------------------------------------------- /code/quaternions/qt_norm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_norm.m -------------------------------------------------------------------------------- /code/quaternions/qt_norm2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_norm2.m -------------------------------------------------------------------------------- /code/quaternions/qt_slerp.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_slerp.m -------------------------------------------------------------------------------- /code/quaternions/qt_unit.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_unit.m -------------------------------------------------------------------------------- /code/quaternions/qt_dircos.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/qt_dircos.m -------------------------------------------------------------------------------- /code/quaternions/make_fighter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/make_fighter.m -------------------------------------------------------------------------------- /code/quaternions/plot_fighter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannemus/InertialScale/HEAD/code/quaternions/plot_fighter.m -------------------------------------------------------------------------------- /code/initializeEstimates.m: -------------------------------------------------------------------------------- 1 | function [A,b,scale,gravity,bias] = initializeEstimates(accVis,qtVis,accImu) 2 | 3 | fprintf('%s', repmat('-', 1, 60)); 4 | fprintf('\nFinding initial estimates by solving a linear system\n'); 5 | tic; 6 | 7 | N = size(accVis,1); 8 | R = qt_dircos(qtVis'); 9 | 10 | % Construct linear system 11 | A = zeros(3*N, 7); 12 | b = zeros(3*N, 1); 13 | 14 | for n = 0:N-1 15 | A(3*n+1:3*n+3,1) = accVis(n+1,:)'; 16 | A(3*n+1:3*n+3,2:4) = R(:,:,n+1); 17 | A(3*n+1:3*n+3,5:7) = eye(3); 18 | b(3*n+1:3*n+3,1) = accImu(n+1,:)'; 19 | end 20 | 21 | x = A\b; 22 | scale = x(1); 23 | gravity = x(2:4); 24 | bias = x(5:7); 25 | 26 | fprintf('scale0 = %.4f\n', scale); 27 | fprintf('gravity0 = [%.4f, %.4f, %.4f]\n', gravity); 28 | fprintf('bias0 = [%.4f, %.4f, %.4f]\n',bias); 29 | fprintf('Finished in %.3f seconds\n', toc); 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /code/quaternions/qt_dircos2quat.m: -------------------------------------------------------------------------------- 1 | %QT_DIRCOS2QUAT Convert direction cosine matrix to quaternion 2 | % 3 | % Syntax: 4 | % q = qt_dircos2quat(C) 5 | % 6 | % In: 7 | % C - Direction cosine matrix 8 | % 9 | % Out: 10 | % q - Unit quaternion 11 | % 12 | % Description: 13 | % Convert direction cosine matrix C to quaternion q. 14 | 15 | % Copyright (C) 2014 Simo S?rkk? 16 | % 17 | % $Id: qt_dircos2quat.m,v 1.1 2014/07/03 06:26:41 simosark Exp $ 18 | % 19 | % This software is distributed under the GNU General Public 20 | % Licence (version 2 or later); please refer to the file 21 | % Licence.txt, included with the software, for details. 22 | 23 | 24 | function q = qt_dircos2quat(C) 25 | 26 | q = zeros(4,1); 27 | q(1) = sqrt(1 + C(1,1) + C(2,2) + C(3,3))/2; 28 | 29 | if abs(q(1)) > 0.01 30 | q(2) = (C(3,2) - C(2,3)) / q(1) / 4; 31 | q(3) = (C(1,3) - C(3,1)) / q(1) / 4; 32 | q(4) = (C(2,1) - C(1,2)) / q(1) / 4; 33 | else 34 | q(2) = sqrt(1 + C(1,1) - C(2,2) - C(3,3))/2; 35 | q(3) = (C(1,2) + C(2,1)) / q(2) / 4; 36 | q(4) = (C(1,3) + C(3,1)) / q(2) / 4; 37 | q(1) = (C(3,2) - C(2,3)) / q(2) / 4; 38 | end 39 | 40 | -------------------------------------------------------------------------------- /code/quaternions/qt_dircos2quat_e.m: -------------------------------------------------------------------------------- 1 | %QT_DIRCOS2QUAT_E Convert direction cosine matrix to quaternion 2 | % 3 | % Syntax: 4 | % q = qt_dircos2quat(C) 5 | % 6 | % In: 7 | % C - Direction cosine matrix 8 | % 9 | % Out: 10 | % q - Unit quaternion 11 | % 12 | % Description: 13 | % Convert direction cosine matrix C to quaternion q. 14 | % This code is compatible with Eigen package. 15 | 16 | % Copyright (C) 2014 Simo S?rkk? 17 | % 18 | % $Id: qt_dircos2quat_e.m,v 1.1 2014/08/11 18:09:21 simosark Exp $ 19 | % 20 | % This software is distributed under the GNU General Public 21 | % Licence (version 2 or later); please refer to the file 22 | % Licence.txt, included with the software, for details. 23 | 24 | 25 | function q = qt_dircos2quat_e(C) 26 | 27 | q = zeros(4,1); 28 | 29 | t = trace(C); 30 | if (t > 0) 31 | t = sqrt(t + 1); 32 | q(1) = 0.5*t; 33 | t = 0.5/t; 34 | q(2) = (C(3,2) - C(2,3)) * t; 35 | q(3) = (C(1,3) - C(3,1)) * t; 36 | q(4) = (C(2,1) - C(1,2)) * t; 37 | else 38 | i = 1; 39 | if (C(2,2) > C(1,1)) 40 | i = 2; 41 | end 42 | if (C(3,3) > C(i,i)) 43 | i = 3; 44 | end 45 | j = rem(i,3) + 1; 46 | k = rem(j,3) + 1; 47 | 48 | t = sqrt(C(i,i)-C(j,j)-C(k,k) + 1); 49 | q(i+1) = 0.5 * t; 50 | t = 0.5 / t; 51 | q(1) = (C(k,j)-C(j,k))*t; 52 | q(j+1) = (C(j,i)+C(i,j))*t; 53 | q(k+1) = (C(k,i)+C(i,k))*t; 54 | end 55 | 56 | -------------------------------------------------------------------------------- /code/quaternions/qt_demo2.m: -------------------------------------------------------------------------------- 1 | % 2 | % Quaternion demo/test 2 3 | % 4 | 5 | %% 6 | % Simulate the angular velocity data 7 | % 8 | 9 | dt = 0.01; 10 | ww = zeros(3,300); 11 | ww(1,1:100) = 0.5; 12 | ww(3,101:end) = 1; 13 | 14 | %% 15 | % Test the differential equation 16 | % 17 | % dq/dt = q * [0;omega]/2 18 | % 19 | E = eye(3); 20 | 21 | q = [1;0;0;0]; 22 | for k=1:size(ww,2) 23 | q = q + dt * qt_mul(q,[0;ww(:,k)]/2) 24 | q = q / qt_norm(q); 25 | 26 | Ep = qt_rot(q,E); 27 | clf 28 | cols = 'rgb'; 29 | lbls = 'xyz'; 30 | for i=1:size(Ep,2) 31 | plot3([0;Ep(1,i)],[0;Ep(2,i)],[0;Ep(3,i)],[cols(i) '-']); 32 | axis([-1 1 -1 1 -1 1]); 33 | camproj perspective; 34 | grid on; 35 | hold on; 36 | text(Ep(1,i),Ep(2,i),Ep(3,i),lbls(i)); 37 | end 38 | 39 | 40 | pause(0.01) 41 | end 42 | 43 | %% 44 | % Test the differential equation 45 | % 46 | % dq/dt = [0;omega]/2 * q 47 | % 48 | E = eye(3); 49 | 50 | q = [1;0;0;0]; 51 | for k=1:size(ww,2) 52 | q = q + dt * qt_mul([0;ww(:,k)]/2,q) 53 | q = q / qt_norm(q); 54 | 55 | Ep = qt_rot(q,E); 56 | clf 57 | cols = 'rgb'; 58 | lbls = 'xyz'; 59 | for i=1:size(Ep,2) 60 | plot3([0;Ep(1,i)],[0;Ep(2,i)],[0;Ep(3,i)],[cols(i) '-']); 61 | axis([-1 1 -1 1 -1 1]); 62 | camproj perspective; 63 | grid on; 64 | hold on; 65 | text(Ep(1,i),Ep(2,i),Ep(3,i),lbls(i)); 66 | end 67 | 68 | 69 | pause(0.01) 70 | end 71 | -------------------------------------------------------------------------------- /code/estimateScale.m: -------------------------------------------------------------------------------- 1 | function [scale,gravity,bias] = ... 2 | estimateScale(A,b,scale0,gravity0,bias0,t) 3 | % Estimation is performed in the frequency domain 4 | 5 | fprintf('%s', repmat('-', 1, 60)); 6 | fprintf('\nFinal estimation in the frequency domain\n'); 7 | tic; 8 | 9 | % Select valid range of frequencies [0Hz - 1.2Hz] 10 | N = length(t); 11 | fmax = 1.2; 12 | fs = 1/mean(diff(t)); 13 | f = fs*(0:(N/2))/N; 14 | freqRange = (f <= fmax); 15 | fprintf('Upper limit for the frequencies = %.2f Hz\n',fmax); 16 | 17 | % Optimize while enforcing gravity constraint: norm(g) = 9.82 18 | options = optimoptions(@fmincon, 'Display', 'off'); 19 | gConst = @gravityConstraint; 20 | 21 | x0 = [scale0; gravity0; bias0]; 22 | x = fmincon(@(x)minFunc(x, A, b, freqRange), ... 23 | x0, [],[],[],[],[],[],gConst,options); 24 | 25 | scale = x(1); 26 | gravity = x(2:4); 27 | bias = x(5:7); 28 | 29 | fprintf('Finished in %.3f seconds\n', toc); 30 | 31 | end 32 | 33 | 34 | function [c,ceq] = gravityConstraint(x) 35 | 36 | c = []; 37 | ceq = norm([x(2) x(3) x(4)])-9.80; 38 | 39 | end 40 | 41 | 42 | function f = minFunc(x, A, b, freqRange) 43 | 44 | Av = A*x; % Visual accelerations 45 | Ai = b; % Inertial accelerations 46 | 47 | Av = [Av(1:3:end) Av(2:3:end) Av(3:3:end)]; 48 | Ai = [Ai(1:3:end) Ai(2:3:end) Ai(3:3:end)]; 49 | 50 | Fv = abs(fft(Av)); 51 | Fi = abs(fft(Ai)); 52 | 53 | Fv = Fv(freqRange,:); 54 | Fi = Fi(freqRange,:); 55 | 56 | f = (Fv - Fi).^2; 57 | f = sum(f(:)); 58 | 59 | end -------------------------------------------------------------------------------- /code/alignCameraIMU.m: -------------------------------------------------------------------------------- 1 | function [accVis,qtVis,accImu,t] = ... 2 | alignCameraIMU(accVis,qtVis,tVis,accImu,tImu,Rs,td) 3 | % 4 | % Align inertial and visual-data both temporarily and spatially. 5 | % 6 | % INPUT: accVis : Visual acceleration [unknown scale] (Nx3 matrix) 7 | % qtVis : Visual orientations (Nx4 matrix) 8 | % tVis : Visual timestamps in seconds (Nx1 vector) 9 | % accImu : Inertial accelerations [m/s^2] (Mx3 matrix) 10 | % tImu : Inertial timestamps in seconds (Mx1 vector) 11 | % Rs : Rotation between the camera and IMU (3x3 matrix) 12 | % td : Time offset between the camera and IMU (scalar) 13 | % 14 | % OUTPUT: accVis : Aligned visual acceleration (Kx3 matrix) 15 | % qtVis : Aligned visual orientations (Kx4 matrix) 16 | % accImu : Aligned inertial accelerations [m/s^2] (Kx3 matrix) 17 | % t : Timestamps in seconds (Kx1 vector) 18 | % 19 | 20 | % Use only time period which all sensors have values 21 | timeStop = min([tVis(end) tImu(end)]); 22 | tVis = tVis(tVis <= timeStop); 23 | tImu = tImu(tImu <= timeStop); 24 | 25 | accVis = accVis(1:length(tVis),:); 26 | qtVis = qtVis(1:length(tVis),:); 27 | accImu = accImu(1:length(tImu),:); 28 | 29 | % Upsample visual-data to match the sampling of the IMU 30 | t = tImu; 31 | accVis = interp1(tVis-td,accVis,t,'linear','extrap'); 32 | qtVis = interp1(tVis-td,qtVis,t,'linear','extrap'); % Consider using SLERP 33 | 34 | % Spatial alignment 35 | accImu = accImu*Rs; 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /code/readVisual.m: -------------------------------------------------------------------------------- 1 | function [posVis, qtVis, timeVis, scaleGT] = readVisual(dataset) 2 | % 3 | % Read camera poses and timestamps from the input files. 4 | % See the README.txt for more information about the input format. 5 | % 6 | % INPUT: dataset : Name of the dataset (string) 7 | % 8 | % OUTPUT: posVis : Visual positions (Nx3 matrix) 9 | % qtVis : Visual orientations (Nx4 matrix) 10 | % timeVis : Timestamps in seconds (Nx1 vector) 11 | % scaleGT : Ground truth scale if defined (scalar) 12 | % 13 | 14 | fprintf('%s', repmat('-', 1, 60)); 15 | fprintf('\nReading visual data (%s)\n', dataset); 16 | 17 | basePath = sprintf('../data/%s/',dataset); 18 | 19 | % Read camera poses 20 | if exist([basePath 'poses.txt'],'file') 21 | fileID = fopen([basePath 'poses.txt']); 22 | vis = textscan(fileID,'%f %f %f %f %f %f %f %f'); 23 | fclose(fileID); 24 | timeVis = vis{1}; 25 | timeVis = 1e-9*(timeVis - timeVis(1)); 26 | posVis = [vis{2} vis{3} vis{4}]; 27 | qtVis = [vis{5} vis{6} vis{7} vis{8}]; 28 | else 29 | error('poses.txt not found! Check inputs.'); 30 | end 31 | 32 | % Read ground truth scale if defined 33 | if exist([basePath 'groundtruth.txt'],'file') 34 | fileID = fopen([basePath 'groundtruth.txt']); 35 | gt = textscan(fileID,'%f'); 36 | fclose(fileID); 37 | scaleGT = gt{1}; 38 | fprintf('Ground truth scale was defined = %.4f\n', scaleGT); 39 | else 40 | scaleGT = 0; 41 | end 42 | 43 | dtVis = mean(diff(timeVis)); 44 | fprintf('Frame rate of the camera = %.2f fps\n', 1/(dtVis)); 45 | 46 | end 47 | 48 | -------------------------------------------------------------------------------- /code/readInertial.m: -------------------------------------------------------------------------------- 1 | function [accImu, angImu, timeImu] = readInertial(dataset) 2 | % 3 | % Read inertial measurements and timestamps from the input files. 4 | % See the README.txt for more information about the input format. 5 | % 6 | % INPUT: dataset : Name of the dataset (string) 7 | % 8 | % OUTPUT: accImu : Inertial accelerations [m/s^2] (Mx3 matrix) 9 | % angImu : Inertial angular velocities [rad/s] (Mx3 matrix) 10 | % timeImu : Timestamps in seconds (Mx1 vector) 11 | % 12 | 13 | fprintf('%s', repmat('-', 1, 60)); 14 | fprintf('\nReading inertial data (%s)\n', dataset); 15 | 16 | basePath = sprintf('../data/%s/',dataset); 17 | 18 | % Read accelerometer readings 19 | if exist([basePath 'accelerometer.txt'],'file') 20 | fileID = fopen([basePath 'accelerometer.txt']); 21 | acc = textscan(fileID,'%f %f %f %f'); 22 | fclose(fileID); 23 | timeAcc = 1e-9*acc{1}; 24 | accImu = [acc{2} acc{3} acc{4}]; 25 | else 26 | error('accelerometer.txt not found! Check inputs.'); 27 | end 28 | 29 | % Read gyroscope readings 30 | if exist([basePath 'gyroscope.txt'],'file') 31 | fileID = fopen([basePath 'gyroscope.txt']); 32 | gyr = textscan(fileID,'%f %f %f %f'); 33 | fclose(fileID); 34 | timeGyr = 1e-9*gyr{1}; 35 | gyrImu = [gyr{2} gyr{3} gyr{4}]; 36 | else 37 | error('gyroscope.txt not found! Check inputs.'); 38 | end 39 | 40 | dtAcc = mean(diff(timeAcc)); 41 | dtGyr = mean(diff(timeGyr)); 42 | fprintf('Sampling rate of the accelerometer = %.2f Hz\n', 1/(dtAcc)); 43 | fprintf('Sampling rate of the gyroscope = %.2f Hz\n', 1/(dtGyr)); 44 | 45 | % Resample gyroscope readings to match the sampling of the accelerometer 46 | t1 = min([timeAcc(1),timeGyr(1)]); 47 | timeImu = timeAcc - t1; 48 | angImu = interp1(timeGyr-t1,gyrImu,timeImu,'linear','extrap'); 49 | 50 | end 51 | 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 1. INTRODUCTION 2 | 3 | Matlab implementation of the scale estimation method presented in: 4 | 5 | Mustaniemi J., Kannala J., Särkkä S., Matas J., Heikkilä J. 6 | "Inertial-Based Scale Estimation for Structure from Motion 7 | on Mobile Devices", International Conference on Intelligent 8 | Robots and Systems (IROS), 2017 9 | 10 | Algorithm recovers the metric scale of the visual reconstruction 11 | given the camera poses and inertial measurements. Temporal and 12 | spatial alignment of the camera and IMU is also performed in the 13 | process. 14 | 15 | 16 | # 2. INPUTS 17 | 18 | ## 2.1. Camera poses and timestamps 19 | 20 | Camera poses and timestamps are read from a text file 'poses.txt'. 21 | Place the file to the folder '/data/dataset_name/poses.txt'. The 22 | file should have the following format: 23 | 24 | 10158706658000 2.3115 -0.7212 2.0553 0.9054 0.4168 -0.0635 -0.0480 25 | 26 | 10158739989000 2.3282 -0.7229 2.0687 0.9049 0.4180 -0.0631 -0.0482 27 | 28 | 10158773320000 2.3859 -0.7232 2.0309 0.9027 0.4222 -0.0675 -0.0464 29 | 30 | The first column contains the timestamps in nanoseconds. Columns 2-4 31 | contain the camera positions (x,y,z). Columns 5-8 contain the 32 | camera orientations in quaternions (qw,qx,qy,qz). 33 | 34 | ## 2.2. Inertial measurements and timestamps 35 | 36 | Angular velocities and accelerations are read from text files 37 | 'gyroscope.txt' and 'accelerometer.txt'. Place files to the folder 38 | '/data/dataset_name/gyroscope.txt', etc. The files should have the 39 | following format: 40 | 41 | 10158731787661 7.92720 0.53151 4.84108 42 | 43 | 10158741791911 7.92720 0.49799 4.90811 44 | 45 | 10158751780744 7.87932 0.49081 4.97755 46 | 47 | The first column contains the timestamps in nanoseconds. Last three 48 | colums contain the measurements (x,y,z). Units should be [rad/s] and 49 | [m/s^2]. Note: inertial and visual timestamp sources can be different. 50 | 51 | ## 2.3. Additional 52 | 53 | If the ground truth scale correction factor is known in advance 54 | (testing purposes), one can place a text file 'groundtruth.txt' to the 55 | folder '/data/dataset_name/groundtruth.txt'. The algorithm will then 56 | report the error of the scale estimate in percents. The file should 57 | have a single line containing the scale factor (e.g. 0.02571850). 58 | 59 | 60 | # 3. OUTPUTS 61 | 62 | The algorithm will output the estimated scale correction factor, 63 | gravity vector and bias of the accelerometer. The code will also 64 | output the estimated time offset between the camera and IMU (td), 65 | as well as the relative rotation between them (R). 66 | 67 | -------------------------------------------------------------------------------- /code/estimateAlignment.m: -------------------------------------------------------------------------------- 1 | function [Rs,td,bg] = estimateAlignment(qtVis,tVis,angImu,tImu) 2 | % 3 | % Estimate temporal and spatial alignment between the camera and IMU. 4 | % Gyroscope bias is also estimated in the process. 5 | % 6 | % INPUT: qtVis : Visual orientations (Nx4 matrix) 7 | % tVis : Visual timestamps in seconds (Nx1 vector) 8 | % angImu : Inertial angular velocities [rad/s] (Mx3 matrix) 9 | % tImu : Inertial timestamps in seconds (Mx1 vector) 10 | % 11 | % OUTPUT: Rs : Rotation between the camera and IMU (3x3 matrix) 12 | % td : Time offset between the camera and IMU (scalar) 13 | % bg : Gyroscope bias [rad/s] (1x3 vector) 14 | % 15 | 16 | fprintf('%s', repmat('-', 1, 60)); 17 | fprintf('\nTemporal and spatial alignment\n'); 18 | tic; 19 | 20 | % Use only time period which all sensors have values 21 | timeStop = min([tVis(end) tImu(end)]); 22 | tVis = tVis(tVis <= timeStop); 23 | tImu = tImu(tImu <= timeStop); 24 | 25 | qtVis = qtVis(1:length(tVis),:); 26 | angImu = angImu(1:length(tImu),:); 27 | 28 | % Upsample visual-data to match the sampling of the IMU 29 | t = tImu; 30 | dt = mean(diff(t)); 31 | qtVis = interp1(tVis,qtVis,t,'linear','extrap'); % Consider using SLERP 32 | 33 | % Compute visual angular velocities 34 | qtDiffs = diff(qtVis); 35 | qtDiffs = [qtDiffs; qtDiffs(end,:)]'; 36 | angVis = -(2/dt)*qt_mul(qtDiffs, qt_inv(qtVis')); 37 | angVis = angVis(2:4,:)'; 38 | 39 | % Smooth angular velocities 40 | angVis(:,1) = smooth(angVis(:,1),15); 41 | angVis(:,2) = smooth(angVis(:,2),15); 42 | angVis(:,3) = smooth(angVis(:,3),15); 43 | angImu(:,1) = smooth(angImu(:,1),15); 44 | angImu(:,2) = smooth(angImu(:,2),15); 45 | angImu(:,3) = smooth(angImu(:,3),15); 46 | 47 | gRatio = (1 + sqrt(5)) / 2; 48 | tolerance = 0.0001; 49 | 50 | maxOffset = 0.5; 51 | a = -maxOffset; 52 | b = maxOffset; 53 | 54 | c = b - (b - a) / gRatio; 55 | d = a + (b - a) / gRatio; 56 | 57 | iter = 0; 58 | 59 | while abs(c - d) > tolerance 60 | 61 | % Evaluate function at f(c) and f(d) 62 | [Rsc,biasc,fc] = solveClosedForm(angVis,angImu,t,c); 63 | [Rsd,biasd,fd] = solveClosedForm(angVis,angImu,t,d); 64 | 65 | if fc < fd 66 | b = d; 67 | Rs = Rsc; 68 | bg = biasc; 69 | else 70 | a = c; 71 | Rs = Rsd; 72 | bg = biasd; 73 | end 74 | 75 | c = b - (b - a) / gRatio; 76 | d = a + (b - a) / gRatio; 77 | 78 | iter = iter + 1; 79 | end 80 | 81 | td = (b + a) / 2; 82 | 83 | fprintf('Golden-section search (%.0f iterations)\n', iter); 84 | fprintf('Finished in %.3f seconds\n', toc); 85 | 86 | 87 | end 88 | 89 | 90 | 91 | function [Rs,bias,f] = solveClosedForm(angVis,angImu,t,td) 92 | % 93 | % Finds the relative rotation between the camera and IMU when using the 94 | % provided time offset td. Gyroscope bias is estimated in the process. 95 | % 96 | % INPUT: angVis : Visual angular velocities [rad/s] (Mx3 matrix) 97 | % angImu : Inertial angular velocities [rad/s] (Mx3 matrix) 98 | % t : Timestamps in seconds 99 | % td : Time offset in seconds 100 | % 101 | % OUTPUT: Rs : Rotation between the camera and IMU (3x3 matrix) 102 | % bias : Gyroscope bias [rad/s] (1x3 vector) 103 | % f : Function value (sum of squared differences) 104 | % 105 | 106 | % Adjust visual angular velocities based on current offset 107 | angVis = interp1(t-td,angVis,t,'linear','extrap'); 108 | N = size(angVis,1); 109 | 110 | % Compute mean vectors 111 | meanImu = repmat(mean(angImu),N,1); 112 | meanVis = repmat(mean(angVis),N,1); 113 | 114 | % Compute centralized point sets 115 | P = angImu - meanImu; 116 | Q = angVis - meanVis; 117 | 118 | % Singular value decomposition 119 | [U,S,V] = svd(P'*Q); 120 | 121 | % Ensure a right handed coordinate system and correct if necessary 122 | C = eye(3); 123 | if (det(V*U') < 0) 124 | C(3,3) = -1; 125 | end 126 | 127 | Rs = V*C*U'; 128 | 129 | % Find the translation, which is the gyroscope bias 130 | bias = mean(angVis) - mean(angImu)*Rs; 131 | 132 | % Residual 133 | D = angVis - (angImu*Rs + repmat(bias,N,1)); 134 | f = sum(D(:).^2); 135 | 136 | end 137 | 138 | --------------------------------------------------------------------------------