added matlab analysys scripts

bag/.gitignore

-*
-*/
+*.bag
+CSV/
+recordings/
+!analysys/
 !.gittignore
 !*.bash
 !*.py

bag/analysys/compare.asv

+clc; clear; clearvars; close all;
+
+CSVFolder = '../CSV/transp/';
+if ~isfolder(CSVFolder)
+    errorMessage = sprintf('Error: The following folder does not exist:\n%s\nPlease specify a new folder.', myFolder);
+    uiwait(warndlg(errorMessage));
+    myFolder = uigetdir(); % Ask for a new one.
+    if myFolder == 0
+         % User clicked Cancel
+         return;
+    end
+end
+Gvec = [];
+Evec = [];
+myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in struct
+for k = 1:length(myFiles)
+    baseFileName = myFiles(k).name;
+    fullFileName = fullfile(CSVFolder, baseFileName);
+    fprintf('Now reading file %s\n', baseFileName);
+    Table = readtable(fullFileName,'PreserveVariableNames',true);
+    if baseFileName(1) == 'G'
+        [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table);
+        Gvec = [Gvec, {baseFileName, Gx, Gy, Gz, Gang, Gt, Gts}'];
+    elseif baseFileName(1) == 'E'
+        [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table);
+        Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
+    end
+
+%     
+%     t10 = linspace(0,min(Gt(end),gt10(end))-1,1e6);
+%     ts0 = resample(Gts, t10);
+%     ts1 = resample(gts10, t10);
+%     ploterrs(ts0, ts1)
+%     
+%     GC20 = readtable('trajectory_ana_lab_2_exp_20_transp.csv','PreserveVariableNames',true);
+%     [gx20, gy20, gz20, gth20, gt20, gts20] = extractdata(GC20);
+%     
+%     t20 = linspace(0,min(Gt(end),gt20(end))-1,1e6);
+%     ts0 = resample(Gts, t20);
+%     ts2 = resample(gts20, t20);
+%     ploterrs(ts0, ts2)
+end
+
+[Edata, Gdata] = matchaxes(Gvec{7,1}, Evec{7,1}, 1, 2);

bag/analysys/compare.m

+clc; clear; clearvars; close all;
+
+CSVFolder = '../CSV/transp/';
+if ~isfolder(CSVFolder)
+    errorMessage = sprintf('Error: The following folder does not exist:\n%s\nPlease specify a new folder.', myFolder);
+    uiwait(warndlg(errorMessage));
+    myFolder = uigetdir(); % Ask for a new one.
+    if myFolder == 0
+         % User clicked Cancel
+         return;
+    end
+end
+Gvec = [];
+Evec = [];
+myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in struct
+for k = 1:length(myFiles)
+    baseFileName = myFiles(k).name;
+    fullFileName = fullfile(CSVFolder, baseFileName);
+    fprintf('Now reading file %s\n', baseFileName);
+    Table = readtable(fullFileName,'PreserveVariableNames',true);
+    if baseFileName(1) == 'G'
+        [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table);
+        Gvec = [Gvec, {baseFileName, Gx, Gy, Gz, Gang, Gt, Gts}'];
+    elseif baseFileName(1) == 'E'
+        [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table);
+        Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
+    end
+
+%     
+%     t10 = linspace(0,min(Gt(end),gt10(end))-1,1e6);
+%     ts0 = resample(Gts, t10);
+%     ts1 = resample(gts10, t10);
+%     ploterrs(ts0, ts1)
+%     
+%     GC20 = readtable('trajectory_ana_lab_2_exp_20_transp.csv','PreserveVariableNames',true);
+%     [gx20, gy20, gz20, gth20, gt20, gts20] = extractdata(GC20);
+%     
+%     t20 = linspace(0,min(Gt(end),gt20(end))-1,1e6);
+%     ts0 = resample(Gts, t20);
+%     ts2 = resample(gts20, t20);
+%     ploterrs(ts0, ts2)
+end
+
+[Edata, Gdata] = matchaxes(Gvec{7,1}, Evec{7,1}, 2, 3);
+error = vecnorm(Edata(end,:)-Gdata(end,:))
\ No newline at end of file

bag/analysys/extractGT.asv

+function [Gx, Gy, Gz, Gth, Gt, Gts] = extractGT(GT)
+    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;
+    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';
+    zexpr = 'field.poses\d{1,4}.pose.position.z';
+    thexpr = 'field.poses\d{1,4}.pose.orientation.x';
+
+    Gt = [];
+    Gx = [];
+    Gy = [];
+    Gz = [];
+    Gth = [];
+    for i=1:height(GT)
+        if (size(regexp(GT(i,1).Var1{1,1}, texpr)) > 0)
+            GT(i,2).Var2 = (GT(i, 2).Var2 - t0)*1e-9;
+            Gt = [Gt, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, xexpr)) > 0)
+            Gx = [Gx, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, yexpr)) > 0)
+            Gy = [Gy, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, zexpr)) > 0)
+            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]);
+            Gth = [Gth, r1];
+        end
+    end
+    
+    Gts = timeseries([Gx' Gy' Gz' Gth'], Gt, 'name', 'groundtruth');
+    Gts.TimeInfo.Units='seconds';
+    
+end
\ No newline at end of file

bag/analysys/extractGT.m

+function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT)
+    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;
+    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';
+    zexpr = 'field.poses\d{1,4}.pose.position.z';
+    thexpr = 'field.poses\d{1,4}.pose.orientation.x';
+
+    Gt = [];
+    Gx = [];
+    Gy = [];
+    Gz = [];
+    Gang = [];
+    for i=1:height(GT)
+        if (size(regexp(GT(i,1).Var1{1,1}, texpr)) > 0)
+            GT(i,2).Var2 = (GT(i, 2).Var2 - t0)*1e-9;
+            Gt = [Gt, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, xexpr)) > 0)
+            Gx = [Gx, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, yexpr)) > 0)
+            Gy = [Gy, GT(i,2).Var2];
+        end
+        if (size(regexp(GT(i,1).Var1{1,1}, zexpr)) > 0)
+            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]);
+            Gang = [Gang, [r1 r2 r3]'];
+        end
+    end
+
+    posts = timeseries([Gx' Gy' Gz'], Gt, 'name', 'pos');
+    posts.DataInfo.Units='meters';
+    angts = timeseries(Gang', Gt, 'name', 'ang');
+    angts.DataInfo.Units='radians';
+
+    Gts = tscollection({posts angts}, 'name', 'groundtruth');
+
+    Gts.TimeInfo.Units='seconds';
+    Gts.TimeInfo.Format='SS';
+end
\ No newline at end of file

bag/analysys/extractdata.m

+function [gx, gy, gz, gang, gt, gts] = extractdata(GC)
+    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}]);
+    gang = [R1'; R2'; R3'];
+    gt = GC{:,3}';
+    gt = gt-t0;
+    gt = gt*1e-9;
+    
+    %remove duplicates
+    [~,~,idx] = unique(gt);
+    gt = accumarray(idx,gt,[],@mean)';
+    gx = accumarray(idx,gx,[],@mean)';
+    gy = accumarray(idx,gy,[],@mean)';
+    gz = accumarray(idx,gz,[],@mean)';
+    gangx = accumarray(idx,gang(1,:),[],@mean);
+    gangy = accumarray(idx,gang(2,:),[],@mean);
+    gangz = accumarray(idx,gang(3,:),[],@mean);
+    gang = [gangx gangy gangz]';
+
+    posts = timeseries([gx' gy' gz'], gt, 'name', 'pos');
+    posts.DataInfo.Units='meters';
+    angts = timeseries(gang', gt, 'name', 'ang');
+    angts.DataInfo.Units='radians';
+
+    gts = tscollection({posts angts}, 'name', 'poses');
+
+    gts.TimeInfo.Units='seconds';
+    gts.TimeInfo.Format='SS';
+end
\ No newline at end of file

bag/analysys/interparc.m

+function [pt,dudt,fofthandle] = interparc(t,px,py,varargin)
+% interparc: interpolate points along a curve in 2 or more dimensions
+% usage: pt = interparc(t,px,py)    % a 2-d curve
+% usage: pt = interparc(t,px,py,pz) % a 3-d curve
+% usage: pt = interparc(t,px,py,pz,pw,...) % a 4-d or higher dimensional curve
+% usage: pt = interparc(t,px,py,method) % a 2-d curve, method is specified
+% usage: [pt,dudt,fofthandle] = interparc(t,px,py,...) % also returns derivatives, and a function handle
+%
+% Interpolates new points at any fractional point along
+% the curve defined by a list of points in 2 or more
+% dimensions. The curve may be defined by any sequence
+% of non-replicated points.
+%
+% arguments: (input)
+%  t   - vector of numbers, 0 <= t <= 1, that define
+%        the fractional distance along the curve to
+%        interpolate the curve at. t = 0 will generate
+%        the very first point in the point list, and
+%        t = 1 yields the last point in that list.
+%        Similarly, t = 0.5 will yield the mid-point
+%        on the curve in terms of arc length as the
+%        curve is interpolated by a parametric spline.
+%
+%        If t is a scalar integer, at least 2, then
+%        it specifies the number of equally spaced
+%        points in arclength to be generated along
+%        the curve.
+%
+%  px, py, pz, ... - vectors of length n, defining
+%        points along the curve. n must be at least 2.
+%        Exact Replicate points should not be present
+%        in the curve, although there is no constraint
+%        that the curve has replicate independent
+%        variables.
+%
+%  method - (OPTIONAL) string flag - denotes the method
+%        used to compute the points along the curve.
+%
+%        method may be any of 'linear', 'spline', or 'pchip',
+%        or any simple contraction thereof, such as 'lin',
+%        'sp', or even 'p'.
+%        
+%        method == 'linear' --> Uses a linear chordal
+%               approximation to interpolate the curve.
+%               This method is the most efficient.
+%
+%        method == 'pchip' --> Uses a parametric pchip
+%               approximation for the interpolation
+%               in arc length.
+%
+%        method == 'spline' --> Uses a parametric spline
+%               approximation for the interpolation in
+%               arc length. Generally for a smooth curve,
+%               this method may be most accurate.
+%
+%        method = 'csape' --> if available, this tool will
+%               allow a periodic spline fit for closed curves.
+%               ONLY use this method if your points should
+%               represent a closed curve.
+%               
+%               If the last point is NOT the same as the
+%               first point on the curve, then the curve
+%               will be forced to be periodic by this option.
+%               That is, the first point will be replicated
+%               onto the end.
+%
+%               If csape is not present in your matlab release,
+%               then an error will result.
+%
+%        DEFAULT: 'spline'
+%
+%
+% arguments: (output)
+%  pt - Interpolated points at the specified fractional
+%        distance (in arc length) along the curve.
+%
+%  dudt - when a second return argument is required,
+%       interparc will return the parametric derivatives
+%       (dx/dt, dy/dt, dz/dt, ...) as an array.
+%
+%  fofthandle - a function handle, taking numbers in the interval [0,1]
+%       and evaluating the function at those points.
+%
+%       Extrapolation will not be permitted by this call.
+%       Any values of t that lie outside of the interval [0,1]
+%       will be clipped to the endpoints of the curve.
+%
+% Example:
+% % Interpolate a set of unequally spaced points around
+% % the perimeter of a unit circle, generating equally
+% % spaced points around the perimeter.
+% theta = sort(rand(15,1))*2*pi;
+% theta(end+1) = theta(1);
+% px = cos(theta);
+% py = sin(theta);
+%
+% % interpolate using parametric splines
+% pt = interparc(100,px,py,'spline');
+%
+% % Plot the result
+% plot(px,py,'r*',pt(:,1),pt(:,2),'b-o')
+% axis([-1.1 1.1 -1.1 1.1])
+% axis equal
+% grid on
+% xlabel X
+% ylabel Y
+% title 'Points in blue are uniform in arclength around the circle'
+%
+%
+% Example:
+% % For the previous set of points, generate exactly 6
+% % points around the parametric splines, verifying
+% % the uniformity of the arc length interpolant.
+% pt = interparc(6,px,py,'spline');
+%
+% % Convert back to polar form. See that the radius
+% % is indeed 1, quite accurately.
+% [TH,R] = cart2pol(pt(:,1),pt(:,2))
+% % TH =
+% %       0.86005
+% %        2.1141
+% %       -2.9117
+% %        -1.654
+% %      -0.39649
+% %       0.86005
+% % R =
+% %             1
+% %        0.9997
+% %        0.9998
+% %       0.99999
+% %        1.0001
+% %             1
+%
+% % Unwrap the polar angles, and difference them.
+% diff(unwrap(TH))
+% % ans =
+% %        1.2541
+% %        1.2573
+% %        1.2577
+% %        1.2575
+% %        1.2565
+%
+% % Six points around the circle should be separated by
+% % 2*pi/5 radians, if they were perfectly uniform. The
+% % slight differences are due to the imperfect accuracy
+% % of the parametric splines.
+% 2*pi/5
+% % ans =
+% %        1.2566
+%
+%
+% See also: arclength, spline, pchip, interp1
+%
+% Author: John D'Errico
+% e-mail: woodchips@rochester.rr.com
+% Release: 1.0
+% Release date: 3/15/2010
+
+% unpack the arguments and check for errors
+if nargin < 3
+  error('ARCLENGTH:insufficientarguments', ...
+    'at least t, px, and py must be supplied')
+end
+
+t = t(:);
+if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0)
+  % t specifies the number of points to be generated
+  % equally spaced in arclength
+  t = linspace(0,1,t)';
+elseif any(t < 0) || any(t > 1)
+  error('ARCLENGTH:impropert', ...
+    'All elements of t must be 0 <= t <= 1')
+end
+
+% how many points will be interpolated?
+nt = numel(t);
+
+% the number of points on the curve itself
+px = px(:);
+py = py(:);
+n = numel(px);
+
+% are px and py both vectors of the same length?
+if ~isvector(px) || ~isvector(py) || (length(py) ~= n)
+  error('ARCLENGTH:improperpxorpy', ...
+    'px and py must be vectors of the same length')
+elseif n < 2
+  error('ARCLENGTH:improperpxorpy', ...
+    'px and py must be vectors of length at least 2')
+end
+
+% compose px and py into a single array. this way,
+% if more dimensions are provided, the extension
+% is trivial.
+pxy = [px,py];
+ndim = 2;
+
+% the default method is 'linear'
+method = 'spline';
+
+% are there any other arguments?
+if nargin > 3
+  % there are. check the last argument. Is it a string?
+  if ischar(varargin{end})
+    method = varargin{end};
+    varargin(end) = [];
+    
+    % method may be any of {'linear', 'pchip', 'spline', 'csape'.}
+    % any any simple contraction thereof.
+    valid = {'linear', 'pchip', 'spline', 'csape'};
+    [method,errstr] = validstring(method,valid);
+    if ~isempty(errstr)
+      error('INTERPARC:incorrectmethod',errstr)
+    end
+  end
+  
+  % anything that remains in varargin must add
+  % an additional dimension on the curve/polygon
+  for i = 1:numel(varargin)
+    pz = varargin{i};
+    pz = pz(:);
+    if numel(pz) ~= n
+      error('ARCLENGTH:improperpxorpy', ...
+        'pz must be of the same size as px and py')
+    end
+    pxy = [pxy,pz]; %#ok
+  end
+  
+  % the final number of dimensions provided
+  ndim = size(pxy,2);
+end
+
+% if csape, then make sure the first point is replicated at the end.
+% also test to see if csape is available
+if method(1) == 'c'
+  if exist('csape','file') == 0
+    error('CSAPE was requested, but you lack the necessary toolbox.')
+  end
+  
+  p1 = pxy(1,:);
+  pend = pxy(end,:);
+  
+  % get a tolerance on whether the first point is replicated.
+  if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1)))
+    % the two end points were not identical, so wrap the curve
+    pxy(end+1,:) = p1;
+    nt = nt + 1;
+  end
+end
+
+% preallocate the result, pt
+pt = NaN(nt,ndim);
+
+% Compute the chordal (linear) arclength
+% of each segment. This will be needed for
+% any of the methods.
+chordlen = sqrt(sum(diff(pxy,[],1).^2,2));
+
+% Normalize the arclengths to a unit total
+chordlen = chordlen/sum(chordlen);
+
+% cumulative arclength
+cumarc = [0;cumsum(chordlen)];
+
+% The linear interpolant is trivial. do it as a special case
+if method(1) == 'l'
+  % The linear method.
+  
+  % which interval did each point fall in, in
+  % terms of t?
+  [junk,tbins] = histc(t,cumarc); %#ok
+  
+  % catch any problems at the ends
+  tbins((tbins <= 0) | (t <= 0)) = 1;
+  tbins((tbins >= n) | (t >= 1)) = n - 1;
+  
+  % interpolate
+  s = (t - cumarc(tbins))./chordlen(tbins);
+  % be nice, and allow the code to work on older releases
+  % that don't have bsxfun
+  pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim);
+  
+  % do we need to compute derivatives here?
+  if nargout > 1
+    dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim);
+  end
+  
+  % do we need to create the spline as a piecewise linear function?
+  if nargout > 2
+    spl = cell(1,ndim);
+    for i = 1:ndim
+      coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)];
+      spl{i} = mkpp(cumarc.',coefs);
+    end
+    
+    %create a function handle for evaluation, passing in the splines
+    fofthandle = @(t) foft(t,spl);
+  end
+  
+  % we are done at this point
+  return
+end
+
+% If we drop down to here, we have either a spline
+% or csape or pchip interpolant to work with.
+
+% compute parametric splines
+spl = cell(1,ndim);
+spld = spl;
+diffarray = [3 0 0;0 2 0;0 0 1;0 0 0];
+for i = 1:ndim
+  switch method
+    case 'pchip'
+      spl{i} = pchip(cumarc,pxy(:,i));
+    case 'spline'
+      spl{i} = spline(cumarc,pxy(:,i));
+      nc = numel(spl{i}.coefs);
+      if nc < 4
+        % just pretend it has cubic segments
+        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
+        spl{i}.order = 4;
+      end
+    case 'csape'
+      % csape was specified, so the curve is presumed closed,
+      % therefore periodic
+      spl{i} = csape(cumarc,pxy(:,i),'periodic');
+      nc = numel(spl{i}.coefs);
+      if nc < 4
+        % just pretend it has cubic segments
+        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
+        spl{i}.order = 4;
+      end
+  end
+  
+  % and now differentiate them
+  xp = spl{i};
+  xp.coefs = xp.coefs*diffarray;
+  xp.order = 3;
+  spld{i} = xp;
+end
+
+% catch the case where there were exactly three points
+% in the curve, and spline was used to generate the
+% interpolant. In this case, spline creates a curve with
+% only one piece, not two.
+if (numel(cumarc) == 3) && (method(1) == 's')
+  cumarc = spl{1}.breaks;
+  n = numel(cumarc);
+  chordlen = sum(chordlen);
+end
+
+% Generate the total arclength along the curve
+% by integrating each segment and summing the
+% results. The integration scheme does its job
+% using an ode solver.
+
+% polyarray here contains the derivative polynomials
+% for each spline in a given segment
+polyarray = zeros(ndim,3);
+seglen = zeros(n-1,1);
+
+% options for ode45
+opts = odeset('reltol',1.e-9);
+for i = 1:spl{1}.pieces
+  % extract polynomials for the derivatives
+  for j = 1:ndim
+    polyarray(j,:) = spld{j}.coefs(i,:);
+  end
+  
+  % integrate the arclength for the i'th segment
+  % using ode45 for the integral. I could have
+  % done this part with quad too, but then it
+  % would not have been perfectly (numerically)
+  % consistent with the next operation in this tool.
+  [tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok
+  seglen(i) = yout(end);
+end
+
+% and normalize the segments to have unit total length
+totalsplinelength = sum(seglen);
+cumseglen = [0;cumsum(seglen)];
+
+% which interval did each point fall into, in
+% terms of t, but relative to the cumulative
+% arc lengths along the parametric spline?
+[junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok
+
+% catch any problems at the ends
+tbins((tbins <= 0) | (t <= 0)) = 1;
+tbins((tbins >= n) | (t >= 1)) = n - 1;
+
+% Do the fractional integration within each segment
+% for the interpolated points. t is the parameter
+% used to define the splines. It is defined in terms
+% of a linear chordal arclength. This works nicely when
+% a linear piecewise interpolant was used. However,
+% what is asked for is an arclength interpolation
+% in terms of arclength of the spline itself. Call s
+% the arclength traveled along the spline.
+s = totalsplinelength*t;
+
+% the ode45 options will now include an events property
+% so we can catch zero crossings.
+opts = odeset('reltol',1.e-9,'events',@ode_events);
+
+ti = t;
+for i = 1:nt
+  % si is the piece of arc length that we will look
+  % for in this spline segment.
+  si = s(i) - cumseglen(tbins(i));
+  
+  % extract polynomials for the derivatives
+  % in the interval the point lies in
+  for j = 1:ndim
+    polyarray(j,:) = spld{j}.coefs(tbins(i),:);
+  end
+  
+  % we need to integrate in t, until the integral
+  % crosses the specified value of si. Because we
+  % have defined totalsplinelength, the lengths will
+  % be normalized at this point to a unit length.
+  %
+  % Start the ode solver at -si, so we will just
+  % look for an event where y crosses zero.
+  [tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok
+  
+  % we only need that point where a zero crossing occurred
+  % if no crossing was found, then we can look at each end.
+  if ~isempty(te)
+    ti(i) = te(1) + cumarc(tbins(i));
+  else
+    % a crossing must have happened at the very
+    % beginning or the end, and the ode solver
+    % missed it, not trapping that event.
+    if abs(yout(1)) < abs(yout(end))
+      % the event must have been at the start.
+      ti(i) = tout(1) + cumarc(tbins(i));
+    else
+      % the event must have been at the end.
+      ti(i) = tout(end) + cumarc(tbins(i));
+    end
+  end
+end
+
+% Interpolate the parametric splines at ti to get
+% our interpolated value.
+for L = 1:ndim
+  pt(:,L) = ppval(spl{L},ti);
+end
+
+% do we need to compute first derivatives here at each point?
+if nargout > 1
+  dudt = zeros(nt,ndim);
+  for L = 1:ndim
+    dudt(:,L) = ppval(spld{L},ti);
+  end
+end
+
+% create a function handle for evaluation, passing in the splines
+if nargout > 2
+  fofthandle = @(t) foft(t,spl);
+end
+
+% ===============================================
+%  nested function for the integration kernel
+% ===============================================
+  function val = segkernel(t,y) %#ok
+    % sqrt((dx/dt)^2 + (dy/dt)^2 + ...)
+    val = zeros(size(t));
+    for k = 1:ndim
+      val = val + polyval(polyarray(k,:),t).^2;
+    end
+    val = sqrt(val);
+    
+  end % function segkernel
+
+% ===============================================
+%  nested function for ode45 integration events
+% ===============================================
+  function [value,isterminal,direction] = ode_events(t,y) %#ok
+    % ode event trap, looking for zero crossings of y.
+    value = y;
+    isterminal = ones(size(y));
+    direction = ones(size(y));
+  end % function ode_events
+
+end % mainline - interparc
+
+
+% ===============================================
+%       end mainline - interparc
+% ===============================================
+%       begin subfunctions
+% ===============================================
+
+% ===============================================
+%  subfunction for evaluation at any point externally
+% ===============================================
+function f_t = foft(t,spl)
+% tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1]
+pdim = numel(spl);
+f_t = zeros(numel(t),pdim);
+
+% convert t to a column vector, clipping it to [0,1] as we do.
+t = max(0,min(1,t(:)));
+
+% just loop over the splines in the cell array of splines
+for i = 1:pdim
+  f_t(:,i) = ppval(spl{i},t);
+end
+end % function foft
+
+
+function [str,errorclass] = validstring(arg,valid)
+% validstring: compares a string against a set of valid options
+% usage: [str,errorclass] = validstring(arg,valid)
+%
+% If a direct hit, or any unambiguous shortening is found, that
+% string is returned. Capitalization is ignored.
+%
+% arguments: (input)
+%  arg - character string, to be tested against a list
+%        of valid choices. Capitalization is ignored.
+%
+%  valid - cellstring array of alternative choices
+%
+% Arguments: (output)
+%  str - string - resulting choice resolved from the
+%        list of valid arguments. If no unambiguous
+%        choice can be resolved, then str will be empty.
+%
+%  errorclass - string - A string argument that explains
+%        the error. It will be one of the following
+%        possibilities:
+%
+%        ''  --> No error. An unambiguous match for arg
+%                was found among the choices.
+%
+%        'No match found' --> No match was found among 
+%                the choices provided in valid.
+%
+%        'Ambiguous argument' --> At least two ambiguous
+%                matches were found among those provided
+%                in valid.
+%        
+%
+% Example:
+%  valid = {'off' 'on' 'The sky is falling'}
+%  
+%
+% See also: parse_pv_pairs, strmatch, strcmpi
+%
+% Author: John D'Errico
+% e-mail: woodchips@rochester.rr.com
+% Release: 1.0
+% Release date: 3/25/2010
+
+ind = find(strncmpi(lower(arg),valid,numel(arg)));
+if isempty(ind)
+  % No hit found
+  errorclass = 'No match found';
+  str = '';
+elseif (length(ind) > 1)
+  % Ambiguous arg, hitting more than one of the valid options
+  errorclass = 'Ambiguous argument';
+  str = '';
+  return
+else
+  errorclass = '';
+  str = valid{ind};
+end
+
+end % function validstring
+

bag/analysys/matchaxes.asv

+function matchaxes(GT, TS, t0, t1)
+    [GTpos TSpos] = synchronize(GT.pos, TS.pos, 'Intersection');
+    [GTang TSang] = synchronize(GT.ang, TS.ang, 'Union');
+    GT = tscollection({GTpos GTang});
+    TS = tscollection({TSpos TSang});
+    Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos.Data;
+    Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos.Data;
+    Tt = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).Time;
+    Gt = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).Time;
+    Grot = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).ang.Data;
+    Trot = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).ang.Data;
+
+    rotangles = Grot(1,:)-Trot(1,:)
+    rotmatrix = eul2rotm(rotangles)
+    Tpos_at_origin = (Tpos - Tpos(1,:));
+    Tpos_rotated_at_origin = Tpos_at_origin*rotmatrix;
+    Tpos_at_Gpos_origin = Tpos_rotated_at_origin + Gpos(1,:);
+
+    figure
+    plot(Gt, Gpos)
+    hold on
+    plot(Tt, Tpos_at_Gpos_origin)
+
+end
\ No newline at end of file

bag/analysys/matchaxes.m

+function [Tpos_at_Gpos_origin, Gdata] = matchaxes(GT, TS, t0, t1)
+    Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos;
+    Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos;
+    [Gpos, Tpos] = synchronize(Gpos, Tpos, 'Union', 'KeepOriginalTimes',true);
+
+    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;
+
+
+    rotangles = Grot.Data(1,:)-Trot.Data(1,:);
+    rotmatrix = eul2rotm(rotangles);
+    Tpos_at_origin = (Tpos.Data - Tpos.Data(1,:));
+    Tpos_rotated_at_origin = Tpos_at_origin*rotmatrix;
+
+    Tpos_at_Gpos_origin = Tpos_rotated_at_origin + Gpos.Data(1,:);
+    Gdata = Gpos.Data;
+
+end
\ No newline at end of file

bag/analysys/ploterrs.m

+function ploterrs(ts0, ts)
+    edist = ts.pos - ts0.pos;
+    eang = ts.ang - ts0.ang;
+    edist.name='Distance error';
+    eang.name='Angular error';
+    edist.DataInfo.Units='meters';
+    eang.DataInfo.Units='radians';
+    edist.TimeInfo.Units='seconds';
+    edist.TimeInfo.Format='SS';
+    eang.TimeInfo.Units='seconds';
+    eang.TimeInfo.Format='SS';
+    
+    %Plot norm of position
+    figure
+    plot(edist.Time, vecnorm([edist.Data(:,1)'; edist.Data(:,2)'; edist.Data(:,3)']))
+    title(edist.name)
+    xlabel(edist.TimeInfo.Units)
+    ylabel(edist.DataInfo.Units)
+    
+    %Plot error of angle
+    figure
+    plot(eang.Time, abs(eang.Data))
+    title(eang.name)
+    xlabel(eang.TimeInfo.Units)
+    ylabel(eang.DataInfo.Units)
+end
\ No newline at end of file

bag/generate_trajectory_csv.bash

@@ -13,8 +13,8 @@ fi
 #   Starting with E ==> experiment, we extract the poses
 for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
 do
-  rm recordings/trajectory_recording$yamlname.bag
   yamlname=${yamlfile%.*}
+  rm recordings/trajectory_recording$yamlname.bag
   roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname rviz:=false
   if [[ ${yamlname:0:1} == 'G' ]]
   then
@@ -25,12 +25,15 @@ do
     rm CSV/$yamlname.csv
     rostopic echo -b recordings/trajectory_recording$yamlname\_tmp.bag /wolf_ros_node/trajectory -p > CSV/$yamlname.csv
     rm recordings/trajectory_recording$yamlname\_tmp.bag
+
+    rm CSV/transp/$yamlname\_transp.csv
+    python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
+
   elif [[ ${yamlname:0:1} == 'E' ]]
   then
     rm CSV/$yamlname.csv
     rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+    cp CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
   fi
-  rm CSV/transp/$yamlname\_transp.csv
-  python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
   
 done

bag/transpose_csv.py

 import pandas as pd
 import sys
 def transpose(in_bag, out_bag):
-    pd.read_csv(in_bag, header=None).T.to_csv(out_bag, header=False, index=False)
+    pd.read_csv(in_bag, header=None, dtype='unicode').T.to_csv(out_bag, header=False, index=False)
 
 if __name__ == '__main__':
     in_bag = sys.argv[1]

yaml/trajectory_analysys/E-1_0.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/wolf_demo_profiling_imu2d_test5.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_E-1_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -137,7 +137,7 @@ config:
       type: "PublisherPose"
       topic: "pose"
       extrinsics: false
-      period: 0.005
+      period: 0.01
       frame_id: "map"
     -
       package: "wolf_ros_laser"

yaml/trajectory_analysys/G-1_0.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/wolf_demo_profiling_imu2d_test0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_G-1_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4