-
Notifications
You must be signed in to change notification settings - Fork 14
/
example.m
75 lines (66 loc) · 2.26 KB
/
example.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
%% An example for the tool.
% Author: Xiaochen Qiu from Beihang Univ.
% Description: Show how to use the tool by an example of:
% Evaluate trajectory position RMSE for a monocular VIO
% on sequence V2_03_difficult from EuRoC dataset.
clc;
clear;
close all;
addpath('DATAS','FUNCTIONS');
%% load ground truth and estimate trajectory
load gt.csv; % provided by EuRoC
load est.txt;
%% need to adjust the time standard of ground truth
take_off_stamp = 1413394886.455760384; % timestamp standard in est.txt
gt(:,1) = gt(:,1)*1e-9;
gt(:,1) = gt(:,1)-take_off_stamp;
%% alignment
time_gt = gt(:,1);
P_gt = gt(:,2:4)';
time_es = est(:,1);
P_es = est(:,9:11)';
[Ids_es, Ids_gt] = findIds (time_es, time_gt, 0.001);
[R_es, t_es, s_es] = sim3DataAlignment (P_es(:,Ids_es), P_gt(:,Ids_gt), 'vio');
%% align the estimated trajectory
P_es_aligned = s_es*R_es*P_es + repmat(t_es,1,size(P_es,2));
%% draw trajectory
time_matched = time_es(Ids_es);
P_es_matched = P_es_aligned(:,Ids_es);
P_gt_matched = P_gt(:,Ids_gt);
figure;
plot3(P_gt_matched(1,:),P_gt_matched(2,:),P_gt_matched(3,:),'r-');
hold on;
plot3(P_es_matched(1,:),P_es_matched(2,:),P_es_matched(3,:),'b-');
for i = 1:size(P_gt_matched,2) % draw difference, this block is time consuming
plot3([P_gt_matched(1,i),P_es_matched(1,i)],...
[P_gt_matched(2,i),P_es_matched(2,i)],[P_gt_matched(3,i),P_es_matched(3,i)],'y-');
end
axis equal;
grid on;
lgd = legend('ground truth','estimated');
set(lgd,'Fontname','Times New Roman','FontWeight','bold','FontSize',15);
title('V203','FontSize',15);
%% draw error in XYZ axes
errX = P_es_aligned(1,Ids_es)-P_gt(1,Ids_gt);
errY = P_es_aligned(2,Ids_es)-P_gt(2,Ids_gt);
errZ = P_es_aligned(3,Ids_es)-P_gt(3,Ids_gt);
figure;
subplot(311);
plot(errX);
title('position error of axis-X (m)');
subplot(312);
plot(errY);
title('position error of axis-Y (m)');
subplot(313);
plot(errZ);
title('position error of axis-Z (m)');
fprintf('mean error in [X Y Z]: [%fm %fm %fm]\n',mean(errX),mean(errY),mean(errZ));
%% some printing
errVec = P_es_aligned(1:3,Ids_es)-P_gt(1:3,Ids_gt);
N = size(errVec,2);
RMSE_trans = 0;
for i = 1:N
RMSE_trans = RMSE_trans+norm(errVec(:,i))^2;
end
RMSE_trans = sqrt(RMSE_trans/N);
fprintf('RMSE of translation is %fm\n',RMSE_trans);