diff --git a/bag/analysys/compare.m b/bag/analysys/compare.m
index 8af4e9f0ad1d6fa081fdabf0539c1746425b6fc8..420d416e60adb0844c2af34156d77af560cdc553 100644
--- a/bag/analysys/compare.m
+++ b/bag/analysys/compare.m
@@ -1,5 +1,6 @@
clc; clear; clearvars; close all;
+%Folder where CSV files are saved
CSVFolder = '../CSV/transp/';
if ~isfolder(CSVFolder)
errorMessage = sprintf('Error: The following folder does not exist:\n%s\nPlease specify a new folder.', myFolder);
@@ -10,10 +11,13 @@ if ~isfolder(CSVFolder)
return;
end
end
+
+%Vectors where data from CSV will be extracted. G is for the GroudTruths, E
+%for the 2d experimens and T for the 3d experiments
Gvec = [];
Evec = [];
Tvec = [];
-myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in struct
+myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in folder
for k = 1:length(myFiles)
baseFileName = myFiles(k).name;
fullFileName = fullfile(CSVFolder, baseFileName);
@@ -31,18 +35,25 @@ for k = 1:length(myFiles)
end
end
+%Vector where errors will be saved
ERRORS=[];
min_len2d = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
min_len3d = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
for i = 1:size(Evec,2)
+ %Get position and angular error vectors for exepriments comparing them
+ %to GroundTruth
[perrors2d, angerrors2d] = geterror(Gvec{7,i}, Evec{7,i});
- [perrors3d, angerrors3d] = geterror(Gvec{7,i}, Tvec{7,i});
+ [perrors3d, angerrors3d] = geterror(Gvec{7,i}, Tvec{7,i});
+
+ %Calculate RMSE without subsampling vectors
Initial_RMSE2d = [sqrt(mean(perrors2d.^2)), sqrt(mean(angerrors2d.^2))];
Initial_RMSE3d = [sqrt(mean(perrors3d.^2)), sqrt(mean(angerrors3d.^2))];
ERRORS = [ERRORS, {strcat(Gvec{1,i},' ', Evec{1,i}, ' ', Tvec{1,i}), Initial_RMSE2d, perrors2d, angerrors2d, Initial_RMSE3d, perrors3d, angerrors3d}']
min_len2d = min(min_len2d, length(perrors2d));
min_len3d = min(min_len3d, length(perrors3d));
+ %Print error figures without showing, they will be compiled tiled in
+ %the following for
figure('visible','off');
plot(linspace(0,366, size(ERRORS{3,i},2)), ERRORS{3,i})
title(strcat('2D Position error for ', ERRORS{1,i}))
@@ -68,8 +79,11 @@ for i = 1:size(Evec,2)
ylabel('radians')
end
+%Plot errors grouped into figures
ploterrorstiled
+%Subsample errors uniformly so that they have the same number of sample and
+%recalculate the RMSE
for i = 1:size(ERRORS,2)
vecp2d = ERRORS{3,i};
rate2d = min_len2d/length(vecp2d);
diff --git a/bag/analysys/extractGT.m b/bag/analysys/extractGT.m
index c410639daff939f411d5aa721c976996882af37f..a4754faa52e58e4f000d2153f7ae55b5e99c4bbb 100644
--- a/bag/analysys/extractGT.m
+++ b/bag/analysys/extractGT.m
@@ -1,7 +1,10 @@
function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT)
+%Given a Table object extracts the necessary data. Intended for the
+%GroundTruth, with message being a Trajectory
t0 = GT(6,2).Var2;
GT(1,2).Var2 = (GT(1,2).Var2 - t0)*1e-9;
GT(3,2).Var2 = (GT(3,2).Var2 - t0)*1e-9;
+ %Regexp expressions to find the different data values needed
texpr = 'field.poses\d{1,4}.header.stamp';
xexpr = 'field.poses\d{1,4}.pose.position.x';
yexpr = 'field.poses\d{1,4}.pose.position.y';
@@ -28,16 +31,18 @@ function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT)
Gz = [Gz, GT(i,2).Var2];
end
if (size(regexp(GT(i,1).Var1{1,1}, thexpr)) > 0)
- [r1 r2 r3] = quat2angle([GT(i+3,2).Var2 GT(i,2).Var2 GT(i+1,2).Var2 GT(i+2,2).Var2]);
+ [r1, r2, r3] = quat2angle([GT(i+3,2).Var2 GT(i,2).Var2 GT(i+1,2).Var2 GT(i+2,2).Var2]);
Gang = [Gang, [r1 r2 r3]'];
end
end
+ %Generate timeseries objects for easier analysys
posts = timeseries([Gx' Gy' Gz'], Gt, 'name', 'pos');
posts.DataInfo.Units='meters';
angts = timeseries(Gang', Gt, 'name', 'ang');
angts.DataInfo.Units='radians';
+ %Group timeseries in a tscollection to return
Gts = tscollection({posts angts}, 'name', 'groundtruth');
Gts.TimeInfo.Units='seconds';
diff --git a/bag/analysys/extractdata.m b/bag/analysys/extractdata.m
index 082612ec05dbf5a17edc16930c796958153d341d..26fbb195176c744c947b884fae129baeb420c990 100644
--- a/bag/analysys/extractdata.m
+++ b/bag/analysys/extractdata.m
@@ -1,15 +1,20 @@
function [gx, gy, gz, gang, gt, gts] = extractdata(GC)
+%Given a Table object extracts the necessary data. Intended for the 2d and
+%3d experiments
+
t0 = GC(2,3).Variables;
gx = GC{:,5}';
gy = GC{:,6}';
gz = GC{:,7}';
- [R1, R2, R3] = quat2angle([GC{:,11} GC{:,8} GC{:,9} GC{:,10}]);
+ [R1, R2, R3] = quat2angle([GC{:,11} GC{:,8} GC{:,9} GC{:,10}]); %Convert quaternions to euler angles
gang = [R1'; R2'; R3'];
gt = GC{:,3}';
gt = gt-t0;
gt = gt*1e-9;
- %remove duplicates
+ %remove duplicates. Due to the high sampling rate of the PosePublisher
+ %some timestamps will have been saved multiple times, this cuts them
+ %out
[~,~,idx] = unique(gt);
gt = accumarray(idx,gt,[],@mean)';
gx = accumarray(idx,gx,[],@mean)';
@@ -20,11 +25,13 @@ function [gx, gy, gz, gang, gt, gts] = extractdata(GC)
gangz = accumarray(idx,gang(3,:),[],@mean);
gang = [gangx gangy gangz]';
+ %Generate timeseries objects for easier analysys
posts = timeseries([gx' gy' gz'], gt, 'name', 'pos');
posts.DataInfo.Units='meters';
angts = timeseries(gang', gt, 'name', 'ang');
angts.DataInfo.Units='radians';
+ %Group timeseries in a tscollection to return
gts = tscollection({posts angts}, 'name', 'poses');
gts.TimeInfo.Units='seconds';
diff --git a/bag/analysys/geterror.m b/bag/analysys/geterror.m
index ec1394bb8bde7fc5834daa4e2e6600ada41fe08c..a1836ad4e5cdd06ee5393774af10dc0e7941fbfc 100644
--- a/bag/analysys/geterror.m
+++ b/bag/analysys/geterror.m
@@ -1,4 +1,6 @@
function [Poserror, Angerror] = geterror(GT, TS)
+ %Returns position and angular error vectors given a GT groundtruth
+ %timeseries and a TS experiment time series
npairs = length(GT.Time)-1;
Poserror = zeros(1, npairs);
diff --git a/bag/analysys/matchaxes2.m b/bag/analysys/matchaxes2.m
index 54e4c4c00b004d9a476c8016f4dff334d7d52d61..bf6e7fd9a428da65ed0b95ebbc445ce2dd0aac65 100644
--- a/bag/analysys/matchaxes2.m
+++ b/bag/analysys/matchaxes2.m
@@ -1,4 +1,7 @@
function [epos, eang] = matchaxes2(GT, TS, t0, t1)
+%Returns position and angular error between the GroundTruth frames with
+%time indexes t0 and t1 (intended to be consecutive). GT and TS are
+%timeseries objects.
%Get Positions of timeseries and synchronize them
Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos;
Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos;
@@ -8,26 +11,24 @@ function [epos, eang] = matchaxes2(GT, TS, t0, t1)
Grot = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).ang;
Trot = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).ang;
[Grot, Trot] = synchronize(Grot, Trot, 'Union', 'KeepOriginalTimes',true');
-
- Tt = Tpos.Time;
- Gt = Gpos.Time;
- %Get quaternions to work
+ %Get quaternions of initial and final frames for both timeseries
+ %(expressed in the global frame)
Gq0 = eul2quat(Grot.Data(1,:));
Gq1 = eul2quat(Grot.Data(end,:));
Tq0 = eul2quat(Trot.Data(1,:));
Tq1 = eul2quat(Trot.Data(end,:));
- %Position vectors of end frame in initial frame
+ %Position vectors of end frame expressed in the initial frame
dpG = quatrotate(quatinv(Gq0), (Gpos.Data(end,:) - Gpos.Data(1,:)));
dpT = quatrotate(quatinv(Tq0), (Tpos.Data(end,:) - Tpos.Data(1,:)));
- %Orientation quaternions of end frame in initial frame
+ %Orientation quaternions of end frame expressed in the initial frame
dqG = quatmultiply(Gq0,quatconj(Gq1));
dqT = quatmultiply(Tq0,quatconj(Tq1));
%Errors
epos = norm(dpT-dpG);
- eang = norm(quat2eul(quatmultiply(quatinv(dqT),dqG)));
+ eang = norm(quat2eul(quatmultiply(quatinv(dqT),dqG))); %Angular error is done by finding the difference quaternion and getting it's norm as an angle vector
end
\ No newline at end of file
diff --git a/bag/analysys/ploterrorstiled.m b/bag/analysys/ploterrorstiled.m
index 5be1c6781c5a025f681638e6bb874dd5a7dca55f..ddea792e41e49c297e8039704c5f63c6a2c801a1 100644
--- a/bag/analysys/ploterrorstiled.m
+++ b/bag/analysys/ploterrorstiled.m
@@ -1,3 +1,4 @@
+%Takes all previous figures and groups them tiled in groups of 4
a = findobj('Type','axes');
m = 2;
n = 2;
diff --git a/bag/analysys/ploterrs.m b/bag/analysys/ploterrs.m
index ae6d9945f1ee83802ac22de8d1aafd6668b053fb..e83a5d1644ef42b7116da4392b3d6d50e4fe6d3a 100644
--- a/bag/analysys/ploterrs.m
+++ b/bag/analysys/ploterrs.m
@@ -1,4 +1,5 @@
function ploterrs(ts0, ts)
+%Takes two timeseries and plots the error of ts in respect to ts0
edist = ts.pos - ts0.pos;
eang = ts.ang - ts0.ang;
edist.name='Distance error';