function cl = orthviews(image_obj, varargin)
% Orthviews display (SPM) for CANlab image_vector (or fmri_data, statistic_image) object
%
% *Usage:
% ::
%
%    orthviews(image_obj, varargin)
%
% :Optional Inputs:
%   **'posneg':**
%        input generates orthviews using solid colors, separated for positive- and negative-valued voxels.
%
%   **'largest_region':**
%        to center the orthviews on the largest region in the image
%
%   **'overlay':**
%        followed by name of anatomical image to use as underlay
%
%   **'unique':**
%        plot groups of contiguous voxels in different, unique colors
%        - autodetect feature turns this on when there are <300 unique
%        values, or for 'atlas' objects. See 'continuous' below to force continuous.
%
%   **'parcels':**
%        plot voxels with each unique value in a different color
%        useful for images in which values indicate parcel or network
%        membership
%        Default is 'unique' for <300 values, 'continuous' for more.  Use
%        'unique' to force unique-valued colors.
%
%   **'continuous':**
%        Plot voxels color-mapped according to their values.
%        Default is 'unique' for <300 values, 'continuous' for more.  Use
%        'unique' to force unique-valued colors.
%
%   **'input_handle':**
%        followd by integer for which orthviews display to add to.
%        spm_orthviews can display multiple linked orthviews sections. If
%        you have initialized a display with multiple images, Enter 1 to
%        add to the first one, 2 for the second, and so forth.
%
%   **'add':**
%        Add to the first orthviews display (e.g., 'input_handle', 1)
%
%   **'color':**
%       Followed by cell with 3-element rgb vector,
%       e.g., 'color', {[1 0 1]}
%       This is superseded by 'unique' and 'posneg' options.
%
% ..
%    Copyright Tor Wager, 2011
% ..

% Aug 2017: Tor Wager edited to allow flexibility in colors; unique,
% parcel, continuous. and autodetect.

%overlay = which('SPM8_colin27T1_seg.img');
overlay = which('keuken_2014_enhanced_for_underlay.img');

doposneg = false;
doreg = false;
input_handle=[];
dounique = false;
doparcels = false;
force_continuous = false;
additional_inputs = {};

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            % reserved keywords
            case 'posneg',  doposneg = 1;

                % functional commands
            case 'overlay', overlay = varargin{i + 1}; varargin{i + 1} = [];

            case {'han', 'handle', 'input_handle'}, input_handle = varargin{i+1};

            case 'add', input_handle = 1;

            case {'largest_region', 'largest_cluster'}, doreg = 1;

            case 'unique'
                dounique = true;

            case 'continuous'
                force_continuous = true;

            case 'parcels'
                doparcels = true; dounique = true;

            case 'color'
                additional_inputs{end + 1} = varargin{i};

                if ~iscell(varargin{i + 1})
                    warning('Color input must be a cell array. Check inputs');
                    varargin{i + 1} = {varargin{i + 1}};
                end

                additional_inputs{end + 1} = varargin{i + 1};

            case 'trans'
                additional_inputs{end + 1} = 'trans';

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

if ~exist(overlay, 'file')
    overlay = which(overlay);
end

if isempty(overlay) || ~exist(overlay, 'file')
    disp('Cannot find overlay image:')
    disp(overlay)
    return
end

if isempty(input_handle)
    % re-initialize new orthviews

    n = size(image_obj.dat, 2);
    if n > 24
        disp('Warning! Only first 24 images can displayed with spm_orthviews.');
        n = 24;
    end

    spm_check_registration(repmat(overlay, n, 1));

    handle_indices = 1:n;

else

    % use existing
    handle_indices = input_handle;

end

% replace missing voxels if necessary
image_obj = replace_empty(image_obj);

% enforce double, same var types across all objects
image_obj.dat = double(image_obj.dat);

% autodetect parcels and turn on 'unique' for maps with few values
% or atlas objects.
% (default = do this)
if isa(image_obj, 'atlas') && ~force_continuous
    dounique = true; doparcels = true;
end

u = unique(image_obj.dat(:));

if ~doparcels && ~force_continuous && all(u == round(u))
    % auto-select parcel unique color plot if there are few unique values and values are integers
    nvalues = length(u);

    if nvalues > 2 && nvalues < 300, dounique = true; doparcels = true; end
end

for i = handle_indices

    wh_image = min(i, size(image_obj.dat,2));  %min() is to differentiate between which subplot to plot on and which image to plot.  used when this is called with orthviews_multiple_objs.  Yoni 11/14

    if doparcels

        cl{i} = region(get_wh_image(image_obj, wh_image), 'unique_mask_values');

    else
        cl{i} = region(get_wh_image(image_obj, wh_image), 'contiguous_regions');

    end

    % old:
    %cl{i} = iimg_indx2clusters(image_obj.dat(:, wh_image), image_obj.volInfo);

    if dounique

        %colors = custom_colors([1 1 0], [0 0 1], length(cl{i}));

        if ~isempty(cl{i})
            cluster_orthviews(cl{i}, 'unique', 'add', 'handle', i, additional_inputs{:});
        else
            fprintf('Image %3.0f empty\n', i);
        end

    elseif doposneg

        warning off
        cl{i} = cluster2region(cl{i});
        warning on % name field warning

        [clpos{i}, clneg{i}] = posneg_separate(cl{i}, 'Z');

        if ~isempty(clpos{i})
            cluster_orthviews(clpos{i}, {[1 1 0]}, 'add', 'handle', i,  'solid'); % additional_inputs{:}); % 'solid');
        else
            fprintf('Image %3.0f: No positive clusters\n', i);
        end

        if ~isempty(clneg{i})
            cluster_orthviews(clneg{i}, {[0 0 1]}, 'add', 'handle', i,  'solid'); % additional_inputs{:}); % 'solid');
        else
            fprintf('Image %3.0f: No negative clusters\n', i);
        end

    else
        if ~isempty(cl{i})
            cluster_orthviews(cl{i}, additional_inputs{:}, 'add', 'handle', i, additional_inputs{:});
        else
            fprintf('Image %3.0f empty\n', i);
        end
    end

end

% Set colormap
% If one image, we can use 'hot/cool' split, which shows zero point
% If multiple images, default to a single colormap, as the entire
% figure can use only one colormap. Note: colormaps can apply to specific
% axes...

if length(handle_indices) > 1
    %spm_orthviews_change_colormap([0 0 1], [1 1 0], [0 1 1], [.5 .5 .5], [1 .5 0]);
    %spm_orthviews_change_colormap([.5 0 1], [1 1 0]);
    spm_orthviews_change_colormap([.2 .2 .6], [1 1 0]);  % slate to yellow

    % won't work now -
    %     maxposcolor = [1 1 0]; % max pos, most extreme values
    %     minposcolor = [1 .4 .5]; % [.8 .3 0]; % min pos
    %     maxnegcolor = [0 .8 .8]; % [.3 .6 .9]; % max neg
    %     minnegcolor = [0 0 1]; % min neg, most extreme values
    %
    %     spm_orthviews_change_colormap(minnegcolor, maxnegcolor, minposcolor, maxposcolor);

else % one image.
    spm_orthviews_hotcool_colormap(image_obj.dat(:), 0);
end

% center image
spm_orthviews('Reposition', [0 0 0]);

% if requested, try to center on largest region
if doreg
    [~,wh] = max(cat(1,cl{1}.numVox));
    spm_orthviews('Reposition', cl{1}(wh).mm_center);
end
drawnow;

end % function


function y = real( x )

%   Disciplined convex/geometric programming information for REAL:
%      REAL(X) may be freely applied to any CVX expression. However,
%      since REAL(X)=X for all real expressions (including convex,
%      concave, log-convex, and log-concave), it is only useful in
%      the complex affine case.

y = cvx( x.size_, real( x.basis_ ) );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


%% CONTRAST2_BATCH_ITHACA uses the BATCH command to run the CONTRAST2 code on the Ithaca cluster.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2010
%
%  Author:
%
%    John Burkardt
%
  clear

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONTRAST2_BATCH_ITHACA\n' );
  fprintf ( 1, '  Run CONTRAST2_SCRIPT on Ithaca.\n' );
%
%  Read a black and white image and filter it locally.
%
  x = imread ( 'surfsup.tif' );
  xf = nlfilter ( x, [3,3], @contrast_enhance );
  xf = uint8 ( xf );
%
%  Set the number of workers to use.
%
  n = 4;
%
%  The BATCH command sends the script for execution.
%
  job = batch ( 'contrast2_script', ...
    'Configuration', 'ithaca_2009b', ...
    'CaptureDiary', true, ...
    'FileDependencies', { 'contrast2_fun', 'contrast_enhance', 'surfsup.tif' }, ...
    'CurrentDirectory', '/home/burkardt/matlab', ...
    'matlabpool', n );
%
%  WAIT pauses the MATLAB session til the job completes.
%
  wait ( job );
%
%  DIARY displays any messages printed during execution.
%
  diary ( job );
%
%  LOAD makes the script's workspace available.
%
%  y = output #1.
%
  load ( job );
%
%  DESTROY cleans up data about the job we no longer need.
%
  destroy ( job );
%
%  Display the original, single filtered, and SPMD filtered versions.
%
  figure ( 1 );

  subplot ( 1, 3, 1 );
  imshow ( x );
  title ( 'Original image' );

  subplot ( 1, 3, 2 );
  imshow( xf );
  title( 'Filtered on Client' );

  subplot ( 1, 3, 3 );
  imshow ( y );
  title ( 'Filtered on 4 SPMD Workers' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONTRAST2_BATCH_ITHACA\n' );
  fprintf ( 1, '  Normal end of execution.\n' );


function trx = SetLandmarkParameters_px(trx,arenatype,arenacenterx,arenacentery,...
  arenaradius)

switch lower(arenatype),
  case 'none',
    return;
  case 'circle',
    for i = 1:numel(trx),
      trx(i).arena.r = arenaradius;
      trx(i).arena.x = arenacenterx;
      trx(i).arena.y = arenacentery;
    end
  case 'rectangle',
end


function linpack_z_test ( )

%*****************************************************************************80
%
%% LINPACK_Z_TEST tests the LINPACK_Z library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINPACK_Z_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the LINPACK_Z library. \n' );

  linpack_z_test01 ( );
  linpack_z_test02 ( );
  linpack_z_test03 ( );
  linpack_z_test04 ( );
  linpack_z_test05 ( );
  linpack_z_test06 ( );
  linpack_z_test07 ( );
  linpack_z_test08 ( );
  linpack_z_test09 ( );

  linpack_z_test10 ( );
  linpack_z_test11 ( );
  linpack_z_test12 ( );
  linpack_z_test13 ( );
  linpack_z_test14 ( );
  linpack_z_test15 ( );
  linpack_z_test16 ( );
  linpack_z_test17 ( );
  linpack_z_test18 ( );
  linpack_z_test19 ( );

  linpack_z_test20 ( );
  linpack_z_test21 ( );
  linpack_z_test22 ( );
  linpack_z_test23 ( );
  linpack_z_test24 ( );
  linpack_z_test25 ( );
  linpack_z_test26 ( );
  linpack_z_test27 ( );
  linpack_z_test28 ( );
  linpack_z_test29 ( );

  linpack_z_test30 ( );
  linpack_z_test31 ( );
  linpack_z_test32 ( );
  linpack_z_test33 ( );
  linpack_z_test34 ( );
  linpack_z_test345 ( );
  linpack_z_test35 ( );
  linpack_z_test36 ( );
  linpack_z_test37 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINPACK_Z_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function w = minus(w,q)
   %MINUS (-) Overloaded waveform subtraction    w - q
   %   w is an N-DIMENSIONAL waveform.
   %   if q is 1x1, it is subtracted from all data values of all waveforms
   %
   %   if q is numeric, and has the same size & shape as w, then each q is
   %     subtracted from the appropriate w  ie.  result(n) = w(n) + q(n)
   %
   %   if q is a vector, it should be of same length as waveform's data
   %     sample-by-sample subtracted from each waveform
   %
   %   if q is waveform, data from q are subtracted from w, assuming both
   %   waveforms have the same number of samples
   %
   %   If q is numeric, and w is of type waveform, then the operation
   %   q - w is invalid.  This is because of a possible ambiguous result type.
   %   Instead:
   %       for a numeric result, use the synatax:
   %              q  - double(w)
   %       for a waveform result, use the syntax:
   %              q + -w             or              -w + q
   %
   % See also: minus, wavform/plus, uminus, fix_data_length

   % AUTHOR: Celso Reyes, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   if ~isa(w,'waveform')
      %yipes.  we have a number minus a waveform
      errtext = [sprintf('subtracting a <waveform> from a <%s> leads to ambiguous answer type\n',class(w)), ...
         sprintf('  for a numerical result, try: <%s> - double(<waveform>)\n',class(w)), ...
         sprintf('  for a waveform result, try:  -<waveform> + <%s>\n',class(w))];
      error('Waveform:minus:invalidClass','Error attempting: <%s> - <%s>\n%s', class(w),class(q),errtext);
   end
   try
      w = w + (-q);
   catch caughtvalue
      %NOTE, it is important to keep this section in sync with waveform/plus
      switch caughtvalue.identifier
         case 'Waveform:plus:unknownOperation'
            wsize = num2str(size(w),'%dx'); wsize = wsize(1:end-1);
            qsize = num2str(size(q),'%dx'); qsize = qsize(1:end-1);
            error('Waveform:minus:unknownOperation',...
               ['unknown waveform subtraction operation:\n',...
               '< %s %s>  - <%s %s>'],wsize, class(w), qsize, class(q));
         case 'Waveform:plus:sizeMismatch'
            error('Waveform:minus:sizeMismatch',...
               ['error in dimensions: [NxM] - [MxN].\nOne possible'...
               ' fix would be to transpose ('') one of the terms '])
         case 'Waveform:plus:invalidClass'
            error('Waveform:minus:invalidClass',...
               'unknown subtraction operation: %s - %s', class(w), class(q));
         otherwise
            rethrow(caughtvalue);
      end
   end
   return;
end


classdef PTKLungSurface < PTKPlugin
    % PTKLungSurface. Plugin for finding points around the surface of the lungs
    %
    %     This is a plugin for the Pulmonary Toolkit. Plugins can be run using
    %     the gui, or through the interfaces provided by the Pulmonary Toolkit.
    %     See PTKPlugin.m for more information on how to run plugins.
    %
    %     Plugins should not be run directly from your code.
    %
    %     PTKLungSurface runs the library function PTKGetSurfaceFromSegmentation
    %     to find the lung surface and returns this as an image.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties
        ButtonText = 'Lung Surface'
        ToolTip = 'Segment the lung surface'
        Category = 'Lungs'

        AllowResultsToBeCached = true
        AlwaysRunPlugin = false
        PluginType = 'ReplaceOverlay'
        HidePluginInDisplay = false
        FlattenPreviewImage = false
        PTKVersion = '1'
        ButtonWidth = 6
        ButtonHeight = 2
        GeneratePreview = true
        Visibility = 'Developer'
    end

    methods (Static)
        function results = RunPlugin(dataset, ~)
            lung_mask = dataset.GetResult('PTKLeftAndRightLungs');
            results = lung_mask.BlankCopy;
            lungs = PTKGetSurfaceFromSegmentation(uint8(lung_mask.RawImage > 0));
            results.ChangeRawImage(lungs);
            results.ImageType = PTKImageType.Colormap;
        end
    end
end


function kern = tensorKernParamInit(kern)

% TENSORKERNPARAMINIT TENSOR kernel parameter initialisation.
% The tensor product (TENSOR) kernel is a container kernel for
% allowing tensor products kernels of separate component kernels.
% several different kernels to be added together. It is created by
% using the kernCreate command with the kernel type given as a
% cell and the first entry given as 'tensor'. For example, to
% create a tensor kernel compound kernel that is composed of
% an RBF kernel and a LIN kernel you call,
%
% kern = kernCreate(X, {'tensor', 'rbf', 'lin'});
%
% Each individual kernel is then stored within the returned kernel
% structure. The kernels are stored in order in a field called
% 'comp'. So display obtain the 'rbf' kernel you write:
%
% kernDisplay(kern.comp{1})
%
% SEEALSO : cmpndKernParamInit, multiKernParamInit
%
% FORMAT
% DESC initialises the tensor product
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

kern.nParams = 0;
kern.transforms = [];
if ~isfield(kern, 'comp')
  kern.comp=cell(0);
end
for i = 1:length(kern.comp)
  kern.comp{i} = kernParamInit(kern.comp{i});
  kern.nParams = kern.nParams + kern.comp{i}.nParams;
  kern.comp{i}.index = [];
end
kern.paramGroups = speye(kern.nParams);

% Warn if component kernels have white variance and check if
% resulting kernel is stationary.
kern.isStationary = true;
whiteVariance = 0;
for i = 1:length(kern.comp)
  if ~kern.comp{i}.isStationary
    kern.isStationary = false;
  end
  if strcmp(kern.comp{i}.type, 'white')
    whiteVariance = whiteVariance + kern.comp{i}.variance;
  else
    if(isfield(kern.comp{i}, 'whiteVariance'))
      whiteVariance = whiteVariance + ...
          kern.comp{i}.whiteVariance;
    end
  end
end
if whiteVariance > 0
  warning(['Components of tensor kernel have non-zero white variance. ' ...
            'This can cause problems some implementations.']);
end


function varargout=rotmr2qr(varargin)
%
% For calling details please see v_rotmr2qr.m
%
% This dummy routine is included for backward compatibility only
% and will be removed in a future release of voicebox. Please use
% v_rotmr2qr.m in future and/or update with v_voicebox_update.m
%
%      Copyright (C) Mike Brookes 2018
%      Version: $Id: rotmr2qr.m 10863 2018-09-21 15:39:23Z dmb $
%
if nargout
    varargout=cell(1,nargout);
    [varargout{:}]=v_rotmr2qr(varargin{:});
else
    v_rotmr2qr(varargin{:});
end


function cvt_write ( ndim, n, batch, seed_init, seed, init_string, it_max, ...
  it_fixed, it_num, it_diff, energy, sample_string, sample_num, r, ...
  file_out_name )

%*****************************************************************************80
%
%% CVT_WRITE writes a CVT dataset to a file.
%
%  Discussion:
%
%    The initial lines of the file are comments, which begin with a
%    '#' character.
%
%    Thereafter, each line of the file contains the NDIM-dimensional
%    components of the next entry in the set.
%
%  Modified:
%
%    17 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NDIM, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer BATCH, sets the maximum number of sample points
%    generated at one time.  It is inefficient to generate the sample
%    points 1 at a time, but memory intensive to generate them all
%    at once.  You might set BATCH to min ( SAMPLE_NUM, 10000 ), for instance.
%
%    Input, integer SEED_INIT, the initial random number seed.
%
%    Input, integer SEED, the current random number seed.
%
%    Input, character INIT_STRING(*), specifies how the initial
%    generators are chosen:
%    filename, by reading data from a file;
%    'GRID', picking points from a grid;
%    'HALTON', from a Halton sequence;
%    'RAND', using MATLAB RAND function;
%    'UNIFORM', using a simple uniform RNG;
%
%    Input, integer IT_MAX, the maximum number of iterations allowed.
%
%    Input, integer IT_FIXED, the maximum number of iterations to take
%    with a fixed set of sample points.
%
%    Input, integer IT_NUM, the actual number of iterations taken.
%
%    Input, real IT_DIFF, the L2 norm of the change
%    in the CVT coordinates on the last iteration.
%
%    Input, real ENERGY, the discrete "energy", divided
%    by the number of sample points.
%
%    Input, string SAMPLE_STRING, specifies how the region is sampled:
%    'GRID', picking points from a grid;
%    'HALTON', from a Halton sequence;
%    'RAND', using MATLAB RAND function;
%    'UNIFORM', using a simple uniform RNG;
%
%    Input, integer SAMPLE_NUM, the number of sampling points used on
%    each iteration.
%
%    Input, real R(NDIM,N), the points.
%
%    Input, string FILE_OUT_NAME, the name of
%    the output file.
%
  file_out_unit = fopen ( file_out_name, 'w' );

  if ( file_out_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_WRITE - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file:\n' );
    fprintf ( 1, '  "%s".\n', file_out_name );
    error ( 'CVT_WRITE - Fatal error!' );
  end

  fprintf ( file_out_unit, '#  %s\n', file_out_name );
  fprintf ( file_out_unit, '#  created by CVT_WRITE.M\n' );
  fprintf ( file_out_unit, '#\n' );
  fprintf ( file_out_unit, '#  Spatial dimension NDIM =   %12d\n', ndim );
  fprintf ( file_out_unit, '#  Number of points N =       %12d\n', n );
  fprintf ( file_out_unit, '#  Initial SEED_INIT =        %12d\n', seed_init );
  fprintf ( file_out_unit, '#  Current SEED =             %12d\n', seed );
  fprintf ( file_out_unit, '#  INIT =                    "%s"\n', init_string );
  fprintf ( file_out_unit, '#  Max iterations IT_MAX =    %12d\n', it_max );
  fprintf ( file_out_unit, '#  IT_FIXED (fixed samples) = %12d\n', it_fixed );
  fprintf ( file_out_unit, '#  Iterations IT_NUM =        %12d\n', it_num );
  fprintf ( file_out_unit, '#  Difference IT_DIFF =       %12f\n', it_diff );
  fprintf ( file_out_unit, '#  CVT ENERGY =               %12f\n', energy );
  fprintf ( file_out_unit, '#  SAMPLE =                  "%s"\n', sample_string );
  fprintf ( file_out_unit, '#  SAMPLE_NUM =               %12d\n', sample_num );
  fprintf ( file_out_unit, '#  Sample BATCH size =        %12d\n', batch );
  fprintf ( file_out_unit, '#  EPSILON (unit roundoff ) = %e\n', eps );
  fprintf ( file_out_unit, '#\n' );

  for j = 1 : n
    for i = 1 : ndim
      fprintf ( file_out_unit, '  %10f', r(i,j) );
    end
    fprintf ( file_out_unit, '\n' );
  end

  fclose ( file_out_unit );

  return
end


fileII = 'test2d_triangle';
b.fileName = fileII;
d = FemDataContainer(b);
femII = FEM.create(d);
femII.solve();
femII.print(fileII);


function [flag]=nonBlocking_isGoalReached(t_Kuka)
% this functions is used with nonBlocking motion functions to chekc whether
% the goal is reached.

%% Syntax:
% [flag]=nonBlocking_isGoalReached()

%% Arreguments
% t_Kuka: is the TCP/IP connection object

%% return value
% flag is a numerical value:
% flag=1: signifies the goal is reached
% flag=0: signifies the goal is not reached
% flag=-1: signifies an error

% Copy right: Mohammad SAFEEA 04th-April-2018

global paramName;
global paramVal;
x='eefpos';
%% if motion at EEF level
if(strcmp(x,paramName))
    % check size of array
    if(max(size(paramVal))==6)
    else
        flag=-1;
        return;
    end
% check if parameters are equal
flag=1;
    [eefpos,tempFlag]=GetActualEEFpos( t_Kuka );
    if tempFlag==false
        flag=1;
        return;
    end
    for i=1:6
        x1=paramVal{i};
        x2=eefpos{i};
        clearance=0.2; % 0.2 mm precission
        if((x1>(x2-clearance))&&(x1<(x2+clearance)))
        else
            flag=0;
            return;
        end
    end
    pause(0.1);
    return;
end
%% if motion at joint space level
x='jpos';
if(strcmp(x,paramName))
    % check size of array
    if(max(size(paramVal))==7)
    else
        flag=-1;
        return;
    end
    % check if parameters are equal
    flag=1;
    [eefpos,tempFlag]=GetActualJOINTSpos( t_Kuka );
    if tempFlag==false
        flag=1;
        return;
    end
    for i=1:6
        x1=paramVal{i};
        x2=eefpos{i};
        clearance=0.1*pi/180; % 0.1 degree precission
        if((x1>(x2-clearance))&&(x1<(x2+clearance)))
        else
            flag=0;
            return;
        end
    end
    pause(0.1);
    return;
end
end

function [ EEFpos,flag ] = GetActualEEFpos( t_Kuka )
EEEFpos={0,0,0,0,0,0};
theCommand='DcSeCarEEfP_';

for i=1:6
    x=EEEFpos{i};
    theCommand=[theCommand,sprintf('%0.5f',x),'_'];
end
% send the message through network
fprintf(t_Kuka, theCommand);
message=fgets(t_Kuka);
[EEFpos,N]=getDoubleFromString(message);
if(N==0)
    flag=false;
    message=fgets(t_Kuka);
    [EEFpos,N]=getDoubleFromString(message);
else
    flag=true;
end
end

function [ EEFpos,flag ] = GetActualJOINTSpos( t_Kuka )
EEEFpos={0,0,0,0,0,0};
theCommand='DcSeCarJP_';

for i=1:6
    x=EEEFpos{i};
    theCommand=[theCommand,sprintf('%0.5f',x),'_'];
end
% send the message through network
fprintf(t_Kuka, theCommand);
message=fgets(t_Kuka);
[EEFpos,N]=getDoubleFromString(message);
if(N==0)
    flag=false;
    message=fgets(t_Kuka);
    [EEFpos,N]=getDoubleFromString(message);
else
    flag=true;
end
end

function [jPos,j]=getDoubleFromString(message)
n=max(max(size(message)));
j=0;
numString=[];
for i=1:n
    if message(i)=='_'
        j=j+1;
        jPos{j}=str2num(numString);
        numString=[];
    else
        numString=[numString,message(i)];
    end
end
if j==0
    jPos={0};
end
end


function [concentrationMatrix, excRxnNames, timeVec, biomassVec] = dynamicFBA(model, substrateRxns, initConcentrations, initBiomass, timeStep, nSteps, plotRxns, exclUptakeRxns)
% Performs dynamic FBA simulation using the static optimization approach
%
% USAGE:
%
%    [concentrationMatrix, excRxnNames, timeVec, biomassVec] = dynamicFBA(model, substrateRxns, initConcentrations, initBiomass, timeStep, nSteps, plotRxns, exclUptakeRxns)
%
% INPUTS:
%    model:                  COBRA model structure
%    substrateRxns:          List of exchange reaction names for substrates
%                            initially in the media that may change (e.g. not
%                            h2o or co2)
%    initConcentrations:     Initial concentrations of substrates (in the same
%                            structure as `substrateRxns`)
%    initBiomass:            Initial biomass (must be non zero)
%    timeStep:               Time step size
%    nSteps:                 Maximum number of time steps
%
% OPTIONAL INPUTS:
%    plotRxns:               Reactions to be plotted (Default = {'EX_glc(e)', 'EX_ac(e)', 'EX_for(e)'})
%    exclUptakeRxns:         List of uptake reactions whose substrate concentrations do not change
%                            (Default = {'EX_co2(e)', 'EX_o2(e)', 'EX_h2o(e)', 'EX_h(e)'})
%
% OUTPUTS:
%    concentrationMatrix:    Matrix of extracellular metabolite concentrations
%    excRxnNames:            Names of exchange reactions for the EC metabolites
%    timeVec:                Vector of time points
%    biomassVec:             Vector of biomass values
%
% If no initial concentration is given for a substrate that has an open
% uptake in the model (i.e. `model.lb < 0`) the concentration is assumed to
% be high enough to not be limiting. If the uptake rate for a nutrient is
% calculated to exceed the maximum uptake rate for that nutrient specified
% in the model and the max uptake rate specified is > 0, the maximum uptake
% rate specified in the model is used instead of the calculated uptake
% rate.
%
% NOTE:
%
%    The dynamic FBA method implemented in this function is essentially
%    the same as the method described in
%    [`Varma, A., and B. O. Palsson. Appl. Environ. Microbiol. 60:3724 (1994)`].
%    This function does not implement the dynamic FBA using dynamic optimization approach
%    described in [`Mahadevan, R. et al. Biophys J, 83:1331-1340 (2003)`].
%
% .. Author: - Markus Herrgard 8/22/06

global WAITBAR_TYPE

if (nargin < 7)
    plotRxns = {'EX_glc(e)','EX_ac(e)','EX_for(e)'};
end

% Uptake reactions whose substrate concentrations do not change
if (nargin < 8)
    exclUptakeRxns = {'EX_co2(e)','EX_o2(e)','EX_h2o(e)','EX_h(e)'};
end

% Find exchange rxns
excInd = findExcRxns(model,false);
excInd = excInd & ~ismember(model.rxns,exclUptakeRxns);
excRxnNames = model.rxns(excInd);
length(excRxnNames)
% Figure out if substrate reactions are correct
missingInd = find(~ismember(substrateRxns,excRxnNames));
if (~isempty(missingInd))
    for i = 1:length(missingInd)
        fprintf('%s\n',substrateRxns{missingInd(i)});
    end
    error('Invalid substrate uptake reaction!');
end

% Initialize concentrations
[~, substrateMatchInd] = ismember(substrateRxns,excRxnNames);
concentrations = zeros(length(excRxnNames),1);
concentrations(substrateMatchInd) = initConcentrations;

% Deal with reactions for which there are no initial concentrations
originalBound = -model.lb(excInd);
noInitConcentration = (concentrations == 0 & originalBound > 0);
concentrations(noInitConcentration) = 1000;

biomass = initBiomass;

% Initialize bounds
uptakeBound =  concentrations/(biomass*timeStep);

% Make sure bounds are not higher than what are specified in the model
aboveOriginal = (uptakeBound > originalBound) & (originalBound > 0);
uptakeBound(aboveOriginal) = originalBound(aboveOriginal);
model.lb(excInd) = -uptakeBound;

concentrationMatrix = sparse(concentrations);
biomassVec = biomass;
timeVec(1) = 0;

fprintf('Step number\tBiomass\n');
showprogress(0,'Dynamic FBA analysis in progress ...');
for stepNo = 1:nSteps
    % Run FBA
    sol = optimizeCbModel(model,'max','one');
    mu = sol.f;
    if (sol.stat ~= 1 || mu == 0)
        fprintf('\nNo feasible solution - nutrients exhausted. Biomass:\t %f\n', biomass);
        break;
    end
    uptakeFlux = sol.x(excInd);
    biomass = biomass*exp(mu*timeStep);
    %biomass = biomass*(1+mu*timeStep);
    biomassVec(end+1) = biomass;

    % Update concentrations
    concentrations = concentrations - uptakeFlux/mu*biomass*(1-exp(mu*timeStep));
    %concentrations = concentrations + uptakeFlux*biomass*timeStep;
    concentrations(concentrations <= 0) = 0;
    concentrationMatrix(:,end+1) = sparse(concentrations);

    % Update bounds for uptake reactions
    uptakeBound =  concentrations/(biomass*timeStep);
    % This is to avoid any numerical issues
    uptakeBound(uptakeBound > 1000) = 1000;
    % Figure out if the computed bounds were above the original bounds
    aboveOriginal = (uptakeBound > originalBound) & (originalBound > 0);
    % Revert to original bounds if the rate was too high
    uptakeBound(aboveOriginal) = originalBound(aboveOriginal);
    uptakeBound(abs(uptakeBound) < 1e-9) = 0;

    model.lb(excInd) = -uptakeBound;

    if WAITBAR_TYPE ~= 1
        fprintf('%d\t%f\n',stepNo,biomass);
    end
    showprogress(stepNo/nSteps);
    timeVec(stepNo+1) = stepNo*timeStep;
end

selNonZero = any(concentrationMatrix>0,2);
concentrationMatrix = concentrationMatrix(selNonZero,:);
excRxnNames = excRxnNames(selNonZero);
selPlot = ismember(excRxnNames,plotRxns);

% Plot concentrations as a function of time
clf
subplot(1,2,1);
plot(timeVec,biomassVec);
axis tight
title('Biomass');
subplot(1,2,2);
plot(timeVec,concentrationMatrix(selPlot,:));
axis tight
legend(strrep(excRxnNames(selPlot),'EX_',''));


plotAbsErr = true;

%% 500-1000

swf10 = load('swf_500_1000_10_dataset3.mat');
swf50 = load('swf_500_1000_50_dataset3.mat');
swf100 = load('swf_500_1000_100_dataset3.mat');
msckf5 = load('msckf_500_1000_min5_maxInf');
msckf10 = load('msckf_500_1000_min10_max50');
msckf20 = load('msckf_500_1000_min20_max100');
imu = load('imu_500_1000.mat');

load('../datasets/dataset3.mat');
kStart = 500;
kEnd = 1000;

transLim = [-0.5 0.5];
rotLim = [-0.25 0.25];
fontSize = 14;

if plotAbsErr
    imu.msckf_trans_err = abs(imu.msckf_trans_err);
    msckf5.msckf_trans_err = abs(msckf5.msckf_trans_err);
    msckf10.msckf_trans_err = abs(msckf10.msckf_trans_err);
    msckf20.msckf_trans_err = abs(msckf20.msckf_trans_err);
    swf10.swf_trans_err = abs(swf10.swf_trans_err);
    swf50.swf_trans_err = abs(swf50.swf_trans_err);
    swf100.swf_trans_err = abs(swf100.swf_trans_err);

    imu.msckf_rot_err = abs(imu.msckf_rot_err);
    msckf5.msckf_rot_err = abs(msckf5.msckf_rot_err);
    msckf10.msckf_rot_err = abs(msckf10.msckf_rot_err);
    msckf20.msckf_rot_err = abs(msckf20.msckf_rot_err);
    swf10.swf_rot_err = abs(swf10.swf_rot_err);
    swf50.swf_rot_err = abs(swf50.swf_rot_err);
    swf100.swf_rot_err = abs(swf100.swf_rot_err);

    transLim = transLim - transLim(1);
    rotLim = rotLim - rotLim(1);
end

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_trans_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(1,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(1,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(1,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(1,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
set(h_legend,'FontSize',10);

title(sprintf('Absolute Translational Error (Interval 1)'))
ylabel('\delta r_x [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_trans_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(2,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(2,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(2,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(2,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_y [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_trans_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(3,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(3,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(3,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(3,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_z [m]')
xlabel('t_k [s]')
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)
grid on
grid minor
box on

filename = sprintf('6-Way-Comparison-500-1000-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_rot_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(1,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(1,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(1,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(1,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
title(sprintf('Absolute Rotational Error (Interval 1)'))
ylabel('\delta\theta_x')
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
set(h_legend,'FontSize',10);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_rot_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(2,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(2,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(2,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(2,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
ylabel('\delta\theta_y')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_rot_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(3,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(3,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(3,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(3,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
ylabel('\delta\theta_z')
xlabel('t_k [s]')
grid on
grid minor
box on

set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('6-Way-Comparison-500-1000-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% 1215-1715

swf10 = load('swf_1215_1715_10_dataset3.mat');
swf50 = load('swf_1215_1715_50_dataset3.mat');
swf100 = load('swf_1215_1715_100_dataset3.mat');
msckf5 = load('msckf_1215_1715_min5_maxInf');
msckf10 = load('msckf_1215_1715_min10_max50');
msckf20 = load('msckf_1215_1715_min20_max100');
imu = load('imu_1215_1715.mat');

plotAbsErr = true;

load('../datasets/dataset3.mat');
kStart = 1215;
kEnd = 1715;

transLim = [0 1];
rotLim = [0 1];
fontSize = 14;

if plotAbsErr
    imu.msckf_trans_err = abs(imu.msckf_trans_err);
    msckf5.msckf_trans_err = abs(msckf5.msckf_trans_err);
    msckf10.msckf_trans_err = abs(msckf10.msckf_trans_err);
    msckf20.msckf_trans_err = abs(msckf20.msckf_trans_err);
    swf10.swf_trans_err = abs(swf10.swf_trans_err);
    swf50.swf_trans_err = abs(swf50.swf_trans_err);
    swf100.swf_trans_err = abs(swf100.swf_trans_err);

    imu.msckf_rot_err = abs(imu.msckf_rot_err);
    msckf5.msckf_rot_err = abs(msckf5.msckf_rot_err);
    msckf10.msckf_rot_err = abs(msckf10.msckf_rot_err);
    msckf20.msckf_rot_err = abs(msckf20.msckf_rot_err);
    swf10.swf_rot_err = abs(swf10.swf_rot_err);
    swf50.swf_rot_err = abs(swf50.swf_rot_err);
    swf100.swf_rot_err = abs(swf100.swf_rot_err);

    transLim = transLim - transLim(1);
    rotLim = rotLim - rotLim(1);
end

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_trans_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(1,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(1,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(1,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(1,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
set(h_legend,'FontSize',10);

title(sprintf('Absolute Translational Error (Interval 2)'))
ylabel('\delta r_x [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_trans_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(2,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(2,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(2,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(2,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_y [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_trans_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_trans_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_trans_err(3,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_trans_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_trans_err(3,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_trans_err(3,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_trans_err(3,:), '--g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_z [m]')
xlabel('t_k [s]')
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)
grid on
grid minor
box on

filename = sprintf('6-Way-Comparison-1215-1715-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_rot_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(1,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(1,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(1,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(1,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
title(sprintf('Absolute Rotational Error (Interval 2)'))
ylabel('\delta\theta_x')
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
set(h_legend,'FontSize',10);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_rot_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(2,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(2,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(2,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(2,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
ylabel('\delta\theta_y')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_rot_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5.msckf_rot_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10.msckf_rot_err(3,:), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20.msckf_rot_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10.swf_rot_err(3,:), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50.swf_rot_err(3,:), '-.g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100.swf_rot_err(3,:), '--g', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
ylabel('\delta\theta_z')
xlabel('t_k [s]')
grid on
grid minor
box on

set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('6-Way-Comparison-1215-1715-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% Less Noisy Dataset

msckf20 = load('msckf_1215_1715_min20_max100_20lessnoisy.mat');
%msckf40 = load('msckf_1215_1715_min20_max100_40lessnoisy.mat');
msckf60 = load('msckf_1215_1715_min20_max100_60lessnoisy.mat');
%msckf80 = load('msckf_1215_1715_min20_max100_60lessnoisy.mat');
msckf100 = load('msckf_1215_1715_min20_max100_100lessnoisy.mat');

imu = load('imu_1215_1715.mat');

load('../datasets/dataset3.mat');
kStart = 1215;
kEnd = 1715;
plotAbsErr = true;
transLim = [0 1];
rotLim = [0 1];
fontSize = 14;

if plotAbsErr
    imu.msckf_trans_err = abs(imu.msckf_trans_err);
    msckf20.msckf_trans_err = abs(msckf20.msckf_trans_err);
    msckf100.msckf_trans_err = abs(msckf100.msckf_trans_err);
    msckf60.msckf_trans_err = abs(msckf60.msckf_trans_err);
%     swf20.swf_trans_err = abs(swf20.swf_trans_err);
%     swf40.swf_trans_err = abs(swf40.swf_trans_err);
%     swf60.swf_trans_err = abs(swf60.swf_trans_err);

    imu.msckf_rot_err = abs(imu.msckf_rot_err);
    msckf20.msckf_rot_err = abs(msckf20.msckf_rot_err);
    msckf100.msckf_rot_err = abs(msckf100.msckf_rot_err);
    msckf60.msckf_rot_err = abs(msckf60.msckf_rot_err);
%     swf20.swf_rot_err = abs(swf20.swf_rot_err);
%     swf40.swf_rot_err = abs(swf40.swf_rot_err);
%     swf60.swf_rot_err = abs(swf60.swf_rot_err);

    transLim = transLim - transLim(1);
    rotLim = rotLim - rotLim(1);
end

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_trans_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_trans_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_trans_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_trans_err(1,:), '-.b', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 20', 'MSCKF 60', 'MSCKF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);

title(sprintf('Feature Density Comparison: Absolute Translational Error'))
ylabel('\delta r_x [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_trans_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_trans_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_trans_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_trans_err(2,:), '-.b', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_y [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_trans_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_trans_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_trans_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_trans_err(3,:), '-.b', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
ylabel('\delta r_z [m]')
xlabel('t_k [s]')
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)
grid on
grid minor
box on

filename = sprintf('6-Way-Comparison-Feat-Density-1215-1715-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
subplot(3,1,1)
plot(t(kStart:kEnd), imu.msckf_rot_err(1,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_rot_err(1,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_rot_err(1,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_rot_err(1,:), '-.b', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
title(sprintf('Feature Density Comparison: Absolute Rotational Error'))
ylabel('\delta\theta_x')
h_legend = legend('IMU Only','MSCKF 20', 'MSCKF 60', 'MSCKF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,2)
plot(t(kStart:kEnd), imu.msckf_rot_err(2,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_rot_err(2,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_rot_err(2,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_rot_err(2,:), '-.b', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd)]);
ylim(rotLim)
ylabel('\delta\theta_y')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on

subplot(3,1,3)
plot(t(kStart:kEnd), imu.msckf_rot_err(3,:), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf20.msckf_rot_err(3,:), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf60.msckf_rot_err(3,:), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf100.msckf_rot_err(3,:), '-.b', 'LineWidth', 1.2)
xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
ylabel('\delta\theta_z')
xlabel('t_k [s]')
grid on
grid minor
box on

set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('6-Way-Comparison-Feat-Density-1215-1715-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% RMSE

msckf20 = load('msckf_1215_1715_min20_max100_20lessnoisy.mat');
msckf40 = load('msckf_1215_1715_min20_max100_40lessnoisy.mat');
msckf60 = load('msckf_1215_1715_min20_max100_60lessnoisy.mat');
msckf100 = load('msckf_1215_1715_min20_max100_100lessnoisy.mat');
swf40 = load('swf_1215_1715_25_40lessnoisy.mat');
swf60 = load('swf_1215_1715_25_60lessnoisy.mat');
swf100 = load('swf_1215_1715_25_100lessnoisy.mat');

imu = load('imu_1215_1715.mat');

load('../datasets/dataset3.mat');
kStart = 1215;
kEnd = 1715;
plotAbsErr = true;
transLim = [0 0.7];
rotLim = [0 0.5];
fontSize = 14;

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), sqrt(sum(imu.msckf_trans_err.^2, 1)/3), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), sqrt(sum(msckf40.msckf_trans_err.^2, 1)/3), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(msckf60.msckf_trans_err.^2, 1)/3), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(msckf100.msckf_trans_err.^2, 1)/3), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf40.swf_trans_err.^2, 1)/3), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf60.swf_trans_err.^2, 1)/3), '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf100.swf_trans_err.^2, 1)/3), '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);
title(sprintf('Feature Density Comparison: Position RMSE'))
xlabel('t_k [s]');
ylabel('RMSE [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
filename = sprintf('RMSE-Comparison-Feat-Density-1215-1715-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), sqrt(sum(imu.msckf_rot_err.^2, 1)/3), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), sqrt(sum(msckf40.msckf_rot_err.^2, 1)/3), '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(msckf60.msckf_rot_err.^2, 1)/3), '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(msckf100.msckf_rot_err.^2, 1)/3), '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf40.swf_rot_err.^2, 1)/3), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf60.swf_rot_err.^2, 1)/3), '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf100.swf_rot_err.^2, 1)/3), '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim([0 0.25])
title(sprintf('Feature Density Comparison: Rotation RMSE'))
ylabel('RMSE')
xlabel('t_k [s]');
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('RMSE-Comparison-Feat-Density-1215-1715-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% RMSE Noisy

swf40 = load('swf_1215_1715_10_40noisy.mat');
swf60 = load('swf_1215_1715_10_60noisy.mat');
swf100 = load('swf_1215_1715_10_100noisy.mat');
imu = load('imu_1215_1715.mat');

load('../datasets/dataset3.mat');
kStart = 1215;
kEnd = 1715;
plotAbsErr = true;
transLim = [0 0.7];
rotLim = [0 0.5];
fontSize = 14;

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), sqrt(sum(imu.msckf_trans_err.^2, 1)/3), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), sqrt(sum(swf40.swf_trans_err.^2, 1)/3), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf60.swf_trans_err.^2, 1)/3), '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf100.swf_trans_err.^2, 1)/3), '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);
title(sprintf('Feature Density Comparison: Position RMSE'))
xlabel('t_k [s]');
ylabel('RMSE [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
filename = sprintf('RMSE-Comparison-Feat-Density-1215-1715-Noisy-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), sqrt(sum(imu.msckf_rot_err.^2, 1)/3), '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), sqrt(sum(swf40.swf_rot_err.^2, 1)/3), '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf60.swf_rot_err.^2, 1)/3), '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), sqrt(sum(swf100.swf_rot_err.^2, 1)/3), '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim([0 0.25])
title(sprintf('Feature Density Comparison: Rotation RMSE'))
ylabel('RMSE')
xlabel('t_k [s]');
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',10);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('RMSE-Comparison-Feat-Density-1215-1715-Noisy-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% RMSE 500-1000 or 1215_1715
% Run relevent calculcateRMSE_1215_1715.m file first

load('../datasets/dataset3.mat');
kStart = 1215;
kEnd = 1715;
plotAbsErr = true;
transLim = [0 0.7];
rotLim = [0 0.5];
fontSize = 8;

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), imu_trans, '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5_inf_trans, '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10_50_trans, '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20_100_trans, '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10_trans, '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50_trans, '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100_trans, '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-20', 'MSCKF 20-100', 'SWF 10','SWF 50','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',fontSize);
title(sprintf('Translation RMSE (Interval 2)'))
xlabel('t_k [s]');
ylabel('RMSE [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
filename = sprintf('RMSE-Comparison-1215-1715-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), imu_rot, '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5_inf_rot, '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10_50_rot, '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20_100_rot, '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10_rot, '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50_rot, '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100_rot, '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
title(sprintf('Rotation RMSE (Interval 2)'))
ylabel('RMSE')
xlabel('t_k [s]');
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',fontSize);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('RMSE-Comparison-1215-1715-Rot.pdf');
export_fig(gcf, filename, '-transparent');

%% RMSE 500-1000 or 1215_1715
% Run calculcateRMSE_500_1000.m file first

load('../datasets/dataset3.mat');
kStart = 500;
kEnd = 1000;
plotAbsErr = true;
transLim = [0 0.7];
rotLim = [0 0.5];
fontSize = 8;

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), imu_trans, '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5_inf_trans, '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10_50_trans, '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20_100_trans, '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10_trans, '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50_trans, '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100_trans, '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(transLim)
h_legend = legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-20', 'MSCKF 20-100', 'SWF 10','SWF 50','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',fontSize);
title(sprintf('Translation RMSE (Interval 1)'))
xlabel('t_k [s]');
ylabel('RMSE [m]')
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
filename = sprintf('RMSE-Comparison-500-1000-Trans.pdf');
export_fig(gcf, filename, '-transparent');

figure
set(gcf, 'Position', [100 100 600 200]);
plot(t(kStart:kEnd), imu_rot, '-k', 'LineWidth', 1.2)
hold on
plot(t(kStart:kEnd), msckf5_inf_rot, '-b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf10_50_rot, '--b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), msckf20_100_rot, '-.b', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf10_rot, '-g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf50_rot, '--g', 'LineWidth', 1.2)
plot(t(kStart:kEnd), swf100_rot, '-.g', 'LineWidth', 1.2)

xlim([t(kStart) t(kEnd) ]);
ylim(rotLim)
title(sprintf('Rotation RMSE (Interval 1)'))
ylabel('RMSE')
xlabel('t_k [s]');
h_legend = legend('IMU Only','MSCKF 40', 'MSCKF 60', 'MSCKF 100', 'SWF 40','SWF 60','SWF 100', 'Location', 'northwest');
set(h_legend,'FontSize',fontSize);
set(gca,'FontSize',fontSize)
grid on
grid minor
box on
set(gca,'FontSize',fontSize)
set(findall(gcf,'type','text'),'FontSize',fontSize)

filename = sprintf('RMSE-Comparison-500-1000-Rot.pdf');
export_fig(gcf, filename, '-transparent');


function munu_m_i(M,ML,MU,N)
%
%  Generates of the data used in 'munu_m'. Data are stored in global
%  variables.
%
%  ML, MU are generated by the routine 'munu_pre'
%
%  Calling sequence:
%
%    munu_m_i(M,ML,MU,N)
%
%  Input:
%
%    M         real matrix M
%    ML, MU    real LU factors,
%    N         real matrix N.
%
%  After calling this routine ML and MU can be deleted to save memory.
%
%
%  LYAPACK 1.0 (Thilo Penzl, September 1999)

if nargin~=4
  error('Wrong number of input arguments.');
end

global LP_M LP_ML LP_MU LP_N

LP_ML = ML;
LP_MU = MU;
LP_M = M;
LP_N = N;


function Prepare_data_Multi_PIE_all()

    % This bit collects all of the multi-pie labels into a single structure for
    % easy access
    labels_root = 'C:\Users\tbaltrus\Dropbox\AAM\test data\MultiPI_AAM/';

    % The location of the Multi-PIE data folder
    multi_pie_root = 'D:\MultiPIE/Image_Data/';

    multi_pie_labels = CollectMultiPieLabels(labels_root, multi_pie_root);

    %%

    % Make sure same images are used across scales
    rng(0);
    ExtractTrainingMultiPIE(0.25, multi_pie_labels);

    rng(0);
    ExtractTrainingMultiPIE(0.35, multi_pie_labels);

    rng(0);
    ExtractTrainingMultiPIE(0.5, multi_pie_labels);

    rng(0);
    ExtractTrainingMultiPIE(1.0, multi_pie_labels);

end
% Now extract the relevant information

function [multi_pie_labels] = CollectMultiPieLabels(labels_root, multi_pie_root)

    multi_pie_labels = struct;
    currentLabel = 0;
    left_to_frontal_map = [1, 28;
                        2, 29;
                        3, 30;
                        4, 31;
                        5, 34;
                        6, 36;
                        7, 27;
                        8, 26;
                        9, 25;
                        10,24;
                        11,46;
                        12,45;
                        13,44;
                        14,48;
                        15,47;
                        16,52;
                        17,53;
                        18,54;
                        19,55;
                        20,56;
                        21,57;
                        22,58;
                        23,63;
                        24,64;
                        26,66;
                        27,67;
                        30,9;
                        31,10;
                        32,11;
                        33,12;
                        34,13;
                        35,14;
                        36,15;
                        37,16;
                        38,17;];

    right_to_frontal_map = [1, 28;
                            2, 29;
                            3, 30;
                            4, 31;
                            5, 34;
                            6, 32;
                            7 ,18;
                            8 ,19;
                            9 ,20;
                            10,21;
                            11,37;
                            12,38;
                            13,39;
                            14,41;
                            15,42;
                            16,52;
                            17,51;
                            18,50;
                            19,49;
                            20,60;
                            21,59;
                            22,58;
                            23,63;
                            24,62;
                            26,68;
                            27,67;
                            30, 9;
                            31, 8;
                            32, 7;
                            33, 6;
                            34, 5;
                            35, 4;
                            36, 3;
                            37, 2;
                            38, 1];

    for i=1:4

        labelsDir = sprintf('%sSession%d/', labels_root, i);

        labels = dir([labelsDir '/*.mat']);

        double_label = false;

        for j = 1:numel(labels)
            if(double_label)
                double_label = false;
                continue;
            end

            load([labelsDir labels(j).name]);

            if(size(pts,1) ~= 68 && size(pts,1) ~= 39)
                continue;
            end

            userID = labels(j).name(1:3);
            recID = labels(j).name(8:9);
            camID = labels(j).name(11:12);
            camID2 = labels(j).name(13);
            viewID = labels(j).name(15:16);

            % doubling labels seem to be odd/incorrect
            if(j < numel(labels))

                userIDN = labels(j+1).name(1:3);
                recIDN = labels(j+1).name(8:9);
                camIDN = labels(j+1).name(11:12);
                camID2N = labels(j+1).name(13);

                if(strcmp(userIDN, userID) && strcmp(recID, recIDN) && strcmp(camIDN, camID) && strcmp(camID2N, camID2))
                    double_label = true;
                    continue;
                end

            end

            % camera id 08 is from a very different perspective
            if(strcmp(camID, '08') && strcmp(camID2, '1'))
                continue;
            end

            currentLabel = currentLabel + 1;

            multi_pie_labels(currentLabel).label_file = [labelsDir labels(j).name];
            multi_pie_labels(currentLabel).user_ID = userID;
            multi_pie_labels(currentLabel).rec_ID = recID;
            multi_pie_labels(currentLabel).cam_ID = camID;
            multi_pie_labels(currentLabel).cam_ID2 = camID2;

            multiPieImageDir = sprintf('%s/session0%d/multiview/%s/%s/%s_%s/', multi_pie_root, i,userID,recID,camID,camID2);
            multiPieImgs = dir([multiPieImageDir '*.png']);

            multi_pie_labels(currentLabel).img_dir = multiPieImageDir;
    %         multi_pie_labels(currentLabel).img_loc = [labels(j+1).name(1:end-7), '.png'];
            actualLabel = dir([multiPieImageDir '/*' viewID '.png']);
            multi_pie_labels(currentLabel).img_locs = {multiPieImgs.name};
            multi_pie_labels(currentLabel).actual_img = actualLabel.name;
            landmark_labels = zeros(68,2);

            % This is a profile image
            if(size(pts,1) == 39)
                % Determine if left or right

                if(pts(4,1) < pts(39,1))
                    multi_pie_labels(currentLabel).type = 'profile_left';
                    landmark_labels(left_to_frontal_map(:,2),:) = pts(left_to_frontal_map(:,1),:);
                else
                    multi_pie_labels(currentLabel).type = 'profile_right';
                    landmark_labels(right_to_frontal_map(:,2),:) = pts(right_to_frontal_map(:,1),:);
                end
            else
                landmark_labels = pts;
            end

            multi_pie_labels(currentLabel).landmark_labels = landmark_labels;

        end
    end
end

function ExtractTrainingMultiPIE( training_scale, multi_pie_labels)
%PREPARETRAININGIMAGEMPIE This function collects a subset of Multi-PIE
%images at different expressions and lighting conditions to store it in a
%format suitable for training patch experts

%   Detailed explanation goes here

    img_size = [400, 400] * training_scale;

    %%
    addpath('PDM_helpers/');
    load 'PDM_helpers/pdm_68_aligned_wild.mat';

    output_location = '../prepared_data/mpie_';
    output_location = [output_location num2str(training_scale,3)];

    num_landmarks = 68;

    % Use mirror images to provide extra training data
    mirror_inds = [1,17;2,16;3,15;4,14;5,13;6,12;7,11;8,10;18,27;19,26;20,25;21,24;22,23;...
                  32,36;33,35;37,46;38,45;39,44;40,43;41,48;42,47;49,55;50,54;51,53;60,56;59,57;...
                  61,65;62,64;68,66];

    % The centres of views we want to extract
    centres_all = [   0,   0,  0
                      0, -20,  0
                      0, -45,  0
                      0, -70,  0
                      0,  20,  0
                      0,  45,  0
                      0,  70,  0
                      ];

    num_centers = size(centres_all, 1);

    counter_colour = zeros(num_centers,1);

    % read in all of the labels, together with names of images used

    landmark_labels = zeros(68,2,numel(multi_pie_labels));
    img_locations = cell(numel(multi_pie_labels),1);
    extra_locations = cell(numel(multi_pie_labels),1);
    img_dirs = cell(numel(multi_pie_labels),1);

    for i=1:numel(multi_pie_labels)

        img_locations{i} = [multi_pie_labels(i).img_dir, multi_pie_labels(i).actual_img];
        extra_locations{i} =  multi_pie_labels(i).img_locs;
        img_dirs{i} =  multi_pie_labels(i).img_dir;

        landmark_labels(:,:,i) = multi_pie_labels(i).landmark_labels;

    end

    % go through all images, see which centres they match and then add to
    % the appropriate bin
    num_imgs = size(landmark_labels,3);
    scales = zeros(num_imgs, 1);
    views = zeros(num_imgs, 1);
    for lbl=1:num_imgs

        Msm = M;
        Vsm = V;
        labels = landmark_labels(:,:,lbl);

        % Find the best PDM parameters given the 2D labels
        [ a,R ] = fit_PDM_ortho_proj_to_2D( Msm, E, Vsm, labels);

        eul = Rot2Euler(R);
        eul_orig = eul * 180 / pi;

        if(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '24_0'))
            eul = [0, -90, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '01_0'))
            eul = [0, -75, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '20_0'))
            eul = [0, -60, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '19_0'))
            eul = [0, -45, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '04_1'))
            eul = [0, -30, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '05_0'))
            eul = [0, -15, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '05_1'))
            eul = [0, 0, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '14_0'))
            eul = [0, 15, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '13_0'))
            eul = [0, 30, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '08_0'))
            eul = [0, 45, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '09_0'))
            eul = [0, 60, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '12_0'))
            eul = [0, 75, 0];
        elseif(strcmp([multi_pie_labels(lbl).cam_ID,'_', multi_pie_labels(lbl).cam_ID2], '11_0'))
            eul = [0, 90, 0];
        end

        % find the closest view (also mirror img?)
        [~, view] = min(sum(abs(centres_all - repmat(eul, num_centers, 1)),2));
        counter_colour(view) = counter_colour(view) + 1;
        scales(lbl) = a;
        views(lbl) = view;
    end

    % preallocate data
    allExamplesColourAllViews = cell(size(centres_all,1),1);
    landmarkLocationsAllViews = cell(size(centres_all,1),1);

    % if we don't have enough labelled original images add ones from diff
    % lighting conditions (that might not be perfectly labelled, but they
    % are better than fewer images)
    images_aim = 8000;
    extra_factors = ones(size(counter_colour));

    for r=1:size(centres_all,1)

         % see if extra is needed
        mirrorIdx = find(sum(abs(centres_all - repmat([centres_all(r,1), -centres_all(r,2), -centres_all(r,3)], size(centres_all,1),1)),2)==0);
        count = counter_colour(r) + counter_colour(mirrorIdx);

        if(count < images_aim)
            extra_factors(r) = images_aim / count;
        end

    end

    for r=1:size(centres_all,1)
        counter_colour(r) = round(counter_colour(r) * extra_factors(r));

        allExamplesColourAllViews{r} = uint8(zeros(counter_colour(r), img_size(1), img_size(2)));
        landmarkLocationsAllViews{r} = zeros(counter_colour(r), num_landmarks, 2);

        actual_imgs_used_all_views{r} = cell(counter_colour(r), 1);

    end

    counter_colour = zeros(num_centers,1);

    % The shape fitting is performed in the reference frame of the
    % patch training scale
    refGlobal = [training_scale, 0, 0, 0, 0, 0]';

    % go through all images and add to corresponding container
    for lbl=1:num_imgs

        labels = landmark_labels(:,:,lbl);
        occluded = labels(:,1) == 0;

        % Convert the labels to matlab format (we expect 1,1 to represent
        % the center of the top left pixel)
        labels = labels + 1;

        labels(occluded,:) = 0;

        imgCol = imread(img_locations{lbl});

        if(size(imgCol,3) == 3)
            imgCol = rgb2gray(imgCol);
        end

        % the reference shape
        [ ~, ~, ~,~, local_params] = fit_PDM_ortho_proj_to_2D( Msm, E, Vsm, labels);
        refShape = GetShapeOrtho(M, Vsm, local_params, refGlobal);

        % Create transform using a slightly modified version of Kabsch that
        % takes scaling into account as well, in essence we get a
        % similarity transform from current estimate to reference shape
        [A_img2ref, T_img2ref, ~, ~] = AlignShapesWithScale(labels(~occluded,:), refShape(~occluded,1:2));

        T_img2ref = T_img2ref + [img_size(1)/2, img_size(2)/2];

        % Create a transform, from shape in image to reference shape
        T = affine2d([A_img2ref';T_img2ref]);

        % transform the current shape to the reference one
        shape2D_in_ref = bsxfun(@plus, (A_img2ref * labels')', T_img2ref);

        % warp the image
        resizeColImage = imwarp(imgCol, T, 'linear', 'OutputView', imref2d(img_size));

        shape2D_in_ref(occluded,1) = 0;
        shape2D_in_ref(occluded,2) = 0;

        counter_colour(views(lbl)) = counter_colour(views(lbl)) + 1;

        allExamplesColourAllViews{views(lbl)}(counter_colour(views(lbl)),:,:) = resizeColImage;

        landmarkLocationsAllViews{views(lbl)}(counter_colour(views(lbl)),:,:) = shape2D_in_ref;
        actual_imgs_used_all_views{views(lbl)}{counter_colour(views(lbl))} = img_locations{lbl};

        % Here to add extra images if missing and not filled yet
        if(extra_factors(views(lbl)) > 1 && counter_colour(views(lbl)) < numel(actual_imgs_used_all_views{views(lbl)}))

            factor = extra_factors(views(lbl));

            % pick if to use this image or not
            if(randi(100) > (factor - floor(factor)) * 100)
                factor = floor(factor);
            else
                factor = ceil(factor);
            end

            while(factor > 1)

                lighting_id =  randi(20);
                [~, img_orig, ext] = fileparts(img_locations{lbl});
                img_orig = [img_orig, ext];
                if( strcmp(img_orig, extra_locations{lbl}{lighting_id}))
                    lighting_id = lighting_id + 1;
                    if(lighting_id > 20)
                        lighting_id = 1;
                    end
                end
                img_loc = [img_dirs{lbl}, extra_locations{lbl}{lighting_id}];
                imgCol = imread(img_loc);

                if(size(imgCol,3) == 3)
                    imgCol = rgb2gray(imgCol);
                end

                % resize the image to desired scale
                resizeColImage = imwarp(imgCol, T, 'linear', 'OutputView', imref2d(img_size));

                counter_colour(views(lbl)) = counter_colour(views(lbl)) + 1;

                allExamplesColourAllViews{views(lbl)}(counter_colour(views(lbl)),:,:) = resizeColImage;

                landmarkLocationsAllViews{views(lbl)}(counter_colour(views(lbl)),:,:) = shape2D_in_ref;
                actual_imgs_used_all_views{views(lbl)}{counter_colour(views(lbl))} = img_loc;
                factor = factor - 1;
            end

        end
        % Make sure same one not added as well

        if(mod(lbl, 100) == 0)
            fprintf('%d/%d done\n', lbl, num_imgs);
        end

    end

    % write out the training data for each view
    for r=1:size(centres_all,1)
        mirrorIdx = find(sum(abs(centres_all - repmat([centres_all(r,1), -centres_all(r,2), -centres_all(r,3)], size(centres_all,1),1)),2)==0);

        % if the mirrored view already added no need to do it again
        % so if (0,-20,0) is done no need to compute (0,20,0)
        if(mirrorIdx < r)
            continue
        end

        % Cap to two thousand images for space reasons, and because more
        % wouldn't actually be used
        max_images = 2000;

        mirrorImgs = allExamplesColourAllViews{mirrorIdx}(1:counter_colour(mirrorIdx),:,:);
        mirrorLbls = landmarkLocationsAllViews{mirrorIdx}(1:counter_colour(mirrorIdx),:,:);

        for i=1:size(mirrorImgs,1)

            flippedImg = fliplr(squeeze(mirrorImgs(i,:,:)));

            flippedLbls = squeeze(mirrorLbls(i,:,:));
            flippedLbls(:,1) = img_size(1) - flippedLbls(:,1) + 1;

            tmp1 = flippedLbls(mirror_inds(:,1),:);
            tmp2 = flippedLbls(mirror_inds(:,2),:);
            flippedLbls(mirror_inds(:,2),:) = tmp1;
            flippedLbls(mirror_inds(:,1),:) = tmp2;

            mirrorImgs(i,:,:) = flippedImg;
            mirrorLbls(i,:,:) = flippedLbls;

            % Ensure that after mirroring invalid (occluded) feature points
            % are set to 0 in both x and y dims
            mirrorLbls(i,flippedLbls(:,2)==0,1) = 0;

        end

        all_images = cat(1, allExamplesColourAllViews{r}(1:counter_colour(r),:,:), mirrorImgs);
        landmark_locations = cat(1, landmarkLocationsAllViews{r}(1:counter_colour(r),:,:), mirrorLbls);
        actual_imgs_used = cat(1, actual_imgs_used_all_views{r}(1:counter_colour(r)), actual_imgs_used_all_views{mirrorIdx}(1:counter_colour(mirrorIdx)));

        centres = centres_all(r,:);

        % identify the visibility of a point
        num_visible = sum(landmark_locations(:,:,1)~=0);
        visible_max = max(num_visible);

        visiIndex = ones(1,68);
        visiIndex(num_visible < 0.5*visible_max) = 0;

        if(size(all_images,1) > max_images)
            im_to_select = randperm(size(all_images,1));
            im_to_select = im_to_select(1:max_images);
            all_images = all_images(im_to_select,:,:);
            landmark_locations = landmark_locations(im_to_select,:,:);
            actual_imgs_used = actual_imgs_used(im_to_select);
        end
        save([output_location '_' num2str(r) '.mat'], 'all_images', 'landmark_locations', 'training_scale', 'centres', 'actual_imgs_used', 'visiIndex', '-v7.3');

    end

end


function [Overlap, NumInsts] = GetOverlap(Proposals, GTInsts)
GTInstsSize = size(GTInsts);
if iscell(GTInsts)
    InstFun = @(Index)CellFun(GTInsts,Index);
    NumInsts = max(GTInstsSize);
else
    if length(GTInstsSize) == 2
        NumInsts = 1;
    else
        NumInsts = GTInstsSize(3);
    end
    InstFun = @(Index)ThreeDMatFun(GTInsts,Index);

end
NumProposals = size(Proposals, 3);

Overlap = zeros(NumInsts, NumProposals);
for i = 1:NumProposals
    Proposal = Proposals(:,:,i);
    for j = 1:NumInsts
        GTInst = InstFun(j);
        Overlap(j,i) = sum(Proposal(:) & GTInst(:)) / sum(Proposal(:) | GTInst(:));
    end
end
end

function InstMask = CellFun(InstList, Index)
InstMask = InstList{Index};
end
function InstMask = ThreeDMatFun(InstList, Index)
InstMask = InstList(:,:,Index);
end
function InstMask = OneDMatFun(InstList, Index)
InstMask = InstList == Index;
end


function[varargout]=printall(varargin)
%PRINTALL  Print and close all open figures.
%
%   PRINTALL prints all open figures to files in the current directory,
%   and closes the figures.
%
%   Files are named with the time stamp YYYY-MM-DD+HH|MM|SS followed
%   by their window number.
%
%   For image file output (png, jpeg, or tiff), the files are also cropped
%   using CROP by A. Bliss.
%
%   PRINTALL(NAME) prepends the string NAME to the file names.
%
%   PRINTALL(NAME,STR) passes the string STR as the printer device, for
%   example, PRINTALL('-djpg -r200').  The default choice is STR='-dpng'.
%
%   PRINTALL(H,...) only prints those figures whose handles are given in H.
%
%   Usage: printall samplefigures
%          printall samplefigures -depsc
%          printall([],' -depsc')
%          printall(h,'-djpeg')
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2013--2020 J.M. Lilly --- type 'help jlab_license' for details

str='-dpng';
name=[];
h=[];
if ~isempty(varargin)
    if ~ischar(varargin{1})
        h=varargin{1};
        varargin=varargin(2:end);
    end
end
if ~isempty(varargin)
    name=varargin{1};
    varargin=varargin(2:end);
end
if ~isempty(varargin)
    str=varargin{1};
    varargin=varargin(2:end);
end

if ~isempty(name)
    name=[name '@'];
end

ext=[];
if contains(str,'jpeg')
    ext='.jpg';
elseif contains(str,'png')
    ext='.png';
elseif contains(str,'tiff')
    ext='.tif';
end

if isempty(h)
    a=findobj('Type','Figure');
    for i=length(a):-1:1
        figure(a(i).Number)
        filename= datestr(now,31);filename(11)='+';filename(14:3:end)='|';
        namestr=[name filename '-' int2str(a(i).Number)];
        eval(['print ' str ' ' namestr])
        if ~isempty(ext)
            eval(['crop ' namestr ext])
        end
        close(a(i).Number);
    end
else
    for i=1:length(h)
        figure(h(i))
        filename= datestr(now,31);filename(11)='+';filename(14:3:end)='|';
        namestr=[name filename '-' int2str(h(i))];
        eval(['print ' str ' ' namestr])
        if ~isempty(ext)
            eval(['crop ' namestr ext])
        end
        close(h(i));
    end
end
%close all


function status = besa_save2Swf(file_path, file_name, data_matrix, ...
    time_samples, source_labels, data_scale_factor, time_scale_factor)

% BESA_SAVE2SWF writes a data matrix into ASCII-source waveform file format
%
% Parameters:
%     [file_path]
%         Full path to the folder where the file should be saved.
%
%     [file_name]
%         The name of the file where the output should be written.
%
%     [data_matrix]
%         A 2D data matrix to be saved. It should have the size
%         [NumberOfChannels x NumberOfTimeSamples].
%
%     [time_samples]
%         An array with the time samples corresponding to the data. The
%         size of the array should be [1 x NumberOfTimeSamples].
%
%     [source_labels]
%         An array containing the channel labels for the current data.
%
%     [data_scale_factor]
%         A constant used for converting the data values from Tesla into fT
%         (10e15) or from Volt into uV (10e6). If the data is already in fT
%         or uV the just set it to 1.
%
%     [time_scale_factor]
%         A constant used for converting the time samples from seconds into
%         milliseconds (1000). If the time samples are already in
%         milliseconds just set the constant to 1.
%
% Return:
%     [status]
%         The status of the writing process. Still unused.
%

% Copyright (C) 2013, BESA GmbH
%
% File name: besa_save2Swf.m
%
% This file is part of MATLAB2BESA.
%
%    MATLAB2BESA is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    MATLAB2BESA is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with MATLAB2BESA. If not, see <http://www.gnu.org/licenses/>.
%
% Author: Todor Jordanov
% Created: 2012-08-27

n_sources = size(source_labels, 2); %size(data_matrix, 1);

% Check if data is vectorized or multiplex.
isMul = 1;  % boolean variable containing the info if the file is
            %vectorized or multiplexed.
tmp1 = size(data_matrix);
if(tmp1(1) == n_sources)

    n_tbins = tmp1(2);
    isMul = 0;

else

    n_tbins = tmp1(1);

end

% Scale the time.
start_time = time_scale_factor * time_samples(1);
time_step = time_scale_factor * (time_samples(2) - time_samples(1));

% Scale the data.
data_matrix = data_matrix .* data_scale_factor;

% Change to the directory where the data should be saved.
cd(file_path)

% Open file for writing.
fid = fopen(file_name, 'w');

% Write the first line of the header.
fprintf(fid, ...
    'Npts= %d   TSB= %f DI= %f SB= 1.000 SC= 200.0 Nchan= %d\n', ...
    n_tbins, start_time, time_step, n_sources);

% Write the data matrix to the file.
for i=1:n_sources

    fprintf(fid, '%s: ', source_labels{i});

    for j=1:n_tbins

        if(isMul == 0)

            fprintf(fid, '%f ', data_matrix(i, j));

        else

            fprintf(fid, '%f ', data_matrix(j, i));

        end

    end

    fprintf(fid, '\n');

end

fclose(fid);

status = 1;


% GLG
%
% Files
%   cell_to_dwt2 - Divide wavedec2 output to cell
%   dwt2_to_cell - Divide wavedec2 output to cell
%   dwt_to_cell  - Divide wavedec output to cell
%   psnr         - Compute PSNR between images
%   setup        - Add the relevant sub directories


%NOFRICTION	return link object with zero friction
%
%	LINK = NOFRICTION(LINK)
%
%

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function  l2 = nofriction(l, only)
%Link.nofriction Remove friction
%
% LN = L.nofriction() is a link object with the same parameters as L except
% nonlinear (Coulomb) friction parameter is zero.
%
% LN = L.nofriction('all') is a link object with the same parameters as L
% except all friction parameters are zero.

	l2 = Link(l);
	if (nargin == 2) && strcmpi(only(1:3), 'all')
		l2.B = 0;
	end
	l2.Tc = [0 0];
end