function [x,fval,exitflag,output,grad]=fminlbfgs(funfcn,x_init,optim)
%FMINLBFGS finds a local minimum of a function of several variables.
%   This optimizer is developed for image registration methods with large
%	amounts of unknown variables.
%
%   Optimization methods supported:
%	- Quasi Newton Broyden-Fletcher-Goldfarb-Shanno (BFGS)
%   - Limited memory BFGS (L-BFGS)
%   - Steepest Gradient Descent optimization.
%
%   [X,FVAL,EXITFLAG,OUTPUT,GRAD] = FMINLBFGS(FUN,X0,OPTIONS)
%
%   Inputs,
%		FUN: Function handle or string which is minimized, returning an
%				error value and optional the error gradient.
%		X0: Initial values of unknowns can be a scalar, vector or matrix
%	 (optional)
%		OPTIONS: Structure with optimizer options, made by a struct or
%				optimset. (optimset doesnot support all input options)
%
%   Outputs,
%		X : The found location (values) which minimize the function.
%		FVAL : The minimum found
%		EXITFLAG : Gives value, which explain why the minimizer stopt
%		OUTPUT : Structure with all important ouput values and parameters
%		GRAD : The gradient at this location
%
%   Extended description of input/ouput variables
%   OPTIONS,
%		OPTIONS.GoalsExactAchieve : If set to 0, a line search method is
%               used which uses a few function calls to do a good line
%               search. When set to 1 a normal line search method with Wolfe
%				conditions is used (default).
%		OPTIONS.GradConstr, Set this variable to true if gradient calls are
%				cpu-expensive (default). If false more gradient calls are
%				used and less function calls.
%	    OPTIONS.HessUpdate : If set to 'bfgs', Broyden�Fletcher�Goldfarb�Shanno
%				optimization is used (default), when the number of unknowns is
%				larger then 3000 the function will switch to Limited memory BFGS,
%				or if you set it to 'lbfgs'. When set to 'steepdesc', steepest
%				decent optimization is used.
%		OPTIONS.StoreN : Number of itterations used to approximate the Hessian,
%			 	in L-BFGS, 20 is default. A lower value may work better with
%				non smooth functions, because than the Hessian is only valid for
%				a specific position. A higher value is recommend with quadratic equations.
%		OPTIONS.GradObj : Set to 'on' if gradient available otherwise finited difference
%				is used.
%     	OPTIONS.Display : Level of display. 'off' displays no output; 'plot' displays
%				all linesearch results in figures. 'iter' displays output at  each
%               iteration; 'final' displays just the final output; 'notify'
%				displays output only if the function does not converge;
%	    OPTIONS.TolX : Termination tolerance on x, default 1e-6.
%	    OPTIONS.TolFun : Termination tolerance on the function value, default 1e-6.
%		OPTIONS.MaxIter : Maximum number of iterations allowed, default 400.
% 		OPTIONS.MaxFunEvals : Maximum number of function evaluations allowed,
%				default 100 times the amount of unknowns.
%		OPTIONS.DiffMaxChange : Maximum stepsize used for finite difference gradients.
%		OPTIONS.DiffMinChange : Minimum stepsize used for finite difference gradients.
%		OPTIONS.OutputFcn : User-defined function that an optimization function calls
%				at each iteration.
%		OPTIONS.rho : Wolfe condition on gradient (c1 on wikipedia), default 0.01.
%		OPTIONS.sigma : Wolfe condition on gradient (c2 on wikipedia), default 0.9.
%		OPTIONS.tau1 : Bracket expansion if stepsize becomes larger, default 3.
%		OPTIONS.tau2 : Left bracket reduction used in section phase,
%		default 0.1.
%		OPTIONS.tau3 : Right bracket reduction used in section phase, default 0.5.
%   FUN,
%		The speed of this optimizer can be improved by also providing
%   	the gradient at X. Write the FUN function as follows
%   	function [f,g]=FUN(X)
%       	f , value calculation at X;
%   	if ( nargout > 1 )
%       	g , gradient calculation at X;
%   	end
%	EXITFLAG,
%		Possible values of EXITFLAG, and the corresponding exit conditions
%		are
%  		1, 'Change in the objective function value was less than the specified tolerance TolFun.';
%  		2, 'Change in x was smaller than the specified tolerance TolX.';
%  		3, 'Magnitude of gradient smaller than the specified tolerance';
%  		4, 'Boundary fminimum reached.';
%  		0, 'Number of iterations exceeded options.MaxIter or number of function evaluations exceeded options.FunEvals.';
%  		-1, 'Algorithm was terminated by the output function.';
%  		-2, 'Line search cannot find an acceptable point along the current search';
%
%   Examples
%       options = optimset('GradObj','on');
%       X = fminlbfgs(@myfun,2,options)
%
%   	% where myfun is a MATLAB function such as:
%       function [f,g] = myfun(x)
%       f = sin(x) + 3;
%	    if ( nargout > 1 ), g = cos(x); end
%
%   See also OPTIMSET, FMINSEARCH, FMINBND, FMINCON, FMINUNC, @, INLINE.
%
%   Function is written by D.Kroon University of Twente (March 2009)

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% See license file BSD_license.txt for license information.
% http://www.cis.hut.fi/projects/somtoolbox/

% Read Optimalisation Parameters
defaultopt = struct('Display','final','HessUpdate','bfgs','GoalsExactAchieve',1,'GradConstr',true,  ...
			'TolX',1e-6,'TolFun',1e-6,'GradObj','off','MaxIter',400,'MaxFunEvals',100*numel(x_init)-1,  ...
			'DiffMaxChange',1e-1,'DiffMinChange',1e-8,'OutputFcn',[], ...
			'rho',0.0100,'sigma',0.900,'tau1',3,'tau2', 0.1, 'tau3', 0.5,'StoreN',20);

if (~exist('optim','var'))
    optim=defaultopt;
else
    f = fieldnames(defaultopt);
    for i=1:length(f),
        if (~isfield(optim,f{i})||(isempty(optim.(f{i})))), optim.(f{i})=defaultopt.(f{i}); end
    end
end

% Initialize the data structure
data.fval=0;
data.gradient=0;
data.fOld=[];
data.xsizes=size(x_init);
data.numberOfVariables = numel(x_init);
data.xInitial = x_init(:);
data.alpha=1;
data.xOld=data.xInitial;
data.iteration=0;
data.funcCount=0;
data.gradCount=0;
data.exitflag=[];
data.nStored=0;
%%% data.timeTotal=tic;
data.timeExtern=0;
% Switch to L-BFGS in case of more than 3000 unknown variables
if(false && optim.HessUpdate(1)=='b')
    if(data.numberOfVariables<3000),
        optim.HessUpdate='bfgs';
    else
        optim.HessUpdate='lbfgs';
    end
end

if(optim.HessUpdate(1)=='l')
    data.deltaX=zeros(data.numberOfVariables,optim.StoreN);
    data.deltaG=zeros(data.numberOfVariables,optim.StoreN);
    data.saveD=zeros(data.numberOfVariables,optim.StoreN);
end

exitflag=[];

% Display column headers
if(strcmp(optim.Display,'iter'))
    disp('     Iteration  Func-count   Grad-count         f(x)         Step-size');
end

% Calculate the initial error and gradient
data.initialStepLength=1;
[data,fval,grad]=gradient_function(data.xInitial,funfcn, data, optim);
data.gradient=grad;
data.dir = -data.gradient;
data.gOld=grad;
data.fInitial = fval;
data.fPrimeInitial= data.gradient'*data.dir(:);

gNorm = norm(data.gradient,Inf);  % Norm of gradient
data.initialStepLength = min(1/gNorm,5);

% Show the current iteration
if(strcmp(optim.Display,'iter'))
        s=sprintf('     %5.0f       %5.0f       %5.0f       %13.6g    ',data.iteration,data.funcCount,data.gradCount,data.fInitial); disp(s);
end

% Hessian intialization
if(optim.HessUpdate(1)=='b')
	data.Hessian=eye(data.numberOfVariables);
end

% Call output function
if(call_output_function(data,optim,'init')), exitflag=-1; end

% Start Minimizing
while(true)
    % Update number of itterations
    data.iteration=data.iteration+1;

    % Set current lineSearch parameters
    data.TolFunLnS = eps(max(1,abs(data.fInitial )));
    data.fminimum = data.fInitial - 1e16*(1+abs(data.fInitial));

	% Make arrays to store linesearch results
    data.storefx=[]; data.storepx=[]; data.storex=[]; data.storegx=[];

    % If option display plot, than start new figure
    if(optim.Display(1)=='p'), figure, hold on; end

    % Find a good step size in the direction of the gradient: Linesearch
    if(optim.GoalsExactAchieve==1)
		data=linesearch(funfcn, data,optim);
    else
        data=linesearch_simple(funfcn, data, optim);
    end

	% Make linesearch plot
	if(optim.Display(1)=='p');
		plot(data.storex,data.storefx,'r*');
		plot(data.storex,data.storefx,'b');

		alpha_test= linspace(min(data.storex(:))/3, max(data.storex(:))*1.3, 10);
		falpha_test=zeros(1,length(alpha_test));
        for i=1:length(alpha_test)
			[data,falpha_test(i)]=gradient_function(data.xInitial(:)+alpha_test(i)*data.dir(:),funfcn, data, optim);
        end
		plot(alpha_test,falpha_test,'g');
        plot(data.alpha,data.f_alpha,'go','MarkerSize',8);
	end

    % Check if exitflag is set
    if(~isempty(data.exitflag)),
        exitflag=data.exitflag;
        data.xInitial=data.xOld;
        data.fInitial=data.fOld;
        data.gradient=data.gOld;
        break,
    end;

    % Update x with the alpha step
    data.xInitial = data.xInitial + data.alpha*data.dir;

    % Set the current error and gradient
    data.fInitial =  data.f_alpha;
	data.gradient = data.grad;

    % Set initial steplength to 1
    data.initialStepLength = 1;

    gNorm = norm(data.gradient,Inf);  % Norm of gradient

    % Set exit flags
    if(gNorm <optim.TolFun), exitflag=1; end
    if(max(abs(data.xOld-data.xInitial)) <optim.TolX), exitflag=2; end
    if(data.iteration>=optim.MaxIter), exitflag=0; end

    % Check if exitflag is set
    if(~isempty(exitflag)), break, end;

    % Update the inverse Hessian matrix
    if(optim.HessUpdate(1)~='s')
        % Do the Quasi-Neton Hessian update.
        data = updateQuasiNewtonMatrix_LBFGS(data,optim);
    else
        data.dir = -data.gradient;
    end

    % Derivative of direction
    data.fPrimeInitial= data.gradient'*data.dir(:);

    % Call output function
    if(call_output_function(data,optim,'iter')), exitflag=-1; end

    % Show the current iteration
    if(strcmp(optim.Display(1),'i')||strcmp(optim.Display(1),'p'))
        s=sprintf('     %5.0f       %5.0f       %5.0f       %13.6g   %13.6g',data.iteration,data.funcCount,data.gradCount,data.fInitial,data.alpha); disp(s);
    end

    % Keep the variables for next iteration
    data.fOld=data.fInitial;
    data.xOld=data.xInitial;
    data.gOld=data.gradient;
end
% Set output parameters
fval=data.fInitial;
grad=data.gradient;
x = data.xInitial;

% Reshape x to original shape
x=reshape(x,data.xsizes);

% Call output function
if(call_output_function(data,optim,'done')), exitflag=-1; end

% Make exist output structure
if(optim.HessUpdate(1)=='b'), output.algorithm='Broyden-Fletcher-Goldfarb-Shanno (BFGS)';
elseif(optim.HessUpdate(1)=='l'), output.algorithm='limited memory BFGS (L-BFGS)';
else output.algorithm='Steepest Gradient Descent';
end
output.message=getexitmessage(exitflag);
output.iteration = data.iteration;
output.funccount = data.funcCount;
output.fval = data.fInitial;
output.stepsize = data.alpha;
output.directionalderivative = data.fPrimeInitial;
output.gradient = reshape(data.gradient, data.xsizes);
output.searchdirection = data.dir;
%%% output.timeTotal=toc;
output.timeExtern=data.timeExtern;
%%%oupput.timeIntern=output.timeTotal-output.timeExtern;
% Display final results
if(~strcmp(optim.Display,'off'))
    disp('    Optimizer Results')
    disp(['        Algorithm Used: ' output.algorithm]);
    disp(['        Exit message : ' output.message]);
    disp(['        iterations : '  int2str(data.iteration)]);
    disp(['        Function Count : ' int2str(data.funcCount)]);
    disp(['        Minimum found : ' num2str(fval)]);
%%%    disp(['        Intern Time : ' num2str(oupput.timeIntern) ' seconds']);
%%%    disp(['        Total Time : ' num2str(output.timeTotal) ' seconds']);
end

function message=getexitmessage(exitflag)
    switch(exitflag)
        case 1, message='Change in the objective function value was less than the specified tolerance TolFun.';
        case 2, message='Change in x was smaller than the specified tolerance TolX.';
        case 3, message='Magnitude of gradient smaller than the specified tolerance';
        case 4, message='Boundary fminimum reached.';
        case 0, message='Number of iterations exceeded options.MaxIter or number of function evaluations exceeded options.FunEvals.';
        case -1, message='Algorithm was terminated by the output function.';
        case -2, message='Line search cannot find an acceptable point along the current search';
        otherwise, message='Undefined exit code';
    end

function stopt=call_output_function(data,optim,where)
stopt=false;
if(~isempty(optim.OutputFcn))
    output.iteration = data.iteration;
    output.funccount = data.funcCount;
    output.fval = data.fInitial;
    output.stepsize = data.alpha;
    output.directionalderivative = data.fPrimeInitial;
    output.gradient = reshape(data.gradient, data.xsizes);
    output.searchdirection = data.dir;
    stopt=feval(optim.OutputFcn,reshape(data.xInitial,data.xsizes),output,where);
end

function data=linesearch_simple(funfcn, data, optim)
% Find a bracket of acceptable points
data = bracketingPhase_simple(funfcn, data, optim);

if (data.bracket_exitflag  == 2)
  % BracketingPhase found a bracket containing acceptable points;
  % now find acceptable point within bracket
  data = sectioningPhase_simple(funfcn, data, optim);
  data.exitflag = data.section_exitflag;
else
  % Already acceptable point found or MaxFunEvals reached
  data.exitflag = data.bracket_exitflag;
end

function data = bracketingPhase_simple(funfcn, data,optim)
% Number of itterations
itw=0;

% Point with smaller value, initial
data.beta=0;
data.f_beta=data.fInitial;
data.fPrime_beta=data.fPrimeInitial;

% Initial step is equal to alpha of previous step.
alpha = data.initialStepLength;

% Going up hill
hill=false;

% Search for brackets
while(true)
    % Calculate the error registration gradient
    if(optim.GradConstr)
        [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
        fPrime_alpha=nan;
        grad=nan;
    else
        [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        fPrime_alpha = grad'*data.dir(:);
    end

	% Store values linesearch
	data.storefx=[data.storefx f_alpha];
    data.storepx=[data.storepx fPrime_alpha];
	data.storex=[data.storex alpha];
	data.storegx=[data.storegx grad(:)];

    % Update step value
    if(data.f_beta<f_alpha),
        % Go to smaller stepsize
        alpha=alpha*optim.tau3;

        % Set hill variable
        hill=true;
    else
        % Save current minium point
        data.beta=alpha; data.f_beta=f_alpha; data.fPrime_beta=fPrime_alpha; data.grad=grad;
        if(~hill)
            alpha=alpha*optim.tau1;
        end
    end

    % Update number of loop iterations
    itw=itw+1;

    if(itw>(log(optim.TolFun)/log(optim.tau3))),
      % No new optium found, linesearch failed.
      data.bracket_exitflag=-2; break;
    end

    if(data.beta>0&&hill)
            % Get the brackets around minimum point
            % Pick bracket A from stored trials
            [t,i]=sort(data.storex,'ascend');
            storefx=data.storefx(i);storepx=data.storepx(i); storex=data.storex(i);
            [t,i]=find(storex>data.beta,1);
            if(isempty(i)), [t,i]=find(storex==data.beta,1); end
            alpha=storex(i); f_alpha=storefx(i); fPrime_alpha=storepx(i);

            % Pick bracket B from stored trials
            [t,i]=sort(data.storex,'descend');
            storefx=data.storefx(i);storepx=data.storepx(i); storex=data.storex(i);
            [t,i]=find(storex<data.beta,1);
            if(isempty(i)), [t,i]=find(storex==data.beta,1); end
            beta=storex(i); f_beta=storefx(i); fPrime_beta=storepx(i);

            % Calculate derivatives if not already calculated
            if(optim.GradConstr)
                gstep=data.initialStepLength/1e6;
                if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
                if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
                [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
                [data,f_beta2]=gradient_function(data.xInitial(:)+(beta+gstep)*data.dir(:),funfcn, data, optim);
                fPrime_alpha=(f_alpha2-f_alpha)/gstep;
                fPrime_beta=(f_beta2-f_beta)/gstep;
            end

            % Set the brackets A and B
            data.a=alpha; data.f_a=f_alpha; data.fPrime_a=fPrime_alpha;
            data.b=beta; data.f_b=f_beta; data.fPrime_b=fPrime_beta;

            % Finished bracketing phase
            data.bracket_exitflag  = 2; return
    end

	% Reached max function evaluations
	if(data.funcCount>=optim.MaxFunEvals), data.bracket_exitflag=0; return; end
end

function data = sectioningPhase_simple(funfcn, data, optim)
% Get the brackets
brcktEndpntA=data.a; brcktEndpntB=data.b;

% Calculate minimum between brackets
[alpha,f_alpha_estimated] = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,data.a,data.b,data.f_a,data.fPrime_a,data.f_b,data.fPrime_b,optim);
if(isfield(data,'beta')&&(data.f_beta<f_alpha_estimated)), alpha=data.beta; end

[t,i]=find(data.storex==alpha,1);
if((~isempty(i))&&(~isnan(data.storegx(i))))
    f_alpha=data.storefx(i); grad=data.storegx(:,i);
else
    % Calculate the error and gradient for the next minimizer itteration
    [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
    if(isfield(data,'beta')&&(data.f_beta<f_alpha)),
        alpha=data.beta;
        if((~isempty(i))&&(~isnan(data.storegx(i))))
            f_alpha=data.storefx(i); grad=data.storegx(:,i);
        else
            [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        end
    end
end

% Store values linesearch
data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

fPrime_alpha = grad'*data.dir(:);
data.alpha=alpha;
data.fPrime_alpha= fPrime_alpha;
data.f_alpha= f_alpha;
data.grad=grad;

% Set the exit flag to succes
data.section_exitflag=[];

function data=linesearch(funfcn, data, optim)

% Find a bracket of acceptable points
data = bracketingPhase(funfcn, data,optim);

if (data.bracket_exitflag  == 2)
  % BracketingPhase found a bracket containing acceptable points;
  % now find acceptable point within bracket
  data = sectioningPhase(funfcn, data, optim);
  data.exitflag = data.section_exitflag;
else
  % Already acceptable point found or MaxFunEvals reached
  data.exitflag = data.bracket_exitflag;
end

function data = sectioningPhase(funfcn, data, optim)
%
% sectioningPhase finds an acceptable point alpha within a given bracket [a,b]
% containing acceptable points. Notice that funcCount counts the total number of
% function evaluations including those of the bracketing phase.

while(true)

    % Pick alpha in reduced bracket
    brcktEndpntA = data.a + min(optim.tau2,optim.sigma)*(data.b - data.a);
    brcktEndpntB = data.b - optim.tau3*(data.b - data.a);

    % Find global minimizer in bracket [brcktEndpntA,brcktEndpntB] of 3rd-degree
    % polynomial that interpolates f() and f'() at "a" and at "b".
    alpha = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,data.a,data.b,data.f_a,data.fPrime_a,data.f_b,data.fPrime_b,optim);

    % No acceptable point could be found
    if (abs( (alpha - data.a)*data.fPrime_a ) <= data.TolFunLnS), data.section_exitflag = -2; return; end

    % Calculate value (and gradient if no extra time cost) of current alpha
    if(~optim.GradConstr)
        [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
        fPrime_alpha = grad'*data.dir(:);
    else
        gstep=data.initialStepLength/1e6;
        if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
        if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
        [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
        fPrime_alpha=(f_alpha2-f_alpha)/gstep;
    end

	% Store values linesearch
	data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

    % Store current bracket position of A
    aPrev = data.a;
    f_aPrev = data.f_a;
    fPrime_aPrev = data.fPrime_a;

    % Update the current brackets
    if ((f_alpha > data.fInitial + alpha*optim.rho*data.fPrimeInitial) || (f_alpha >= data.f_a))
        % Update bracket B to current alpha
        data.b = alpha; data.f_b = f_alpha; data.fPrime_b = fPrime_alpha;
    else
        % Wolfe conditions, if true then acceptable point found
        if (abs(fPrime_alpha) <= -optim.sigma*data.fPrimeInitial),
            if(optim.GradConstr)
                % Gradient was not yet calculated because of time costs
                [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
                fPrime_alpha = grad'*data.dir(:);
            end
            % Store the found alpha values
            data.alpha=alpha; data.fPrime_alpha= fPrime_alpha; data.f_alpha= f_alpha;
            data.grad=grad;
            data.section_exitflag = []; return,
        end

        % Update bracket A
        data.a = alpha; data.f_a = f_alpha;  data.fPrime_a = fPrime_alpha;

        if (data.b - data.a)*fPrime_alpha >= 0
            % B becomes old bracket A;
            data.b = aPrev; data.f_b = f_aPrev;  data.fPrime_b = fPrime_aPrev;
        end
    end

    % No acceptable point could be found
    if (abs(data.b-data.a) < eps), data.section_exitflag = -2; return, end

    % maxFunEvals reached
    if(data.funcCount >optim.MaxFunEvals), data.section_exitflag = -1; return, end
end

function data = bracketingPhase(funfcn, data, optim)
% bracketingPhase finds a bracket [a,b] that contains acceptable points; a bracket
% is the same as a closed interval, except that a > b is allowed.
%
% The outputs f_a and fPrime_a are the values of the function and the derivative
% evaluated at the bracket endpoint 'a'. Similar notation applies to the endpoint
% 'b'.

% Parameters of bracket A
data.a = [];
data.f_a = [];
data.fPrime_a = [];

% Parameters of bracket B
data.b = [];
data.f_b = [];
data.fPrime_b = [];

% First trial alpha is user-supplied
% f_alpha will contain f(alpha) for all trial points alpha
% fPrime_alpha will contain f'(alpha) for all trial points alpha
alpha = data.initialStepLength;
f_alpha = data.fInitial;
fPrime_alpha = data.fPrimeInitial;

% Set maximum value of alpha (determined by fminimum)
alphaMax = (data.fminimum - data.fInitial)/(optim.rho*data.fPrimeInitial);
alphaPrev = 0;

while(true)
  % Evaluate f(alpha) and f'(alpha)
  fPrev = f_alpha;
  fPrimePrev = fPrime_alpha;

  % Calculate value (and gradient if no extra time cost) of current alpha
  if(~optim.GradConstr)
      [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
      fPrime_alpha = grad'*data.dir(:);
  else
      gstep=data.initialStepLength/1e6;
      if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
      if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
      [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
      [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
      fPrime_alpha=(f_alpha2-f_alpha)/gstep;
  end

  % Store values linesearch
  data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

  % Terminate if f < fminimum
  if (f_alpha <= data.fminimum), data.bracket_exitflag = 4; return; end

  % Bracket located - case 1 (Wolfe conditions)
  if (f_alpha > (data.fInitial + alpha*optim.rho*data.fPrimeInitial)) || (f_alpha >= fPrev)
    % Set the bracket values
    data.a = alphaPrev; data.f_a = fPrev;  data.fPrime_a = fPrimePrev;
    data.b = alpha; data.f_b = f_alpha;  data.fPrime_b = fPrime_alpha;
    % Finished bracketing phase
    data.bracket_exitflag  = 2; return
  end

  % Acceptable steplength found
  if (abs(fPrime_alpha) <= -optim.sigma*data.fPrimeInitial),
      if(optim.GradConstr)
          % Gradient was not yet calculated because of time costs
          [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
          fPrime_alpha = grad'*data.dir(:);
      end
      % Store the found alpha values
      data.alpha=alpha;
      data.fPrime_alpha= fPrime_alpha; data.f_alpha= f_alpha; data.grad=grad;
      % Finished bracketing phase, and no need to call sectioning phase
      data.bracket_exitflag = [];  return
  end

  % Bracket located - case 2
  if (fPrime_alpha >= 0)
    % Set the bracket values
    data.a = alpha; data.f_a = f_alpha;  data.fPrime_a = fPrime_alpha;
    data.b = alphaPrev; data.f_b = fPrev; data.fPrime_b = fPrimePrev;
    % Finished bracketing phase
    data.bracket_exitflag  = 2; return
  end

  % Update alpha
  if (2*alpha - alphaPrev < alphaMax )
      brcktEndpntA = 2*alpha-alphaPrev;
      brcktEndpntB = min(alphaMax,alpha+optim.tau1*(alpha-alphaPrev));
      % Find global minimizer in bracket [brcktEndpntA,brcktEndpntB] of 3rd-degree polynomial
      % that interpolates f() and f'() at alphaPrev and at alpha
      alphaNew = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,alphaPrev,alpha,fPrev, ...
                                         fPrimePrev,f_alpha,fPrime_alpha,optim);
      alphaPrev = alpha;
      alpha = alphaNew;
  else
      alpha = alphaMax;
  end

  % maxFunEvals reached
  if(data.funcCount >optim.MaxFunEvals), data.bracket_exitflag = -1; return, end
end

function [alpha,f_alpha]= pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,alpha1,alpha2,f1,fPrime1,f2,fPrime2,optim)
% finds a global minimizer alpha within the bracket [brcktEndpntA,brcktEndpntB] of the cubic polynomial
% that interpolates f() and f'() at alpha1 and alpha2. Here f(alpha1) = f1, f'(alpha1) = fPrime1,
% f(alpha2) = f2, f'(alpha2) = fPrime2.

% determines the coefficients of the cubic polynomial with c(alpha1) = f1,
% c'(alpha1) = fPrime1, c(alpha2) = f2, c'(alpha2) = fPrime2.
coeff = [(fPrime1+fPrime2)*(alpha2-alpha1)-2*(f2-f1) ...
    3*(f2-f1)-(2*fPrime1+fPrime2)*(alpha2-alpha1) (alpha2-alpha1)*fPrime1 f1];

% Convert bounds to the z-space
lowerBound = (brcktEndpntA - alpha1)/(alpha2 - alpha1);
upperBound = (brcktEndpntB - alpha1)/(alpha2 - alpha1);

% Swap if lowerbound is higher than the upperbound
if (lowerBound  > upperBound), t=upperBound; upperBound=lowerBound; lowerBound=t; end

% Find minima and maxima from the roots of the derivative of the polynomial.
sPoints = roots([3*coeff(1) 2*coeff(2) coeff(3)]);

% Remove imaginaire and points outside range

sPoints(imag(sPoints)~=0)=[];
sPoints(sPoints<lowerBound)=[]; sPoints(sPoints>upperBound)=[];

% Make vector with all possible solutions
sPoints=[lowerBound sPoints(:)' upperBound];

% Select the global minimum point
[f_alpha,index]=min(polyval(coeff,sPoints)); z=sPoints(index);

% Add the offset and scale back from [0..1] to the alpha domain
alpha = alpha1 + z*(alpha2 - alpha1);

% Show polynomial search
if(optim.Display(1)=='p');
    vPoints=polyval(coeff,sPoints);
    plot(sPoints*(alpha2 - alpha1)+alpha1,vPoints,'co');
    plot([sPoints(1) sPoints(end)]*(alpha2 - alpha1)+alpha1,[vPoints(1) vPoints(end)],'c*');
    xPoints=linspace(lowerBound/3, upperBound*1.3, 50);
    vPoints=polyval(coeff,xPoints);
    plot(xPoints*(alpha2 - alpha1)+alpha1,vPoints,'c');
end

function [data,fval,grad]=gradient_function(x,funfcn, data, optim)
    % Call the error function for error (and gradient)
    if ( nargout <3 )
        %%% timem=tic;
        fval=funfcn(reshape(x,data.xsizes));
        %%% data.timeExtern=data.timeExtern+toc(timem);
        data.funcCount=data.funcCount+1;
    else
        if(strcmp(optim.GradObj,'on'))
            %%% timem=tic;
            [fval, grad]=feval(funfcn,reshape(x,data.xsizes));
            %%% data.timeExtern=data.timeExtern+toc(timem);
            data.funcCount=data.funcCount+1;
            data.gradCount=data.gradCount+1;
        else
            % Calculate gradient with forward difference if not provided by the function
            grad=zeros(length(x),1);
            fval=funfcn(reshape(x,data.xsizes));
            gstep=data.initialStepLength/1e6;
            if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
            if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
            for i=1:length(x),
                x_temp=x; x_temp(i)=x_temp(i)+gstep;
                %%% timem=tic;
                [fval_g]=feval(funfcn,reshape(x_temp,data.xsizes)); data.funcCount=data.funcCount+1;
                %%% data.timeExtern=data.timeExtern+toc(timem);
                grad(i)=(fval_g-fval)/gstep;
            end
        end
        grad=grad(:);
    end

function data = updateQuasiNewtonMatrix_LBFGS(data,optim)
% updates the quasi-Newton matrix that approximates the inverse to the Hessian.
% Two methods are support BFGS and L-BFGS, in L-BFGS the hessian is not
% constructed or stored.
% Calculate position, and gradient diference between the
% itterations
deltaX=data.alpha* data.dir;
deltaG=data.gradient-data.gOld;

if ((deltaX'*deltaG) >= sqrt(eps)*max( eps,norm(deltaX)*norm(deltaG) ))

    if(optim.HessUpdate(1)=='b')
        % Default BFGS as described by Nocedal
        p_k = 1 / (deltaG'*deltaX);
        Vk = eye(data.numberOfVariables) - p_k*deltaG*deltaX';
        % Set Hessian
        data.Hessian = Vk'*data.Hessian *Vk + p_k * deltaX*deltaX';
        % Set new Direction
        data.dir = -data.Hessian*data.gradient;
    else
        % L-BFGS with scaling as described by Nocedal

        % Update a list with the history of deltaX and deltaG
        data.deltaX(:,2:optim.StoreN)=data.deltaX(:,1:optim.StoreN-1); data.deltaX(:,1)=deltaX;
        data.deltaG(:,2:optim.StoreN)=data.deltaG(:,1:optim.StoreN-1); data.deltaG(:,1)=deltaG;

        data.nStored=data.nStored+1; if(data.nStored>optim.StoreN), data.nStored=optim.StoreN; end

        % Initialize variables
        a=zeros(1,data.nStored);
        p=zeros(1,data.nStored);

        q = data.gradient;
        for i=1:data.nStored
            p(i)= 1 / (data.deltaG(:,i)'*data.deltaX(:,i));
            a(i) = p(i)* data.deltaX(:,i)' * q;
            q = q - a(i) * data.deltaG(:,i);
        end
        % Scaling of initial Hessian (identity matrix)
        p_k = data.deltaG(:,1)'*data.deltaX(:,1) / sum(data.deltaG(:,1).^2);

        % Make r = - Hessian * gradient
        r = p_k * q;
        for i=data.nStored:-1:1,
            b = p(i) * data.deltaG(:,i)' * r;
            r = r + data.deltaX(:,i)*(a(i)-b);
        end

        % Set new direction
        data.dir = -r;
    end
end

function earlystoppedreturn = LVQ_progresser(variables, output, where)
%
% control actions during each iteration of the fminlbfgs optimization
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
% Kerstin Bunte based on the code from Marc Strickert
%
global earlystopped threshstop

if sum(where == 'iter') < 4
    earlystopped = false;
    earlystoppedreturn = earlystopped;
    return
end

earlystopped = exist ('stop','file') > 0;
earlystoppedreturn = earlystopped;

if earlystopped
    delete('stop');
    return
end

if isempty(threshstop)
    return
end

if threshstop > 0  % early stopping by simple thresholding: if too good: stop
    earlystopped = output.fval < threshstop;  % close to perfect
    earlystoppedreturn = earlystopped;
    if earlystopped
      return
    end
end

global useEarlyStopping

if isempty(useEarlyStopping)
    return
end

%%% early stopping

global training_data training_label prototypeLabel% n_vec bestvariables
nb_prototypes = numel(prototypeLabel);
model.w = variables(1:nb_prototypes,:);
model.c_w = prototypeLabel;
model.omega = variables(nb_prototypes+1:end,:);
estimatedLabels = GMLVQ_classify(training_data, model);
ref = mean( training_label ~= estimatedLabels );
% fprintf('error: %f',ref);
%   if isempty(n_vec) || n_vec < 1 % GRLVQ
%     lam = variables(1,:); % weights
%     protos = variables(2:end,:); % protos
%     [cls C] = applyprotoseuc(protos, protolabl, lam, datval, labval);
%   else % MRLVQ
%     lam = variables(1:n_vec,:).';
%     protos = variables((n_vec+1):end,:);
%     [cls C] = applyprotosmat(protos, protolabl, lam, datval, labval);
%   end
% ref = trace(C)/sum(C(:))-1; % negative classif error

persistent last penalty
% disp(last);
if isempty(last)
%     bestvariables = variables;
    penalty = -25;  % penalty counter, skip first 15 iterations (burn-in phase)
%     last = ref * ones(5,1);  % memory of 5 elements
    last = ref;
else
%     [srt idxsrt] = sort([last; ref],'descend');
%     fvalval = srt(1);
%     fpos = find(idxsrt == 6);  % index of added element
%     if fpos == 6  % last position?
      penalty = penalty + 1;
      if penalty > 10  % allow a maximum of 10 times to fail
        earlystopped = true;
        earlystoppedreturn = earlystopped;
        return
      end
%     else
% %       last = srt(1:5);
% %       if fpos == 1  % best position
% %         bestvariables = variables;
% %       end
%       if penalty < 0  % still in burn-in phase
%         penalty = penalty + 1;
%       end
%       if penalty > 0  % do not go below here
%         penalty = penalty - 1;
%       end
%     end
end
earlystoppedreturn = earlystopped;


%%*********************************************************************
%% gdcomp: Compute gd = 1/td in Equation (15) of the paper:
%%
%% R.M. Freund, F. Ordonez, and K.C. Toh,
%% Behavioral measures and their correlation with IPM iteration counts
%% on semi-definite programming problems,
%% Mathematical Programming, 109 (2007), pp. 445--475.
%%
%% [gd,info,yfeas,Zfeas,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS,solveyes);
%%
%% yfeas,Zfeas: a dual feasible pair when gd is finite.
%%              That is, if
%%              Aty = Atyfun(blk,At,[],[],yfeas);
%%              Rd = ops(C,'-',ops(Zfeas,'+',Aty));
%%              then
%%              ops(Rd,'norm') should be small.
%%
%%*********************************************************************

  function [gd,info,yfeas,Zfeas,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS,solveyes);

  if (nargin < 6); solveyes = 1; end
  if (nargin < 5)
     OPTIONS = sqlparameters;
     OPTIONS.vers   = 1;
     OPTIONS.gaptol = 1e-10;
     OPTIONS.printlevel = 3;
  end
  if isempty(OPTIONS); OPTIONS = sqlparameters; end
  if ~isfield(OPTIONS,'solver'); OPTIONS.solver = 'HSDsqlp'; end
  if ~isfield(OPTIONS,'printlevel'); OPTIONS.printlevel = 3; end
  if ~iscell(C); tmp = C; clear C; C{1} = tmp; end
%%
  m = length(b);
  blk2 = blk;
  At2 = cell(size(blk,1),1);
  C2 = cell(size(blk,1),1);
  EE = cell(size(blk,1),1);
%%
%%
%%
  dd = zeros(1,m);
  alp = 0;
  beta = 0;
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     n = sum(pblk{2});
     if strcmp(pblk{1},'s')
        C2{p,1} = sparse(n,n);
     else
        C2{p,1} = zeros(n,1);
     end
     dd = dd + sqrt(sum(At{p}.*At{p}));
     beta = beta + norm(C{p},'fro');
     alp = alp + sqrt(n);
  end
  alp  = 1./max(1,alp);
  beta = 1./max(1,beta);
  dd   = 1./max(1,dd);
%%
%% New multipliers in dual problem:
%% [v; tt; theta].
%%
   D = spdiags(dd',0,m,m);

   ss = 0; cc = 0; aa = zeros(1,m);
   exist_ublk = 0;
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      if strcmp(pblk{1},'s')
         At2{p} = [At{p}*D, svec(pblk,alp*speye(n,n),1), -svec(pblk,beta*C{p},1)];
         ss = ss + n;
         cc = cc + trace(C{p});
         aa = aa + svec(pblk,speye(n),1)'*At{p};
         EE{p} = speye(n,n);
      elseif strcmp(pblk{1},'q')
         eq = zeros(n,1);
         idx1 = 1+[0,cumsum(pblk{2})];
         idx1 = idx1(1:length(idx1)-1);
         eq(idx1) = ones(length(idx1),1);
         At2{p} = [At{p}*D, 2*sparse(alp*eq), -sparse(beta*C{p})];
         ss = ss + 2*length(pblk{2});
         cc = cc + sum(C{p}(idx1));
         aa = aa + eq'*At{p};
         EE{p} = eq;
      elseif strcmp(pblk{1},'l')
         el = ones(n,1);
         At2{p} = [At{p}*D, sparse(alp*el), -sparse(beta*C{p})];
         ss = ss + n;
         cc = cc + el'*C{p};
         aa = aa + el'*At{p};
         EE{p} = el;
      elseif strcmp(pblk{1},'u')
         At2{p} = [At{p}*D, sparse(n,1), -sparse(beta*C{p})];
         exist_ublk = 1;
         EE{p} = sparse(n,1);
      end
   end
   aa = aa.*dd;
   cc = cc*beta;
%%
%% 4 additional inequality constraints in dual problem.
%%
   numblk = size(blk,1);
   blk2{numblk+1,1} = 'l'; blk2{numblk+1,2} = 4;
   C2{numblk+1,1}  = [1; 1; 0; 0];
   At2{numblk+1,1} = [-aa,         0,   cc;
		     zeros(1,m),   0,   beta;
		     zeros(1,m),  alp, -beta
                     zeros(1,m), -alp,  0];
   At2{numblk+1} = sparse(At2{numblk+1});
   b2 = [zeros(m,1); alp; 0];
%%
%% Solve SDP
%%
   gd = []; info = []; yfeas = []; Zfeas = [];
   if (solveyes)
      if strcmp(OPTIONS.solver,'sqlp')
         [X0,y0,Z0] = infeaspt(blk2,At2,C2,b2,2,100);
         [obj,X,y,Z,info] = sqlp(blk2,At2,C2,b2,OPTIONS,X0,y0,Z0);
      else
         [obj,X,y,Z,info] = HSDsqlp(blk2,At2,C2,b2,OPTIONS);
      end
      tt = alp*abs(y(m+1)); theta = beta*abs(y(m+2));
      yfeas = D*y(1:m)/theta;
      Zfeas = ops(ops(Z(1:numblk),'+',EE,tt),'/',theta);
      %%
      if (obj(2) > 0) | (abs(obj(2)) < 1e-8)
         gd = 1/abs(obj(2));
      elseif (obj(1) > 0)
         gd = 1/obj(1);
      else
         gd = 1/exp(mean(log(abs(obj))));
      end
      err = max(info.dimacs([1,3,6]));
      if (OPTIONS.printlevel)
         fprintf('\n ******** gd = %3.2e, err = %3.1e\n',gd,err);
         if (err > 1e-6);
            fprintf('\n----------------------------------------------------')
            fprintf('\n gd problem is not solved to sufficient accuracy');
            fprintf('\n----------------------------------------------------\n')
         end
      end
   end
%%*********************************************************************


function moebius_values_test ( )

%*****************************************************************************80
%
%% MOEBGIUS_VALUES_TEST demonstrates the use of MOEBIUS_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MOEBIUS_VALUES_TEST:\n' );
  fprintf ( 1, '  MOEBIUS_VALUES returns values of \n' );
  fprintf ( 1, '  the Moebius function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         MU(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = moebius_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n', n, fn );

  end

  return
end


function [zCart,RCart]=ruv2CartStdRefracCubature(zRUVBiased,SR,useHalfRange,zTx,zRx,M,Ns,xi,w,ce,rE,spherCent,xMax)
%%RUV@CARTSTDREFRACCUBATURE Use cubature integration to approximate the
%              moments of measurements converted from refraction-corrupted
%              bistatic r-u-v coordinates into Cartesian coordinates using
%              a standard exponential atmospheric model. For a two-way
%              monostatic conversion, set zTx=[0;0;0]; to make the
%              transmitter and receiver collocated.
%
%INPUTS: z A 3XnumMeas matrix of numMeas vectors to convert. Each has
%          elements [r;u;v], where r is the bistatic range from the
%          transmitter to the target to the receiver, and u and v are
%          direction cosines.
%       SR The 3X3XnumMeas lower-triangular square roots of the measurement
%          covariance matrices for the measurements. If all of the matrices
%          are the same, then this can just be a single 3X3 matrix.
% useHalfRange A boolean value specifying whether the bistatic range value
%          should be divided by two. This normally comes up when operating
%          in monostatic mode, so that the range reported is a one-way
%          range. The default if this parameter is not provided (or an
%          empty matrix is provided) is false.
%      zTx The 3X1 [x;y;z] location vector of the transmitter in global
%          Cartesian coordinates.  If this parameter is omitted or an empty
%          matrix is passed, then the receiver is placed at the origin.
%      zRx The 3X1 [x;y;z] location vector of the receiver in global
%          Cartesian coordinates. If this parameter is omitted or an empty
%          matrix is passed, then the receiver is placed at the origin.
%        M A 3X3 rotation matrix to go from the alignment of the global
%          coordinate system to the local alignment fo the receiver. The z
%          vector of the local coordinate system of the receiver is the
%          pointing direction of the receiver. If this matrix is omitted,
%          then the identity matrix is used.
%       Ns The atmospheric refractivity reduced to the reference sphere.
%          Note that the refractivity is (n-1)*1e6, where n is the index
%          of refraction. The function reduceStdRefrac2Spher can be used
%          to reduce a refractivity to the surface of a reference
%          ellipsoid. This function does not allow different
%          refractivities to be used as the transmitter and receiver. If
%          this parameter is omitted or an empty matrix is passed, a
%          default value of 313 is used.
%       xi A 3 X numCubaturePoints matrix of cubature points for the
%          numeric integration. If this and the next parameter are omitted
%          or empty matrices are passed, then fifthOrderCubPoints is used
%          to generate cubature points.
%        w A numCubaturePoints X 1 vector of the weights associated with
%          the cubature points.
%       ce The optional decay constant of the exponential model. The
%          refractivity N at height h is N=Ns*exp(-ce*(h-h0)) where h0 is
%          the reference height (in this function, the height of the
%          reference ellipsoid surface is used). ce is related to the
%          change in refractivity at an elevation of 1km based on the
%          refractivity at sea level as
%          ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
%          where the change in refractivity for a change in elevation of
%          1km is DeltaN=-multConst*exp(expConst*Ns); In [1], standard
%          values for the two constants are expConst=0.005577; and
%          multConst=7.32; If ce is omitted or an empty matrix is passed,
%          the value based on the standard model is used.
% rE,spherCent The radius of the Earth to use for the spherical Earth
%           approximation used in the model and also the offset between the
%           global model and the local spherical model. It is assumed that
%           zC,zTx,and zRx are all given in the global model and will need
%           to be transformed to the local model to the used. If rE is
%           omitted or an empty matrix is passed, then the default of
%           [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx)) is
%           used. The defaults here mean that a WGS-84 reference ellipsoid
%           is approximated by the local osculating sphere.
%     xMax This function traces the ray to a maximum displacement in the
%          local tangent plane prior (or vertically for nearly vertical
%          directions) to attempting to find a particular range. This is an
%          optional parameter specifying the maximum distance to trace the
%          ray. The default if this parameter is omitted or an empty matrix
%          is passed is 1000e3 (1000km).
%
%OUTPUTS: zCart The approximate means of the PDF of the measurements
%               in global Cartesian [x;y;z] coordinates. This is a
%               3XnumMeas matrix.
%         RCart The approximate 3X3XnumMeas covariance matrices of the
%               PDF of the Cartesian converted measurements. This is a
%               3X3XnumMeas hypermatrix.
%
%The basic cubature conversion approach is detailed in [1] and [2]. The
%standard exponential measurement model is from [3] and the model is
%discussed in more detail in the comments to ruv2CartStdRefrac.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%[2] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[3] B. R. Bean and G. D. Thayer, CRPL Exponential Reference Atmosphere.
%    Washington, D.C.: U. S. Department of Commerce, National Bureau of
%    Standards, Oct. 1959. [Online]. Available:
%    http://digicoll.manoa.hawaii.edu/techreports/PDF/NBS4.pdf
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=size(zRUVBiased,2);

if(nargin<3||isempty(useHalfRange))
    useHalfRange=false;
end

if(nargin<4||isempty(zTx))
   zTx=zeros(3,1);
end

if(nargin<5||isempty(zRx))
   zRx=zeros(3,1);
end

if(nargin<6||isempty(M))
   M=eye(3,3);
end

if(nargin<7||isempty(Ns))
   Ns=313;
end

if(nargin<8||isempty(xi))
    [xi,w]=fifthOrderCubPoints(3);
end

if(nargin<10||isempty(ce))
    expConst=0.005577;
    multConst=7.32;

    %The change in refractivity at an elevation of 1km based on the
    %refractivity on the surface of the Earth.
    DeltaN=-multConst*exp(expConst*Ns);
    ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
end

if(nargin<11||isempty(rE))
    %Use the radius of the Earth that is the radius of the osculating
    %sphere at the location of the observed. This will be the radius used
    %in the local spherical Earth approximation for computing atmospheric
    %refraction. This uses the WGS-84 reference ellipsoid.
    [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx));
end

if(nargin<13||isempty(xMax))
    xMax=1000e3;%1000 kilometer assumed maximum x displacement.
end

if(size(SR,3)==1)
    SR=repmat(SR,[1,1,numMeas]);
end

zCart=zeros(3,numMeas);
RCart=zeros(3,3,numMeas);
for curMeas=1:numMeas
    %Transform the cubature points to match the given Gaussian.
    cubPoints=transformCubPoints(xi,zRUVBiased(:,curMeas),SR(:,:,curMeas));

    %Convert all of the points into Cartesian space
    cubPoints=ruv2CartStdRefrac(cubPoints,useHalfRange,zTx,zRx,M,Ns,ce,rE,spherCent,xMax);

    %Extract the first two moments of the transformed points.
    [zCart(:,curMeas),RCart(:,:,curMeas)]=calcMixtureMoments(cubPoints,w);
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the TWSC algorithm for additive white Gaussian noise
% noise removal.
%
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk / nankaimathxujun@gmail.com
%          The Hong Kong Polytechnic University
%
% Please refer to the following paper if you find this code helps:
%
% @article{TWSC_ECCV2018,
% 	author = {Jun Xu and Lei Zhang and David Zhang},
% 	title = {A Trilateral Weighted Sparse Coding Scheme for Real-World Image Denoising},
% 	journal = {ECCV},
% 	year = {2018}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------
clear;
Original_image_dir  =    'cleanimages/'; % put clean images in this folder
Sdir = regexp(Original_image_dir, '\', 'split');
fpath = fullfile(Original_image_dir, '*.png');
im_dir  = dir(fpath);
im_num = length(im_dir);

method = 'TWSC';
write_MAT_dir = [method '/Results_AWGN/'];
write_sRGB_dir  = [write_MAT_dir method '/'];
if ~isdir(write_sRGB_dir)
    mkdir(write_sRGB_dir);
end
for nSig = [15 25 35 50 75]
    %% Parameters
    Par.innerIter = 2;
    Par.win = 30;
    Par.lambda1 = 0;
    Par.ps = 8;
    Par.outerIter = 10;
    Par.step = 3;
    Par.nlspini = 90;
    Par.nlspgap = 10;
    if 0 < nSig <= 20
        Par.outerIter = 8;
        Par.delta = .07;
        Par.nlspini = 70;
        Par.lambda2 = .9;
    elseif 20 < nSig <= 30
        Par.delta = .06;
        Par.lambda2 = .76;
    elseif 30 < nSig <= 40
        Par.delta = .07;
        Par.lambda2 = .78;
    elseif 40 < nSig <= 60
        Par.nlspini = 120;
        Par.nlspgap = 15;
        Par.delta = .05;
        Par.lambda2 = .72;
    elseif 60 < nSig <= 80
        Par.ps = 9;
        Par.outerIter = 14;
        Par.step = 4;
        Par.nlspini = 140;
        Par.delta = .05;
        Par.lambda2 = .68; % .66
    else
        disp('Please tune the above parameters by yourself, thanks!');
    end
    % record all the results in each iteration
    Par.PSNR = zeros(Par.outerIter, im_num, 'double');
    Par.SSIM = zeros(Par.outerIter, im_num, 'double');
    T512 = [];
    T256 = [];
    for i = 1:im_num
        Par.nlsp = Par.nlspini;  % number of non-local patches
        Par.image = i;
        Par.nSig = nSig/255;
        Par.I =  im2double( imread(fullfile(Original_image_dir, im_dir(i).name)) );
        S = regexp(im_dir(i).name, '\.', 'split');
        randn('seed',0);
        Par.nim =   Par.I + Par.nSig*randn(size(Par.I));
        fprintf('%s :\n',im_dir(i).name);
        PSNR =   csnr( Par.nim*255, Par.I*255, 0, 0 );
        SSIM      =  cal_ssim( Par.nim*255, Par.I*255, 0, 0 );
        fprintf('The initial value of PSNR = %2.4f, SSIM = %2.4f \n', PSNR,SSIM);
        time0 = clock;
        [im_out, Par]  =  TWSC_Sigma_AWGN(Par);
        if size(Par.I,1) == 512
            T512 = [T512 etime(clock,time0)];
            fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
        elseif size(Par.I,1) ==256
            T256 = [T256 etime(clock,time0)];
            fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
        end
        im_out(im_out>1)=1;
        im_out(im_out<0)=0;
        % calculate the PSNR
        Par.PSNR(Par.outerIter, Par.image)  =   csnr( im_out*255, Par.I*255, 0, 0 );
        Par.SSIM(Par.outerIter, Par.image)      =  cal_ssim( im_out*255, Par.I*255, 0, 0 );
        imname = sprintf([write_sRGB_dir method '_nSig' num2str(nSig) '_oIte' num2str(Par.outerIter) '_iIte' num2str(Par.innerIter) '_ps' num2str(Par.ps) '_step' num2str(Par.step) '_nlspini' num2str(Par.nlspini) '_nlspgap' num2str(Par.nlspgap) '_delta' num2str(Par.delta) '_l1' num2str(Par.lambda1) '_l2' num2str(Par.lambda2) '_' im_dir(i).name]);
        imwrite(im_out,imname);
        fprintf('%s : PSNR = %2.4f, SSIM = %2.4f \n',im_dir(i).name, Par.PSNR(Par.outerIter, Par.image),Par.SSIM(Par.outerIter, Par.image)     );
    end
    PSNR = Par.PSNR(end,:);
    mPSNR=mean(PSNR,2);
    SSIM = Par.SSIM(end,:);
    mSSIM=mean(SSIM,2);
    mT512 = mean(T512);
    sT512 = std(T512);
    mT256 = mean(T256);
    sT256 = std(T256);
    fprintf('The average PSNR = %2.4f, SSIM = %2.4f. \n', mPSNR,mSSIM);
    name = sprintf([write_MAT_dir method '_nSig' num2str(nSig) '_oIte' num2str(Par.outerIter) '_iIte' num2str(Par.innerIter) '_ps' num2str(Par.ps) '_step' num2str(Par.step) '_nlspini' num2str(Par.nlspini) '_nlspgap' num2str(Par.nlspgap) '_delta' num2str(Par.delta) '_l1' num2str(Par.lambda1) '_l2' num2str(Par.lambda2) '.mat']);
    save(name,'nSig','PSNR','SSIM','mPSNR','mSSIM','mT512','sT512','mT256','sT256');
end


classdef CEC2010_F4 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2010.mat'),'Data');
            obj.O = Data{4};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 50;
            obj.upper    = zeros(1,obj.D) + 50;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = max(Z,[],2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = abs(mean((Z.*cos(sqrt(abs(Z)))),2)) - 1e-4;
            PopCon(:,2) = abs(sum((Z(:,1:end/2-1)-Z(:,2:end/2)).^2,2)) - 1e-4;
            PopCon(:,3) = abs(sum((Z(:,end/2+1:end-1).^2-Z(:,end/2+2:end)).^2,2)) - 1e-4;
            PopCon(:,4) = abs(sum(Z,2)) - 1e-4;
        end
    end
end


function saveOBBs(obblist,filename)

fid = fopen(filename,'wt');

for i = 1:size(obblist,1)
    p = obblist(i,:);

    center = p(1:3);
    dir_1 = p(4:6);
    dir_2 = p(7:9);
    lengths = p(10:12);

    dir_1 = dir_1/norm(dir_1);
    dir_2 = dir_2/norm(dir_2);
    dir_3 = cross(dir_1,dir_2);
    dir_3 = dir_3/norm(dir_3);
    cornerpoints = zeros(8,3);

    d1 = 0.5*lengths(1)*dir_1;
    d2 = 0.5*lengths(2)*dir_2;
    d3 = 0.5*lengths(3)*dir_3;
    cornerpoints(1,:) = center-d1-d2-d3;
    cornerpoints(2,:) = center-d1+d2-d3;
    cornerpoints(3,:) = center+d1-d2-d3;
    cornerpoints(4,:) = center+d1+d2-d3;
    cornerpoints(5,:) = center-d1-d2+d3;
    cornerpoints(6,:) = center-d1+d2+d3;
    cornerpoints(7,:) = center+d1-d2+d3;
    cornerpoints(8,:) = center+d1+d2+d3;

    for j = 1:size(cornerpoints,1)
        fprintf(fid,'v  %f  %f  %f  \n',cornerpoints(j,1),cornerpoints(j,2),cornerpoints(j,3));
    end
end

N = 8;
for i = 1:size(obblist,1)
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+6,(i-1)*N+4,(i-1)*N+2);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+6,(i-1)*N+8,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+8,(i-1)*N+7,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+7,(i-1)*N+3,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+7,(i-1)*N+5,(i-1)*N+3);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+2,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+6,(i-1)*N+2);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+4,(i-1)*N+3,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+2,(i-1)*N+4,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+8,(i-1)*N+6);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+7,(i-1)*N+8);
end

fclose(fid);
% plot3([cornerpoints(1,1),cornerpoints(2,1)],[cornerpoints(1,2),cornerpoints(2,2)],[cornerpoints(1,3),cornerpoints(2,3)],col);hold on;
% plot3([cornerpoints(1,1),cornerpoints(3,1)],[cornerpoints(1,2),cornerpoints(3,2)],[cornerpoints(1,3),cornerpoints(3,3)],col);hold on;
% plot3([cornerpoints(2,1),cornerpoints(4,1)],[cornerpoints(2,2),cornerpoints(4,2)],[cornerpoints(2,3),cornerpoints(4,3)],col);hold on;
% plot3([cornerpoints(3,1),cornerpoints(4,1)],[cornerpoints(3,2),cornerpoints(4,2)],[cornerpoints(3,3),cornerpoints(4,3)],col);hold on;
% plot3([cornerpoints(5,1),cornerpoints(6,1)],[cornerpoints(5,2),cornerpoints(6,2)],[cornerpoints(5,3),cornerpoints(6,3)],col);hold on;
% plot3([cornerpoints(5,1),cornerpoints(7,1)],[cornerpoints(5,2),cornerpoints(7,2)],[cornerpoints(5,3),cornerpoints(7,3)],col);hold on;
% plot3([cornerpoints(6,1),cornerpoints(8,1)],[cornerpoints(6,2),cornerpoints(8,2)],[cornerpoints(6,3),cornerpoints(8,3)],col);hold on;
% plot3([cornerpoints(7,1),cornerpoints(8,1)],[cornerpoints(7,2),cornerpoints(8,2)],[cornerpoints(7,3),cornerpoints(8,3)],col);hold on;
% plot3([cornerpoints(1,1),cornerpoints(5,1)],[cornerpoints(1,2),cornerpoints(5,2)],[cornerpoints(1,3),cornerpoints(5,3)],col);hold on;
% plot3([cornerpoints(2,1),cornerpoints(6,1)],[cornerpoints(2,2),cornerpoints(6,2)],[cornerpoints(2,3),cornerpoints(6,3)],col);hold on;
% plot3([cornerpoints(3,1),cornerpoints(7,1)],[cornerpoints(3,2),cornerpoints(7,2)],[cornerpoints(3,3),cornerpoints(7,3)],col);hold on;
% plot3([cornerpoints(4,1),cornerpoints(8,1)],[cornerpoints(4,2),cornerpoints(8,2)],[cornerpoints(4,3),cornerpoints(8,3)],col);hold on;

end


function ADE = estimate_average_displacement_error(displacementField1,displacementField2,mask)
% ESTIMATE_AVERAGE_DISPLACEMENT_ERROR Estimates the average displacement error
%
% ADE = estimate_average_displacement_error(displacementField1,displacementField2,mask)
%
% INPUT ARGUMENTS
% displacementField1    - Displacement field 1
% displacementField2    - Displacement field 2
% mask                  - Mask, to remove certain points in the data
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% ADE                   - Average displacement error between the two
%                         provided displacement fields

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% This program 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.
%
% This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

dims = length(displacementField1);
numberOfElements = sum(mask(:) ~= 0);
switch dims
    case 1
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        ADE = sum(sqrt(x_error(:).^2))/numberOfElements;
    case 2
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        y_error = mask.*(displacementField1{2}-displacementField2{2});
        ADE = sum(sqrt(x_error(:).^2 + y_error(:).^2))/numberOfElements;
    case 3
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        y_error = mask.*(displacementField1{2}-displacementField2{2});
        z_error = mask.*(displacementField1{3}-displacementField2{3});
        ADE = sum(sqrt(x_error(:).^2 + y_error(:).^2 + z_error(:).^2))/numberOfElements;
    otherwise
        error('Not implemented for dimensions higher than three.')
end


function ic = check_incircle_edge(vertex, face, edge)

% check_incicle_edge - compute "empty circle" property for a set of edges
%
%   ic = check_incicle_edge(vertex,face, edge);
%
%   ic(i)==1 if edge(:,i) is delaunay valid (boundary or empty circles or non convex).
%   It thus should be flipped if ic(i)==0.
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin<3
    edge = compute_edges(face);
end

n = size(edge, 2);
e2f = compute_edge_face_ring(face);

m = size(e2f,1);
f1 = full( e2f(edge(1,:)+(edge(2,:)-1)*m) );
f2 = full( e2f(edge(2,:)+(edge(1,:)-1)*m) );

if not( isempty( find(f1==0 & f2==0) ) )
    warning('Problem with triangulation');
end

ic = ones(n,1);
isconvex = ones(n,1);

I = find(f1>0 & f2>0);

f1 = f1(I); f2 = f2(I);

f1 = face(:,f1);
f2 = face(:,f2);

edge = edge(:,I);
n = length(I);

points1 = find_other(f2, edge(1,:), edge(2,:));
ic1 = check_incircle(vertex, f1, points1 );

points2 = find_other(f1, edge(1,:), edge(2,:));
ic2 = check_incircle(vertex, f2, points2 );

ic(I) = ic1 & ic2;

% check for convexity
v = vertex(:,points2) - vertex(:,points1);
u1 = vertex(:,edge(1,:)) - vertex(:,points1);
u2 = vertex(:,edge(2,:)) - vertex(:,points1);
A = zeros(2,2,n); B = zeros(2,2,n);
A(:,1,:) = reshape(v, [2 1 n]);
A(:,2,:) = reshape(u1, [2 1 n]);
B(:,1,:) = reshape(v, [2 1 n]);
B(:,2,:) = reshape(u2, [2 1 n]);
isconvex(I) = ( det3(A) .* det3(B) ) < 0;
% non convex should not be flipped
ic(isconvex==0) = 1;

%%
function f = find_other(f,a,b)

u = f - repmat(a, [3 1]); f(u==0) = 0;
u = f - repmat(b, [3 1]); f(u==0) = 0;
if sum( sum(f==0)~=2 )>0
    error('Problem with triangulation');
end
f = sum(f);


function [sig,a] = spm_shoot_blur(t,prm,its,sig)
% A function for blurring ("smoothing") tissue probability maps
% FORMAT [sig,a_new] = spm_shoot_blur(t,prm,its,sig)
%     t   - sufficient statistics
%     prm - regularisation parameters (1,1,1, 0.01,0.02,1)
%     its - max no. iterations (12)
%     sig - optional starting estimates
%
%     sig - "smoothed" average
%     a   - parameters
%
% The core of this procedure is described in:
%     John Ashburner & Karl J. Friston.
%     "Computing Average Shaped Tissue Probability Templates"
%     NeuroImage, In Press, Accepted Manuscript, Available online 24 December 2008
%
% However, there is an additional modification such that the the null space
% of the parameters is rotated out.
%________________________________________________________
% (c) Wellcome Trust Centre for NeuroImaging (2009)

% John Ashburner
% $Id: spm_shoot_blur.m 7387 2018-08-03 15:13:57Z john $

d   = [size(t),1,1,1];
if nargin<3, its = 16;                         end % Maximum no. iterations
if nargin<2, prm = [1,1,1, 0.01 0.02 1];       end % Default regularisation
rits = [1 1]; % No. cycles and no. relaxation iterations

W    = zeros([d(1:3) round(((d(4)-1)*d(4))/2)],'single'); % 2nd derivatives
gr   = zeros([d(1:3),d(4)-1],'single');                   % 1st derivatives

% Re-organise sufficient statistics to a form that is easier to work with
t    = max(t,eps('single')*1000);
s    = sum(t,4);
for k=1:d(4)
    t(:,:,:,k) = t(:,:,:,k)./s;
end
maxs   = max(s(:)); % Used for scaling the regularisation
prm(4) = prm(4)+maxs*d(4)*1e-6;

% Only d(4)-1 fields need to be estimated because sum(a,4) = 0.  This matrix
% is used to rotate out the null space
R     = null(ones(1,d(4)));

% Initial starting estimates (if sig is passed)
a     = zeros([d(1:3),d(4)-1],'single');
if nargin>=4
    for z=1:d(3) % Loop over planes
        sz = sig(:,:,z,:);
        sz = min(max(sz,0),1);
        sz(~isfinite(sz)) = 1/d(4);
        sz = squeeze(log(double(sz*(1-d(4)*1e-3)+1e-3)));
        for j1=1:(d(4)-1)
            az = zeros(d(1:2));
            for j2=1:d(4)
                az = az + R(j2,j1)*sz(:,:,j2); % Note the rotation
            end
            a(:,:,z,j1) = az;
        end
        clear sz az
    end
end

for i=1:its

    ll  = 0;
    for z=1:d(3) % Loop over planes

        % Compute softmax for this plane
        sig = double(reshape(sftmax(a(:,:,z,:),R),[d(1:2),d(4)]));

        % -ve log likelihood of the likelihood
        ll  = ll - sum(sum(sum(log(sig).*reshape(t(:,:,z,:),[d(1:2),d(4)]),3).*s(:,:,z)));

        % Compute first derivatives (d(4)-1) x 1
        grz = sig - double(reshape(t(:,:,z,:),[d(1:2),d(4)]));
        for j1=1:(d(4)-1)
            gr(:,:,z,j1) = 0;
            for j2=1:d(4)
                gr(:,:,z,j1) = gr(:,:,z,j1) + R(j2,j1)*grz(:,:,j2); % Note the rotation
            end
            gr(:,:,z,j1) = gr(:,:,z,j1).*s(:,:,z);
        end

        % Compute d(4) x d(4) matrix of second derivatives at each voxel.
        % These should be positive definate, but rounding errors may prevent this.
        % Regularisation is included to enforce +ve definateness.
        wz = zeros([d(1:2),d(4),d(4)]);
        for j1=1:d(4)
            wz(:,:,j1,j1) =   (1-sig(:,:,j1)).*sig(:,:,j1).*s(:,:,z);
            for j2=1:(j1-1)
                wz(:,:,j1,j2) = -sig(:,:,j1) .*sig(:,:,j2).*s(:,:,z);
                wz(:,:,j2,j1) = wz(:,:,j1,j2);
            end
        end

        % First step of rotating 2nd derivatives to (d(4)-1) x (d(4)-1)
        % by R'*W*R
        wz1 = zeros([d(1:2),d(4),d(4)-1]);
        for j1=1:d(4)
            for j2=1:(d(4)-1)
                tmp = zeros(d(1:2));
                for j3=1:d(4)
                    tmp = tmp + wz(:,:,j1,j3)*R(j3,j2);
                end
                wz1(:,:,j1,j2) = tmp;
            end
        end

        % Second step of rotating 2nd derivatives to (d(4)-1) x (d(4)-1)
        % by R'*W*R
        wz = zeros([d(1:2),d(4)-1,d(4)-1]);
        for j1=1:(d(4)-1)
            for j2=1:(d(4)-1)
                tmp = zeros(d(1:2));
                for j3=1:d(4)
                    tmp = tmp + R(j3,j1)*wz1(:,:,j3,j2);
                end
                wz(:,:,j1,j2) = tmp;
            end
        end

        % First pull out the diagonal of the 2nd derivs
        for j1=1:d(4)-1
            W(:,:,z,j1) = wz(:,:,j1,j1);% + maxs*sqrt(eps('single'))*d(4)^2;
        end

        % Then pull out the off diagonal parts (note that matrices are symmetric)
        jj = d(4);
        for j1=1:d(4)-1
           for j2=(j1+1):(d(4)-1)
               W(:,:,z,jj) = wz(:,:,j2,j1);
               jj = jj+1;
           end
        end
    end

    % ss1 and ss2 are for examining how close the 1st derivatives are to zero.
    % At convergence, the derivatives from the likelihood term should match those
    % from the prior (regularisation) term.
    ss1 = sum(sum(sum(sum(gr.^2))));
    gr1 = spm_field('vel2mom',a,prm);        % 1st derivative of the prior term
    ll1 = 0.5*sum(sum(sum(sum(gr1.*a)))); % -ve log probability of the prior term
    gr  = gr + gr1;                       % Combine the derivatives of the two terms
    ss2 = sum(sum(sum(sum(gr.^2))));      % This should approach zero at convergence
    mx  = max(max(max(sum(gr.^2,4))));

    fprintf('%2d %8.4f %8.4f %8.4f %g\n', i, ll/prod(d(1:3)),ll1/prod(d(1:3)), (ll+ll1)/prod(d(1:3)), (ss2)/prod(d(1:3)));

    reg = double(0.01*sqrt(mx)*d(4));
   %reg = double(0.1*sqrt(ss2/prod(d(1:3))));
    a   = a - spm_field(W,gr,[prm(1:3) prm(4)+reg prm(5:6) rits]); % Gauss-Newton update

    if ss2/ss1<1e-4, break; end        % Converged?
end

sig = sftmax(a,R);
%________________________________________________________

%________________________________________________________
function sig = sftmax(a,R)
% Softmax function

d     = [size(a) 1 1 1];
sig   = zeros([d(1:3),d(4)+1],'single');
trunc = log(realmax('single')*(1-eps('single'))/(d(4)+1));

for j=1:size(a,3) % Loop over planes

    % Rotate the null-space back in to the data
    aj  = double(reshape(a(:,:,j,:),[d(1:2),d(4)]));
    sj  = zeros([d(1:2),d(4)+1]);
    for j1=1:d(4)+1
        sj(:,:,j1) = 0;
        for j2=1:d(4)
            sj(:,:,j1) = sj(:,:,j1) + R(j1,j2)*aj(:,:,j2);
        end
    end

    % Compute softmax
    sj = min(max(sj,-trunc),trunc);
    sj = exp(sj)+eps('single')*(d(4)+1);
    s  = sum(sj,3);
    for i=1:d(4)+1
        sig(:,:,j,i) = single(sj(:,:,i)./s);
    end

end
%________________________________________________________


function quatProd = quatMult(quat1, quat2)
%
% Multiplies two quaternions using the {i,j,k,1} convention
%
    if( size(quat1,1) ~= 4 || size(quat1,2) ~= 1 ...
        || size(quat2,1) ~= 4 || size(quat2,2) ~= 1 )
        error('Input quaternions must be 4x1');
    end

    quatProd = quatLeftComp(quat1) * quat2;
end


function p_sensor = directionalResponse(kgrid, sensor, sensor_mask_index, p_k)
%DIRECTIONALRESPONSE    Apply directivity to the sensor measurements.
%
% DESCRIPTION:
%       directionalResponse takes a pressure field in k-space, p_k,
%       multiplies by a directivity function, and outputs the grid points
%       indicated in sensor.mask.
%
% USAGE:
%       p_sensor = directionalResponse(kgrid, sensor, p_k)
%
% INPUTS:
%       kgrid       - k-Wave grid structure returned by makeGrid
%
%       sensor      - k-Wave sensor structure containing the following
%                     fields:
%
%           .directivity_angle
%                   - a matrix with the same structure as
%                     sensor.mask that allocates a directivity angle to
%                     each sensor element as defined in sensor.mask. The
%                     angles are in radians:
%                     0 = max sensitivity in y direction (up/down)
%                     pi/2 or -pi/2 = max sensitivity in x direction (left/right)
%
%       	.directivity_unique_angles
%                   - list of the unique directivity angles returned by
%                     sensor.directivity_unique_angles =
%                     unique(sensor.directivity_angle(sensor.mask == 1));
%
%       	.directivity_pattern
%                   - a text string with currently only one
%                     option. 'pressure' indicates that the directional
%                     response should be of the kind due to spatial
%                     averaging over a sensor surface, so a sinc function
%                     in 2D.
%
%       	.directivity_size
%                   - the directivity pattern used is what
%                     would be the directivity if the sensor were this
%                     length (width). The larger this is the more
%                     directional the response.
%
%       	.directivity_wavenumbers
%                   - this is set to [kgrid.ky(:)'; kgrid.kx(:)']. It is
%                     precomputed to allow data casting, as kgrid.kx (etc)
%                     are computed on the fly.
%
%       sensor_mask_index
%                   - indices of the active sensor elements in sensor.mask
%
%       p_k         - the acoustic pressure field in the k-space domain.
%
% Currently works for binary sensor_mask, but not when sensor_mask is given
% as Cartesian coordinates. Also, currently works only in 2D.
%
% ABOUT:
%       author: Ben Cox and Bradley Treeby
%       date: 21st January 2010
%       last update: 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% sensor mask indices
Ns = length(sensor_mask_index);

% pre-allocate p_sensor vector
p_sensor = zeros(Ns, 1, class(sensor.directivity_angle));

% loop over number of unique angles
for loop = 1:length(sensor.directivity_unique_angles);

   % get current angle
   theta = sensor.directivity_unique_angles(loop);

   % find which of the sensors have this directivity
   indices = find(sensor.directivity_angle(sensor_mask_index) == theta);

   switch sensor.directivity_pattern
      case 'pressure'
         % calculate magnitude of component of wavenumber along sensor face
         k_tangent = reshape([cos(theta) -sin(theta)]*sensor.directivity_wavenumbers, kgrid.Nx, kgrid.Ny);
         directionality = fftshift(sinc(k_tangent*sensor.directivity_size/2));
      case 'gradient'
         % calculate magnitude of component of wavenumber normal to the sensor face
         k_normal = reshape([sin(theta), cos(theta)]*[kgrid.ky(:)'; kgrid.kx(:)'], kgrid.Nx, kgrid.Ny);
         temp = k_normal./kgrid.k;
         temp(kgrid.k==0) = 0;
         directionality = fftshift(temp);
      otherwise
         error('Unsupported directivity pattern')
   end

   % apply the directivity response to the pressure field (in k-space)
   p_directivity = real(ifft2(p_k.*directionality));

   % pick out the response at the sensor points
   p_sensor(indices) = p_directivity(sensor_mask_index(indices));

end


function [T,P]=fnipals(X,w,T)

%FNIPALS nipals algorithm for PCA
%
% function [T,P]=fnipals(X,w,T)
%
% 'fnipals.m'
%
% This algorithm requires the presence of:
% 'missmean.m'
%
% ----------------------------------------------------
%        Find eigenvectors according to NIPALS
% ----------------------------------------------------
%
% [T,P]=fnipals(X,w,T);
% [T,P]=fnipals(X,w);
%
% T is found so that X = T*P', s.t ||T||=1 and T'T=I
%
% X        : The matrix to be decomposed.
% w        : Number of factors to extract.
%            If w is high (perhaps>20) consider using SVD.
% T        : Initial guess of the solution, optional.
%            If T is not specified, a little time will
%            be used on finding orthogonal random
%            starting values.
%
% You may want to calculate P afterwards by typing 'P=X*T'.
% Note that the T returned is orthonormal.
% Calculation of P is left of this implementation to save FLOP's.
% It handles missing values NaNs (very dispersed, less than 15%)
% If the problem is small enough you would prefer the SVD rather
% than NIPALS for finding T. NIPALS may be inaccurate when
% extracting too many factors, i.e., many more than the rank
% of X.

%scalar ConvLim WarnLim ItMax a b i

% $ Version 1.01 $ Date 18. June 1998 $ Not compiled $

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program 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 2 of the License, or (at your option) any later version.
%
% This program 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
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

ConvLim=1e-12;
WarnLim=1e-4;
ConvLimMiss=100*ConvLim;
ItMax=100;

filename='fnipals.m';

[a b]=size(X);

if (w>a | w>b) | w<1,
    help(filename);
    error(['Error in ' filename ': Number of factors to extract is invalid!'])
end;

np=isnan(X);
MissingExist=any(np);

if ~exist('T'),
    T=orth(randn(a,w));
end;

if exist('P'),
    P=[];
end;

if ~MissingExist
    if (size(T) == [a w]),
        if a>b,
            P=X'*T;
            l2=Inf;
            Z=X'*X;
            for i=1:w,
                p=P(:,i);
                d=1;
                it=0;
                while (d>ConvLim) & (it<ItMax),
                    it=it+1;
                    p=Z*p;
                    l1=sqrt(p'*p);
                    p=p/l1;
                    d=(l1-l2)^2;
                    l2=l1;
                end;
                P(:,i)=sqrt(l1)*p;
                Z=Z-P(:,i)*P(:,i)';
                WarnLim=sqrt(l1)/1000;
                if it>=ItMax & d>WarnLim,
                    disp('FNIPALS, High-X: Iterated up to the ItMax limit!')
                    disp('FNIPALS, High-X: The solution has not converged!')
                end;
            end;
            T=X*P;
        else
            P=[];
            l2=Inf;
            Z=X*X';
            for i=1:w,
                t=T(:,i);
                d=1;
                it=0;
                while (d>ConvLim) & (it<ItMax),
                    it=it+1;
                    t=Z*t;
                    l1=sqrt(t'*t);
                    t=t/l1;
                    d=(l1-l2).^2;
                    l2=l1;
                end;
                T(:,i)=sqrt(l1)*t;
                Z=Z-T(:,i)*T(:,i)';
                WarnLim=sqrt(l1)/1000;
                if it>=ItMax & d>WarnLim,
                    disp('FNIPALS, Wide-X: Iterated up to the ItMax limit!')
                    disp('FNIPALS, Wide-X: The solution has not converged!')
                end;
            end;
        end;
        T=gsm(T);
    else
        error(['Error in ' filename ': Number of factors to extract is invalid!'])
    end;
else
    MissIdx=find(np);
    [i j]=find(np);
    mnx=missmean(X)/2;
    mny=missmean(X')/2;
    n=size(i,1);
    for k=1:n,
        i_i=i(k);
        j_j=j(k);
        X(i_i,j_j) = mny(i_i) + mnx(j_j);
    end;
    mnz=(missmean(mnx)+missmean(mny))/2;

    ssmisold=sum(sum( X(MissIdx).^2 ));
    sstotold=sum(sum( X.^2 ));
    ssrealold=sstotold-ssmisold;
    iterate=1;
    while iterate

        if (size(T) == [a w]),
            if a>b,
                P=X'*T;
                l2=Inf;
                Z=X'*X;
                for i=1:w,
                    p=P(:,i);
                    d=1;
                    it=0;
                    while (d>ConvLim) & (it<ItMax),
                        it=it+1;
                        p=Z*p;
                        l1=sqrt(p'*p);
                        p=p/l1;
                        d=(l1-l2)^2;
                        l2=l1;
                    end;
                    P(:,i)=sqrt(l1)*p;
                    Z=Z-P(:,i)*P(:,i)';
                    WarnLim=sqrt(l1)/1000;
                    if it>=ItMax & d>WarnLim,
                        disp('FNIPALS, High-X: Iterated up to the ItMax limit!')
                        disp('FNIPALS, High-X: The solution has not converged!')
                    end;
                end;
                T=X*P;
            else
                P=[];
                l2=Inf;
                Z=X*X';
                for i=1:w,
                    t=T(:,i);
                    d=1;
                    it=0;
                    while (d>ConvLim) & (it<ItMax),
                        it=it+1;
                        t=Z*t;
                        l1=sqrt(t'*t);
                        t=t/l1;
                        d=(l1-l2).^2;
                        l2=l1;
                    end;
                    T(:,i)=sqrt(l1)*t;
                    Z=Z-T(:,i)*T(:,i)';
                    WarnLim=sqrt(l1)/1000;
                    if it>=ItMax & d>WarnLim,
                        disp('FNIPALS, Wide-X: Iterated up to the ItMax limit!')
                        disp('FNIPALS, Wide-X: The solution has not converged!')
                    end;
                end;
            end;
            T=gsm(T);
        else
            error(['Error in ' filename ': Number of factors to extract is invalid!'])
        end;

        P=X'*T;
        Xm=T*P';
        X(MissIdx)=Xm(MissIdx);
        ssmis=sum(sum( Xm(MissIdx).^2 ));
        sstot=sum(sum( X.^2 ));
        ssreal=sstot-ssmis;
        if abs(ssreal-ssrealold)<ConvLim*ssrealold & abs(ssmis-ssmisold)<ConvLimMiss*ssmisold,
            iterate=0;
        end;
        ssrealold=ssreal;
        ssmisold=ssmis;
    end;
end;
T=gsm(T);


function [varargout]=subQuadCatmullClark(varargin)

% function [Fs,Vs,Cs,CV]=subQuadCatmullClark(F,V,n,fixBoundaryOpt)
% ------------------------------------------------------------------------
%
%
%
% Change log:
% 2022/04/28 Minor array (Vn) allocation improvement
% ------------------------------------------------------------------------

%% parse input

switch nargin
    case 2
        F=varargin{1};
        V=varargin{2};
        n=1;
        fixBoundaryOpt=0;
    case 3
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=0;
    case 4
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=varargin{4};
end

C=(1:1:size(F,1))'; %Face colors or indices

%%

if n>0
    for qIter=1:1:n
        M=patchConnectivity(F,V,{'ev','ef','vf','ve','vv'});

        edgeVertexMat=M.edge.vertex;
        edgeFaceMat=M.edge.face;

        vertexFaceMat=M.vertex.face;
        vertexEdgeMat=M.vertex.edge;
        vertexVertexMat=M.vertex.vertex;

        numPoints = size(V,1);
        numEdges = size(edgeVertexMat,1);

        % Get indices of the three edges associated with each face
        A = sparse(edgeVertexMat(:,1),edgeVertexMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
        A = max(A,A'); %Copy symmetric

        %Indices for A matrix
        indA_12=F(:,1)+(F(:,2)-1)*numPoints;
        indA_23=F(:,2)+(F(:,3)-1)*numPoints;
        indA_34=F(:,3)+(F(:,4)-1)*numPoints;
        indA_41=F(:,4)+(F(:,1)-1)*numPoints;

        %Get indices for vertex array
        indV_12=full(A(indA_12));
        indV_23=full(A(indA_23));
        indV_34=full(A(indA_34));
        indV_41=full(A(indA_41));

        indV_mid=(1:1:size(F,1))'+numPoints+size(edgeVertexMat,1);

        %Create faces array
        Fs=[[F(:,1)  indV_12 indV_mid indV_41];...
            [indV_12 F(:,2)  indV_23 indV_mid];...
            [indV_mid indV_23 F(:,3)  indV_34 ];...
            [indV_41 indV_mid indV_34 F(:,4) ]];

        %Face centre points
        Vm=patchCentre(F,V);

        %Create vertex arrays

        Vne=(V(edgeVertexMat(:,1),:)+ V(edgeVertexMat(:,2),:))/2;

        if size(edgeFaceMat,2)==1
            logicBoundaryEdges=true(size(edgeFaceMat,1),1);
        else
            logicBoundaryEdges=edgeFaceMat(:,2)==0;
        end

        V_F_mean=zeros(size(V));
        logicValid=vertexFaceMat>0;
        X=nan(size(vertexFaceMat));
        for q=1:1:size(V,2)
            X(logicValid)=Vm(vertexFaceMat(logicValid),q);
            V_F_mean(:,q)=mean(X,2,'omitnan');
        end

        V_E_mean=zeros(size(V));
        logicValid=vertexEdgeMat>0;
        X=nan(size(vertexEdgeMat));
        for q=1:1:size(V,2)
            X(logicValid)=Vne(vertexEdgeMat(logicValid),q);
            V_E_mean(:,q)=mean(X,2,'omitnan');
        end

        N=sum(logicValid,2);
        Vv=(V_F_mean+2*V_E_mean+(N-3).*V)./N;

        Vn=zeros(numEdges,size(V,2));
        if nnz(logicBoundaryEdges)>0
            %Use normal mid-edge nodes for boundary edges
            Vn(logicBoundaryEdges,:)=Vne(logicBoundaryEdges,:);
            indBoundaryVertices=unique(edgeVertexMat(logicBoundaryEdges,:));
            if fixBoundaryOpt==1
                %Replace boundary nodes with original
                Vv(indBoundaryVertices,:)=V(indBoundaryVertices,:);
            else
                vvm=vertexVertexMat(indBoundaryVertices,:);
                logicCheck=~ismember(vvm,indBoundaryVertices);
                vvm(logicCheck)=0;
                vvm=sort(vvm,2,'descend');
                vvm=vvm(:,[1 2]);

                Vv(indBoundaryVertices,:)= 6/8*V(indBoundaryVertices,:) + 1/8*(V(vvm(:,1),:)+V(vvm(:,2),:));
            end

        end

        Vn(~logicBoundaryEdges,:)=(V(edgeVertexMat(~logicBoundaryEdges,1),:)+V(edgeVertexMat(~logicBoundaryEdges,2),:)+...
            Vm(edgeFaceMat(~logicBoundaryEdges,1),:) + Vm(edgeFaceMat(~logicBoundaryEdges,2),:) )/4;

        Vs = [Vv; Vn; Vm]; %Join point sets

        if qIter>1
            CV=[CV; 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
        else
            CV=[zeros(size(V,1),1); 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
        end

    %Override input for looping
    C=repmat(C,[size(Fs,1)/size(F,1),1]);
    F=Fs;
    V=Vs;
    end

else
    CV=zeros(size(V,1),1);
end

varargout{1}=F;
varargout{2}=V;
varargout{3}=C;
varargout{4}=CV;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program 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.
%
% This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.


function [ahm, AHM_rf, AHM_sf, AHM_sk, AHM_sc, AHM_u, AHM_rho] = ...
    retroProjAhmPntFromPinHoleOnRob(Rf, Sf, Sk, Sc, u, n)

% RETROPROJAHMPNTFROMPINHOLEONROB Retro-project ahm from pinhole on robot.
%
%   AHM = RETROPROJAHMPNTFROMPINHOLEONROB(RF, SF, SK, SC, U, N) gives the
%   retroprojected AHM in World Frame from an observed pixel U. RF and SF
%   are Robot and Sensor Frames, SK and SD are camera calibration and
%   distortion correction parameters. U is the pixel coordinate and N is
%   the non-observable inverse depth. AHM is a 7-vector :
%     AHM = [X Y Z U V W IDepth]'
%
%   [AHM, AHM_rf, AHM_sf, AHM_k, AHM_c, AHM_u, AHM_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, U and N.
%
%   See also INVPINHOLEAHM, FROMFRAMEAHM.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% Frame World -> Robot  :  Rf
% Frame Robot -> Sensor :  Sf

if(isempty(Sc))
    % AHM in Sensor Frame
    [ahms, AHMS_u, AHMS_rho, AHMS_sk] = invPinHoleAhm(u,n,Sk) ;
else
    % AHM in Sensor Frame
    [ahms, AHMS_u, AHMS_rho, AHMS_sk, AHMS_sc] = invPinHoleAhm(u,n,Sk,Sc) ;
end

[ahmr, AHMR_sf, AHMR_ahms] = fromFrameAhm(Sf,ahms);
[ahm , AHM_rf , AHM_ahmr]  = fromFrameAhm(Rf,ahmr);

AHM_ahms = AHM_ahmr*AHMR_ahms;
AHM_sk   = AHM_ahms*AHMS_sk ;
AHM_sf   = AHM_ahmr*AHMR_sf;

if(isempty(Sc))
    AHM_sc = [] ;
else
    AHM_sc = AHM_ahms*AHMS_sc ;
end

AHM_u = AHM_ahms*AHMS_u ;
AHM_rho = AHM_ahms*AHMS_rho ;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE