Added comments to matlab scripts

bag/analysys/compare.m

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

bag/analysys/extractGT.m

 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';

bag/analysys/extractdata.m

 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';

bag/analysys/geterror.m

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

bag/analysys/matchaxes2.m

 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

bag/analysys/ploterrorstiled.m

+%Takes all previous figures and groups them tiled in groups of 4
 a = findobj('Type','axes');
 m = 2;
 n = 2;

bag/analysys/ploterrs.m

 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';