function [model, varargout] = grlvq_core(trainSet, trainLab, varargin)
%grlvq_core.m - trains the Generalized Relevance LVQ algorithm
%NOTE: minimal requirement version 7.4.0.336 (R2007a)
%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  %% Use the wrapper GRLVQ.M to access the functionality in style of %%
%  %% the SOM Toolbox (i.e. with data structs).                       %%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GRLVQ_model=GRLVQ_train(trainSet,trainLab); % minimal parameters required
%  estimatedTrainLabels = GRLVQ_classify(trainSet, GRLVQ_model);
%  trainError = mean( trainLab ~= estimatedTrainLabels );
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : vector with the labels of the training set
% optional parameters:
%  PrototypesPerClass: (default=1) the number of prototypes per class used. This could
%  be a number or a vector with the number for each class
%  initialPrototypes : (default=[]) a set of prototypes to start with. If not given initialization near the class means
%  initialRelevances : the relevances to start with. If not given all
%  relevances are equal at start
%  regularization    : (default=0) values usually between 0 and 1 treat with care.
%  Regularizes the relevances to be more homogeneous
%  testSet           : (default=[]) an optional test set used to compute
%  the test error. The last column is expected to be a label vector
%  comparable        : (default=0) a flag which resets the random generator
%  to produce comparable results if set to 1
%  optimization      : (default=fminlbfgs) indicates which optimization is used: sgd or fminlbfgs
% parameter for the stochastic gradient descent sgd
%  nb_epochs             : (default=100) the number of epochs for sgd
%  learningRatePrototypes: (default=[]) the learning rate for the prototypes.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  learningRateRelevances: (default=[]) the learning rate for the matrix.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  relevanceStart        : (default=1) the epoch to start the matrix training
% parameter for the build-in function fminlbfgs
%  threshstop       : (default=0) the training error for early stopping
%  useEarlyStopping : (default=1) use early stopping based on threshstop
%  Display          : (default='off') the optimization output 'iter' or 'off'
%  GradObj          : (default=on) use the gradient information or not
%  HessUpdate       : (default=lbfgs) the update can be 'lbfgs', 'bfgs' or 'steepdesc'
%  TolFun           : (default=1e-6) the tolerance
%  MaxIter          : (default=2500) the maximal number of iterations
%  MaxFunEvals      : (default=1000000) the maximal number of function evaluations
%  TolX             : (default=1e-10) tolerance
%  DiffMinChange    : (default=1e-10) minimal change
%
% output: the GRLVQ model with prototypes w their labels c_w and the relevances lamda
%  optional output:
%  initialization : a struct containing the settings
%  trainError     : error in the training set
%  testError      : error in the training set (only computed if 'testSet' is given)
%  costs          : the output of the cost function
%
% Citation information:
% B. Hammer and T. Villmann, Generalized relevance learning vector quantization, Neural Networks 15, 1059-1068, 2002.
%
% P. Schneider, K. Bunte, B. Hammer and M. Biehl, Regularization in Matrix Relevance Learning,
% IEEE Transactions on Neural Networks, vol. 21, nb. 5, pp. 831-840, 2010.
%
% Kerstin Bunte
% uses the Fast Limited Memory Optimizer fminlbfgs.m written by Dirk-Jan Kroon available at the MATLAB central
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% 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!
%

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Kerstin Bunte
% http://www.cis.hut.fi/projects/somtoolbox/

nout = max(nargout,1)-1;
p = inputParser;   % Create an instance of the class.
p.addRequired('trainSet', @isfloat);
p.addRequired('trainLab', @(x) length(x)==size(trainSet,1) & isnumeric(x));

p.addOptional('testSet', [], @(x)(size(x,2)-1)==size(trainSet,2) & isfloat(x));
p.addParamValue('PrototypesPerClass', ones(1,length(unique(trainLab))), @(x)(sum(~(x-floor(x)))/length(x)==1 && (length(x)==length(unique(trainLab)) || length(x)==1)));
p.addParamValue('initialPrototypes',[], @(x)(size(x,2)-1==size(trainSet,2) && isfloat(x)));
p.addParamValue('initialRelevances',[], @(x)(numel(x)==size(trainSet,2) && isfloat(x)));
p.addOptional('comparable', 0, @(x)(~(x-floor(x))));
p.addParamValue('regularization',0, @(x)(isfloat(x) && x>=0));
p.addOptional('optimization', 'fminlbfgs', @(x)any(strcmpi(x,{'sgd','fminlbfgs'})));
% parameter for the stochastic gradient descent
p.addOptional('nb_epochs', 100, @(x)(~(x-floor(x))));
p.addParamValue('learningRatePrototypes', [], @(x)(isfloat(x) || isa(x,'function_handle')));
p.addParamValue('learningRateRelevances', [], @(x)(isfloat(x) || isa(x,'function_handle')));
p.addOptional('relevanceStart', 1, @(x)(~(x-floor(x))));
% parameter for the build-in function
p.addOptional('threshstop',0,@(x) isfloat(x));
p.addOptional('useEarlyStopping',1,@(x)(~(x-floor(x))));
p.addOptional('Display', 'off', @(x)any(strcmpi(x,{'iter','off'})));
p.addOptional('GradObj', 'on', @(x)any(strcmpi(x,{'on','off'})));
p.addOptional('HessUpdate', 'lbfgs', @(x)any(strcmpi(x,{'lbfgs', 'bfgs', 'steepdesc'})));
p.addOptional('TolFun',1e-6,@(x) isfloat(x));
p.addOptional('MaxIter', 2500, @(x)(~(x-floor(x))));
p.addOptional('MaxFunEvals', 1000000, @(x)(~(x-floor(x))));
p.addOptional('TolX',1e-10,@(x) isfloat(x));
p.addOptional('DiffMinChange',1e-10,@(x)isfloat(x));
p.CaseSensitive = true;
p.FunctionName = 'GRLVQ';
% Parse and validate all input arguments.
p.parse(trainSet, trainLab, varargin{:});

%%% check if results should be comparable
if p.Results.comparable,
    rng('default');
end
%%% set useful variables
nb_samples = size(trainSet,1);
nb_features = size(trainSet,2);
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

classes = unique(trainLab);
nb_classes = length(classes);
testSet = p.Results.testSet;

% global regularization;
regularization = p.Results.regularization;
if regularization, disp(['Regularize the relevances with ',num2str(regularization)]);end

initialization = rmfield(p.Results, 'trainSet');
initialization.trainSet = [num2str(nb_samples),'x',num2str(nb_features),' matrix'];
initialization = rmfield(initialization, 'trainLab');
initialization.trainLab = ['vector of length ',num2str(length(trainLab))];
if ~isempty(testSet)
    initialization = rmfield(initialization, 'testSet');
    initialization.testSet = [num2str(size(testSet,1)),'x',num2str(size(testSet,2)),' matrix'];
end
switch(p.Results.optimization)
case{'sgd'}
    initialization = rmfield(initialization, 'useEarlyStopping');
    initialization = rmfield(initialization, 'Display');
    initialization = rmfield(initialization, 'GradObj');
    initialization = rmfield(initialization, 'HessUpdate');
    initialization = rmfield(initialization, 'TolFun');
    initialization = rmfield(initialization, 'MaxIter');
    initialization = rmfield(initialization, 'MaxFunEvals');
    initialization = rmfield(initialization, 'TolX');
    initialization = rmfield(initialization, 'DiffMinChange');
case{'fminlbfgs'}
    disp('The fminlbfgs optimization uses some global variables:');
    disp('threshstop earlystopped useEarlyStopping');
    initialization = rmfield(initialization, 'nb_epochs');
    initialization = rmfield(initialization, 'learningRatePrototypes');
    initialization = rmfield(initialization, 'learningRateRelevances');
    initialization = rmfield(initialization, 'relevanceStart');
end
% Display all arguments.
%disp 'Settings for GRLVQ:'
%disp(initialization);

%%% check the number of prototypes per class if one integer is given and turn
%%% it into a vector
nb_ppc = p.Results.PrototypesPerClass;
if length(nb_ppc)~=nb_classes,
    nb_ppc = ones(1,nb_classes)*nb_ppc;
end

%%% initialize the prototypes
if isempty(p.Results.initialPrototypes)
    % initialize near the class centers
    w = zeros(sum(nb_ppc),nb_features);
    c_w = zeros(sum(nb_ppc),1);
    actPos = 1;
    for actClass=1:nb_classes
        nb_prot_c = nb_ppc(actClass);
        classMean = mean(trainSet(trainLab==classes(actClass),:));
        % set the prototypes to the class mean and add a random variation
        % between -0.1 and 0.1
        w(actPos:actPos+nb_prot_c-1,:) = classMean(ones(nb_prot_c,1),:)+(rand(nb_prot_c,nb_features)*2-ones(nb_prot_c,nb_features))/10;
        c_w(actPos:actPos+nb_prot_c-1) = classes(actClass);
        actPos = actPos+nb_prot_c;
    end
else
    % initialize with given w
    w = p.Results.initialPrototypes(:,1:end-1);
    c_w = p.Results.initialPrototypes(:,end);
end
%%% initialize the matrix
if isempty(p.Results.initialRelevances)
    lambda = ones(1,nb_features);
else
%     if isvector(p.Results.initialRelevances)
%         omega = diag(p.Results.initialRelevances);
%     else
    lambda = p.Results.initialRelevances;
%     end
end
% normalize the relevances
lambda = lambda / sum(lambda);
model = struct('w',w,'c_w',c_w,'lambda',lambda);
clear w c_w lambda;

dim = length(model.lambda);

switch(p.Results.optimization)
case{'sgd'}
    %%% gradient descent variables
    nb_epochs = p.Results.nb_epochs;
    relevanceStart = p.Results.relevanceStart;
    % compute the vector of nb_epochs learning rates alpha for the prototype learning
    if isa(p.Results.learningRatePrototypes,'function_handle')
        % with a given function specified from the user
        alphas = arrayfun(p.Results.learningRatePrototypes, 1:nb_epochs);
    elseif length(p.Results.learningRatePrototypes)>2
        if length(p.Results.learningRatePrototypes)==nb_epochs
            alphas = p.Results.learningRatePrototypes;
        else
            disp('The learning rate vector for the prototypes does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRatePrototypes)
            initialization.learningRatePrototypes = [nb_features/100, nb_features/10000];
        end
        alpha_start = initialization.learningRatePrototypes(1);
        alpha_end = initialization.learningRatePrototypes(2);
        alphas = arrayfun(@(x) alpha_start * (alpha_end/alpha_start)^(x/nb_epochs), 1:nb_epochs);
    %     alphas = arrayfun(@(x) alpha_start / (1+(x-1)*alpha_end), 1:nb_epochs);
    end
    % compute the vector of nb_epochs learning rates epsilon for the Matrix learning
    epsilons = zeros(1,nb_epochs);
    if isa(p.Results.learningRateRelevances,'function_handle')
        % with a given function specified from the user
    % 	epsilons = arrayfun(p.Results.learningRateRelevances, 1:nb_epochs);
        epsilons(relevanceStart:nb_epochs) = arrayfun(p.Results.learningRateRelevances, relevanceStart:nb_epochs);
    elseif length(p.Results.learningRateRelevances)>2
        if length(p.Results.learningRateRelevances)==nb_epochs
            epsilons = p.Results.learningRateRelevances;
        else
            disp('The learning rate vector for the Matrix does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRateRelevances)
            initialization.learningRateRelevances = [nb_features/1000, nb_features/100000];
        end
        eps_start = initialization.learningRateRelevances(1);
        eps_end = initialization.learningRateRelevances(2);
    %     epsilons = arrayfun(@(x) eps_start * (eps_end/eps_start)^(x/nb_epochs), 1:nb_epochs);
        epsilons(relevanceStart:nb_epochs) = arrayfun(@(x) eps_start * (eps_end/eps_start)^((x-relevanceStart)/(nb_epochs-relevanceStart)), relevanceStart:nb_epochs);
    end

    %%% initialize requested outputs
    trainError = [];
    costs = [];
    testError = [];
    if nout>=2,
        % train error requested
        trainError = ones(1,nb_epochs+1);
        estimatedLabels = GRLVQ_classify(trainSet, model); % error after initialization
        trainError(1) = sum( trainLab ~= estimatedLabels )/nb_samples;
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                testError = ones(1,nb_epochs+1);
                estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError(1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
            end
            if nout>=4,
                % costs requested
                disp('The computation of the costs is an expensive operation, do it only if you really need it!');
                costs(1) = GRLVQ_costfun(trainSet, trainLab, model, regularization);
            end
        end
    end

    %%% optimize with stochastic gradient descent
    for epoch=1:nb_epochs
        if mod(epoch,100)==0, disp(epoch); end
        % generate order to sweep through the trainingset
        order = randperm(nb_samples);

        % perform one sweep through trainingset
        for i=1:nb_samples
            % select one training sample randomly
            xi = trainSet(order(i),:);
            c_xi = trainLab(order(i));
%             dist = ((xi(ones(size(model.w,1),1),:))-model.w)*model.omega'*(model.omega*((xi(ones(size(model.w,1),1),:))-model.w)');
%             dist = diag(dist);
            dist = sum(bsxfun(@times,bsxfun(@minus,xi,model.w).^2,model.lambda), 2);
            % determine the two winning prototypes
            % nearest prototype with the same class
            [sortDist,sortIdx] = sort(dist);
            count = 1;
            J = sortIdx(count);
            while model.c_w(sortIdx(count)) ~= c_xi,
                count = count+1;
                J = sortIdx(count);
            end
            dJ = sortDist(count);
            count = 1;
            K = sortIdx(count);
            while model.c_w(sortIdx(count)) == c_xi,
                count = count+1;
                K = sortIdx(count);
            end
            dK = sortDist(count);

            wJ = model.w(J,:);
            wK = model.w(K,:);
            % prototype update
            norm_factor = (dJ + dK)^2;
            DJ = (xi-wJ);
            DK = (xi-wK);

            dwJ = (2*dK/norm_factor)*2*model.lambda.*DJ;
            dwK = (2*dJ/norm_factor)*2*model.lambda.*DK;

            model.w(J,:) = wJ + alphas(epoch) * dwJ;
            model.w(K,:) = wK - alphas(epoch) * dwK;

            % update relevances
            if epsilons(epoch)>0, % epoch >= RelevanceStart
%                 f1 = (2*dK/norm_factor)*2*(model.omega*DJ')*DJ;
%                 f2 = (2*dJ/norm_factor)*2*(model.omega*DK')*DK;
                f1 = (2*dK/norm_factor)*DJ.^2;
                f2 = (2*dJ/norm_factor)*DK.^2;
                % update lambda
                if regularization,
                    f3 = diag(pinv(sqrt(diag(model.lambda))))';
%                     test = bsxfun(@times,model.lambda,ones(1,length(model.lambda))');
%                     test(1:nb_features+1:nb_features*nb_features) = 1;
%                     f3 = 1/(prod(model.lambda)+eps)*prod(test,2)';
                else
                    f3 = 0;
                end
                model.lambda = model.lambda-epsilons(epoch) * (f1-f2  - regularization * f3);
                % normalization
                model.lambda (model.lambda < 0) = 0;
                model.lambda = model.lambda / sum(model.lambda);
            end
        end
        if nout>=2,
            % train error requested
            estimatedLabels = GRLVQ_classify(trainSet, model); % error after epoch
            trainError(epoch+1) = sum( trainLab ~= estimatedLabels )/nb_samples;
            if nout>=3,
                % test error requested
                if ~isempty(testSet)
                    estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                    testError(epoch+1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
                end
                if nout>=4,
                    % costs requested
%                     dist = computeDistance(trainSet, model.w, model);
%                     if regularization,
%                         regTerm = regularization * log(det(model.omega*model.omega'));
%                     else
%                         regTerm = 0;
%                     end
%                     costs(epoch+1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                                       min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
                    costs(epoch+1) = GRLVQ_costfun(trainSet, trainLab, model, regularization);
                end
            end
        end
    end
case{'fminlbfgs'}
    %%% optimization options
    options = struct( ...
      'Display',p.Results.Display, ...
      'GradObj',p.Results.GradObj, ...
      'GradConstr',false, ...
      'GoalsExactAchieve',0, ...
      'TolFun',p.Results.TolFun, ...
      'MaxIter',p.Results.MaxIter, ...
      'MaxFunEvals', p.Results.MaxFunEvals, ...
      'TolX',p.Results.TolX, ...
      'DiffMinChange',p.Results.DiffMinChange, ...
      'OutputFcn','LVQ_progresser', ...
      'HessUpdate',p.Results.HessUpdate ...
    );
    clear('progresser'); % memory therein might need reset
    global threshstop earlystopped useEarlyStopping % for LVQ_progresser.m datval labval n_vec
    useEarlyStopping = p.Results.useEarlyStopping; % use early stopping
%     useEarlyStopping = []; % if empty, no validation set given/used for early stopping
    earlystopped = false;
    threshstop = p.Results.threshstop; % stop if classification below this threshold for early stopping
%     global training_data training_label LabelEqualsPrototype LRrelevances LRprototypes prototypeLabel;
%     training_data = trainSet;
%     training_label = trainLab;
%     prototypeLabel = model.c_w;
    nb_prototypes = numel(model.c_w);
%     nb_samples = size(training_data,1);
%     LabelEqualsPrototype = trainLab*ones(1,nb_prototypes) == (model.c_w*ones(1,nb_samples))';
    LabelEqualsPrototype = bsxfun(@eq,trainLab,model.c_w');
    earlystopped = false; % don't change, assigned in progresser.m
    clear('progresser'); % memory therein might need reset
    newfval = realmax('single');
    % fminlbfgs optimizer courtesy of Dirk-Jan Kroon:
    % http://www.mathworks.de/matlabcentral/fileexchange/23245
    % early stopping to be implemented in progresser function
    variables = zeros(nb_prototypes+nb_features,size(trainSet,2));
    variables(1:nb_prototypes,:) = model.w;
    variables(nb_prototypes+1:end,:) = diag(sqrt(model.lambda));
    LRprototypes = 1; % learn prototype locations
    LRrelevances = 0; % don't learn metric
%     [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    LRprototypes = 0; % don't learn prototype locations
    LRrelevances = 1; % learn metric
%     [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    if not(isempty(fval))
      clear('progresser'); % memory therein might need reset
      LRprototypes = 1;
      for i = 1:100  % depending on data, re-iterations might further improve
%         [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
        [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
        if not(isempty(fval))
          if abs(fval - newfval) < 1e-3
            newfval = fval;
            break
          end
          newfval = fval;
        end
        if isempty(fval) || earlystopped
          break
        end
      end
    end
    model.w = variables(1:nb_prototypes,:);
    model.lambda = diag(variables(nb_prototypes+1:end,:)'*variables(nb_prototypes+1:end,:))';
%     model.lambda = model.lambda/sum(model.lambda);
    if nout>=2,
        % train error requested
        estimatedLabels = GRLVQ_classify(trainSet, model); % error after initialization
        trainError = mean( trainLab ~= estimatedLabels );
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError = mean( testSet(:,end) ~= estimatedLabels );
            end
            if nout>=4,
                % costs requested
%                 dist = computeDistance(trainSet, model.w, model);
%                 if regularization,
%                     regTerm = regularization * log(det(model.omega*model.omega'));
%                 else
%                     regTerm = 0;
%                 end
%                 costs = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                          min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
                costs = GRLVQ_costfun(trainSet, trainLab, model, regularization);
            end
        end
    end
end

%%% output of the training
varargout = cell(nout);
for k=1:nout
	switch(k)
		case(1)
			varargout(k) = {initialization};
		case(2)
			varargout(k) = {trainError};
		case(3)
			varargout(k) = {testError};
		case(4)
            varargout(k) = {costs};
	end
end

function cost = GRLVQ_costfun(trainSet, trainLab, model, regularization)
%GRLVQ_costfun.m - computes the costs for a given training set and GRLVQ
%model with or without regularization
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  costs = GMLVQ_costfun(trainSet, trainLab, GMLVQ_model, 0);
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : a vector of training labels
%  model    : GMLVQ model with prototypes w their labels c_w and the matrix omega
%  regularization: the factor>=0 for the regularization
%
% output    : cost function value
%
% Kerstin Bunte (based on the code from Marc Strickert)
% kerstin.bunte@googlemail.com
% Mon Nov 05 09:05:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
nb_samples = length(trainLab);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

% LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
LabelEqPrototype = bsxfun(@eq,trainLab,model.c_w');
dists = computeDistance(trainSet, model.w, model);
if regularization,
    regTerm = regularization * log(prod(model.lambda));
% if strcmp(p.Results.optimization,'sgd')
else
    regTerm = 0;
end
Dwrong = dists;
Dwrong(LabelEqPrototype) = realmax(class(Dwrong));   % set correct labels impossible
distwrong = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
distcorrect = min(Dcorrect.'); % closest correct
clear Dcorrect;
clear dists;
distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
if regularization,
    regTerm = regularization * log(prod(model.lambda));
else
    regTerm = 0;
end
cost = sum(mu)-regTerm;

function [f G]  = GMLVQ_optfun(variables,training_data,LabelEqualsPrototype,LRrelevances,LRprototypes,prototypeLabel,regularization)
% [f G] = GMLVQ_optfun(variables)
% function to be optimzed by matrix relevance learning vector quantization
% variables = [prototype matrix;omega matrix]
% global variables are
%   training_data        : data vectors as row vectors, i.e. attributes in columns
%   LabelEqualsPrototype : binary matrix indicating coocurrences of
%   training labels and prototype labels
%   prototypeLabel       : label vector for the prototypes
%   regularization       : the regularization parameter
%   LRrelevances         : learning rate for the relevance matrix
%   LRprototypes         : learning rate for the prototypes
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/)
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
if isempty(LRprototypes) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRprototypes = 1; % no relevance learning by default
end
if isempty(LRrelevances) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRrelevances = 0; % no relevance learning by default
end
[n_data, n_dim] = size(training_data);
nb_prototypes =  numel(prototypeLabel);
omegaT = variables(nb_prototypes+1:end,:)';
n_vec = size(variables,1) - nb_prototypes;

dists = squaredEuclidean(training_data*omegaT, variables(1:nb_prototypes,:)*omegaT);

Dwrong = dists;
Dwrong(LabelEqualsPrototype) = realmax(class(Dwrong));   % set correct labels impossible
[distwrong pidxwrong] = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqualsPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
[distcorrect pidxcorrect] = min(Dcorrect.'); % closest correct
clear Dcorrect;

distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
% callitq = 1./(1 + exp(-squashsigmoid * mu)); % apply sigmoidal

if regularization,
    regTerm = regularization * log(det(omegaT'*omegaT));
else
    regTerm = 0;
end
f = sum(mu)-regTerm;
% f = mean(callitq);

if nargout > 1  % gradient needed not just function eval
    G = zeros(size(variables)); % initially no gradient
    %       callitq = squashsigmoid * callitq .* (1-callitq); % derivative of sigmoid
    %       distcorrectpluswrong = 2 * callitq ./ distcorrectpluswrong.^2; % degeneration?
    distcorrectpluswrong = 4 ./ distcorrectpluswrong.^2; % norm_factor for derivative for every data sample
    if LRrelevances > 0
        Gw = zeros(n_vec,n_dim);
    end
    for k=1:nb_prototypes%(n_vec+1):size(lambda,1) % update all prototypes
        idxc = (k == pidxcorrect);  % Js: idxs where actual prototype is nearest correct
        idxw = (k == pidxwrong);    % Ks: idxs where actual prototype is nearest wrong

        dcd =  distcorrect(idxw) .* distcorrectpluswrong(idxw);
        dwd =    distwrong(idxc) .* distcorrectpluswrong(idxc);
        if LRrelevances > 0
            % part of derivative of distance
            difc = bsxfun(@minus,training_data(idxc,:),variables(k,:)); % DJs
            difw = bsxfun(@minus,training_data(idxw,:),variables(k,:)); % DKs
            % update omega
            Gw = Gw - (bsxfun(@times,difw,dcd.') * omegaT).' * difw + ...
                      (bsxfun(@times,difc,dwd.') * omegaT).' * difc;
            if LRprototypes > 0
                G(k,:) = dcd * difw - dwd * difc;
            end
        else
            if LRprototypes > 0
                G(k,:) = dcd * training_data(idxw,:) - dwd * training_data(idxc,:) + (sum(dwd)-sum(dcd)) * variables(k,:);
            end
        end
    end
if regularization,
    f3 = (pinv(omegaT'))';
else
    f3 = 0;
end
    % some rescalings needed
    if LRrelevances > 0
        G(nb_prototypes+1:nb_prototypes+n_vec,:) = 2/n_data * LRrelevances * Gw - regularization*f3;
    end
    if LRprototypes > 0
        G(1:nb_prototypes,:) = 1./n_data * LRprototypes * G(1:nb_prototypes,:) * omegaT * omegaT.';
    end
    G = G .* (1 + .0001 * (rand(size(G))-.5)); % help break symmetries
end
% if 0,
% w = variables(1:nb_prototypes,:);
% dJs = zeros(1,nb_samples);
% dKs = zeros(1,nb_samples);
% Js = zeros(1,nb_samples);
% Ks = zeros(1,nb_samples);
% norm_factors = zeros(1,nb_samples);
% DJs = zeros(nb_samples,size(training_data,2));
% DKs = zeros(nb_samples,size(training_data,2));
% for i=1:nb_samples
%     % select one training sample randomly
%     xi = training_data(i,:);
%     c_xi = trainLab(i);
%
%     dist = ((xi(ones(size(w,1),1),:))-w)*omegaT*(omegaT'*((xi(ones(size(w,1),1),:))-w)');
%     dist = diag(dist);
%     % determine the two winning prototypes
%     % nearest prototype with the same class
%     [sortDist,sortIdx] = sort(dist);
%     count = 1;
%     J = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) ~= c_xi,
%         count = count+1;
%         J = sortIdx(count);
%     end
%     dJ = sortDist(count);
%     dJs(i) = dJ;
%     Js(i) = J;
%     count = 1;
%     K = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) == c_xi,
%         count = count+1;
%         K = sortIdx(count);
%     end
%     dK = sortDist(count);
%     dKs(i) = dK;
%     Ks(i) = K;
%
%     wJ = w(J,:);
%     wK = w(K,:);
%     % prototype update
%     norm_factors(i) = 4/((dJ + dK)^2);
%     DJ = (xi-wJ);
%     DK = (xi-wK);
%     DJs(i,:) = DJ;
%     DKs(i,:) = DK;
% end
% end

function D = squaredEuclidean(A, B)
% computes the sqared Euclidean distance
%
%   D = squaredEuclidean(X) returns the squared Euclidean distance matrix of data in rows of X
%   D = squaredEuclidean(X, Y) returns the distance matrix with all distances between the points in X and Y.
%
if nargin == 1 % means that one matrix
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(A,2).', 2*A*A.')); % 2*(Y*Y.')
else
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(B,2).', 2*A*B.'));
end
D = max(D,0);

function sq  = sumsquared(x,dim)
% sq  = sumsquared(x,dim)
% sum of all squared element of matrix x along dimension dim

persistent isoctave

if isempty(isoctave)
  isoctave = exist('OCTAVE_VERSION','builtin');
end

if nargin == 1
  dim = 1;
end

if isoctave
  sq = sumsq(x,dim);
else
  sq = sum(x.^2, dim);
end


function [ y, m ] = month_carry_gregorian ( y, m )

%*****************************************************************************80
%
%% MONTH_CARRY_GREGORIAN carries a year of months on the Gregorian calendar.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, integer M, the year and month.
%
%    Output, integer Y, integer M, the year and month.
%    On output, M is no greater than 12.
%
  while ( 1 )

    months = year_length_months_gregorian ( y );

    if ( m <= months )
      break
    end

    m = m - months;
    y = y + 1;

  end

  return
end


function [fevd_record]=fevd_ols_IV(struct_irf_record,gamma_record,It,Bu,IRFperiods,n)

% function [fevd_record]=bear.fevd(struct_irf_record,gamma_record,It,Bu,IRFperiods,n)
% runs the gibbs sampler to obtain draws from the posterior distribution of FEVD
% inputs:  - cell 'struct_irf_record': record of the gibbs sampler draws for the orthogonalised IRFs
%          - matrix 'gamma_record': record of the gibbs sampler draws for the structural disturbances variance-covariance matrix gamma
%          - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
% outputs: - cell 'fevd_record': record of the gibbs sampler draws for the FEVD

It=3;
Bu=0;

% this function implements algorithm 3.1.1

% preliminary tasks

% define the time horizon of FEVD as that of IRFs
FEVDperiods=IRFperiods;

% create the first cell
temp=cell(n,n+1);

% now prepare the evaluation of (3.1.13)
% start by filling the first column of every Tij matrix in the cell
% loop over rows of temp
for jj=1:n
   % loop over columns of temp
   for ii=1:n
   % square each IRF element
   temp{jj,ii}(:,1)=struct_irf_record{jj,ii}(:,1).^2;
   end
end

% fill all the other entries of the Tij matrices
% loop over rows of temp
for jj=1:n

   % loop over columns of temp
   for ii=1:n

      % loop over remaining columns
      for kk=2:FEVDperiods

      % define the column as the square of the corresponding column in orthogonalised_irf_record
      % additioned to the value of the preceeding columns, which creates the cumulation
      temp{jj,ii}(:,kk)=struct_irf_record{jj,ii}(:,kk).^2+temp{jj,ii}(:,kk-1);

      end
   end
end

% multiply each matrix in the cell by the variance of the structural shocks
% to do so, loop over simulations (rows of the Tij matrices)
for jj=1:It-Bu
% recover the covariance matrix of structural shocks gamma for this iteration
gamma=reshape(gamma_record,n,n);
% loop over rows of temp
   for ii=1:n
   % loop over columns of temp
      for kk=1:n
      % multiply row jj of the matrix by the variance of the structural shock
      temp{ii,kk}(jj,:)=temp{ii,kk}(jj,:)*gamma(kk,kk);
      end
   end
% then go for next iteration
end

% obtain now the values for Ti, the (n+1)th matrix of each row

% loop over rows of temp
for jj=1:n

% start the summation over Tij matrices
temp{jj,n+1}=temp{jj,1};
   % sum over remaining columns
   for ii=2:n
   temp{jj,n+1}=temp{jj,n+1}+temp{jj,ii};
   end
end

% create the output cell fevd_record
fevd_record=cell(n,n);

% fill the cell
% loop over rows of fevd_record
for jj=1:n
   % loop over columns of fevd_record
   for ii=1:n
   % define the matrix Vfij as the division (pairwise entry) of Tfij by Tfj
   fevd_record{jj,ii}=temp{jj,ii}./temp{jj,n+1};
   end
end


function codingLength = total_coding_length(data, labels, epsilon, affine)

% total_coding_length
%
%   compute the total number of bits needed to code a set of data vectors
%   given a grouping, and an error tolerance.
%
% Inputs:
%   data - a matrix where each column is a data vector.
%   labels - a list of integer labels that partitions the data into groups
%   epsilon - the error tolerance
%   affine - if true, compute using the affine coding length function.
%               otherwise use the linear coding length function
%
% Outputs:
%   codingLength - the total number of bits needed to code this set of data
%
% Dependencies:
%   coding_length
%
% Sep. '07  Shankar Rao -- srrao@uiuc.edu

% Copyright 2007, University of Illinois. All rights reserved.

codingLength = 0;
groupCount = max(labels);
sampleCount = length(labels);
% Make sure outlier indices are counted too
% modified 10-24-07
setOfIndices = unique(labels);
for groupIndex = setOfIndices(:)',
    codingLength = codingLength + coding_length(data(:, labels == groupIndex), epsilon^2, sampleCount, affine);
end


function p = imag(p);
%IMAG         Imaginary part of (interval) polynomial
%
%   r = imag(p);
%

% written  08/28/00     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  p.c = imag(p.c);
  p = normalize(p);


function varargout = quiver( F, varargin )
%QUIVER   Quiver plot of DISKFUNV.
%   QUIVER(F) plots the vector velocity field of F. QUIVER automatically
%   attempts to scale the arrows to fit within the grid.
%
%   QUIVER(F,S) automatically scales the arrows to fit within the grid and then
%   stretches them by S.  Use S=0 to plot the arrows without the automatic
%   scaling.
%
%   QUIVER(X,Y,F,...) is the same as QUIVER(F,...) except the arrows are on the
%   grid given in X and Y.
%
%   QUIVER(...,LINESPEC) uses the plot linestyle specified for the velocity
%   vectors.  Any marker in LINESPEC is drawn at the base instead of an arrow on
%   the tip.  Use a marker of '.' to specify no marker at all.  See PLOT for
%   other possibilities.
%
%   QUIVER(...,'numpts',N) plots arrows on a N by N grid.
%
%   H = QUIVER(...) returns a quivergroup handle.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

numpts = 30;

% Empty check:
if ( isempty( F ) )
    quiver( [] )
    return
end

if ( isempty( varargin ) )
    varargin = {};
end

holdState = ishold;
if ( ~holdState )
    hold on;
end

if ( ~holdState )
    % Generate a unit disk
    N = 200;
    th = linspace(-pi, pi, N)';
    r = exp( 1i*th );
    plot(real(r), imag(r), 'k--');
end

% Number of points to plot
j = 1;
argin = {};
while ( ~isempty( varargin ) )
    if strcmpi( varargin{1}, 'numpts' )
        numpts = varargin{2};
        varargin(1:2) = [];
    else
        argin{j} = varargin{1};
        varargin(1) = [];
        j = j+1;
    end
end
varargin = argin;

if ( isa(F, 'diskfunv') )             % quiver(F,...)

        % Plot quiver with arrows at equally spaced points:
        [xx, yy] = diskpts(numpts);
        F1 = F.components{1};
        F2 = F.components{2};
        vals1 = feval(F1, xx, yy, 'cart');
        vals2 = feval(F2, xx, yy, 'cart');
        h = quiver(xx, yy, vals1,vals2, varargin{:});
        if ( ~holdState )
            axis tight;
            axis( max([max(abs(xlim)) abs(ylim)])*[-1 1 -1 1] );
            axis equal;
        end

elseif ( nargin >= 3 )                 % quiver(x,y,F,...)

    % First two arguments contain arrow locations: we assume these are
    % Cartesian coords.
    xx = F;
    yy = varargin{1};

    if ( isa(varargin{2}, 'diskfunv') )

        F = varargin{2};
            F1 = F.components{1};
            F2 = F.components{2};
            vals1 = feval(F1, xx, yy, 'cart');
            vals2 = feval(F2, xx, yy, 'cart');
            h = quiver( xx, yy, vals1, vals2, varargin{3:end} );
            if ( ~holdState )
                axis tight;
                axis(max([max(abs(xlim)) abs(ylim)])*[-1 1 -1 1]);
                axis equal;
            end

    else

        error('DISKFUN:DISKFUNV:quiver:inputs', ...
                                  'Third argument should be a diskfunv.');

    end

end

if ( ~holdState )
    hold off;
end

if ( nargout > 0 )
    varargout = {h};
end

end

% Generates a nice set of points on the unit disk that are roughly equally
% spaced.
function [xx,yy] = diskpts(numpts)
% Idea is to use a polar grid (r,theta), but instead of using the same
% number of points in theta for every r, we make it a function of r.  We
% start with the origin, then as r increases from the origin there will be
% 6, 9, 15, 21, ... until the outer r=1 radius is reached.  This appears to
% give a nice sampling of the disk.

% The number of radii to use to get approximately numpts is
n = floor( numpts/sqrt(3) );

% Increment for the radii
dr = 1/n;

% Add the origin
xx = 0;
yy = 0;

% Handle the second ring from the origin a bit differently (i.e. do 6
% points instead of 3).
th = trigpts(6,[-pi pi]);
xx = [xx; dr*cos(th)];
yy = [yy; dr*sin(th)];

% Add points 3*(2*k-1) points for radii k.
for k = 2:n
    th = trigpts(3*(2*k-1), [-pi pi]);
    xx = [xx ; dr*k*cos(th)];
    yy = [yy ; dr*k*sin(th)];
end
end

% ANOTHER OPTION, BUT I DON'T THINK IT'S AS NICE.
% % Generates a nice set of points on the unit disk using the technique
% % described in Section 3.3 of
% % D. Calhoun, C. Helzel, R. J. LeVeque. Logically rectangular grids and
% % finite volume methods for PDEs in circular and spherical domains. SIAM
% % Review Vol. 50, Issue 4, pp. 723-752. (2008)
% function [xx,yy] = diskpts(numpts)
%
% % Generate equally spaced points over [-1,1]x[-1,1].  These will be mapped
% % to the unit disk according the the algorithm given in Section 3.3. of the
% % paper referenced above.
% [xc, yc] = meshgrid(linspace(-1,1,numpts));
%
% r1 = 1;   % map [-1,1] x [-1,1] to circle of radius r1
% d = max(abs(xc),abs(yc));
% r = sqrt(xc.^2 + yc.^2);
% r = max(r,1e-10);
% xx = r1 * d .* xc./r;
% yy = r1 * d .* yc./r;
% w = d.^2;
% xx = w.*xx + (1-w).*xc/sqrt(2);
% yy = w.*yy + (1-w).*yc/sqrt(2);
%
% end


function plotGraph(vertices, edges, figureRef)

if nargin < 3
    figureRef = figure;
end

cla(figureRef);

figure(figureRef);
plot(vertices(:,1),vertices(:,2),'rx');
hold on;

nv = size(vertices,1);
ne = size(edges,1);

v1 = vertices(edges(:,1),:);
v2 = vertices(edges(:,2),:);

vList = ones(ne*3,2);

vList(1:3:ne*3,:) = v1;
vList(2:3:ne*3,:) = v2;
vList(3:3:ne*3,:) = NaN;

plot(vList(:,1),vList(:,2),'b-');

axis equal;
xlim([0 1]);
ylim([0 1]);


function mask = voxel2mask(voxels,maskdims)
% :Usage:
% ::
%
%     function mask = voxel2mask(voxels, x y z mask dimensions)
%
% :Voxels:
%   - 3 column vectors
%   - [i j k] = row, column, slice
%   - [x y z] in brain if brain is in analyze format
%
%     (x is rows, y is columns, z is slices)
%
% ..
%    % Tor Wager, 10/17/01
% ..

[n, m] = size(voxels);
wh_bad = false(n, 1);

% Check for illegal voxels
if m ~= 3, warning('voxel2mask: Illegal voxel input list', 'Voxels must be k x 3 matrix'); end

mv = max(voxels);
if any(mv > maskdims)
    warning('voxel2mask: Illegal voxel input list', 'Voxels outside mask, Voxel indices > mask dims');
    wh_bad = any(voxels > mv(ones(n, 1), :), 2);
end

mv = min(voxels);
if any(mv < 1)
    warning('voxel2mask: Illegal voxel input list', 'Voxels outside mask, Voxel indices < 1');
    wh_bad = [wh_bad | any(voxels < ones(n, 3), 2)];
end

if ~isempty(wh_bad)
    wh_bad = find(wh_bad);
    disp('You need to check your images!');
    disp('Offending voxel numbers: '); disp(wh_bad)
    disp('Offending voxel coordinates: '); disp(voxels(wh_bad, :))
    voxels(wh_bad, :) = [];
end

mask = zeros(maskdims);

for i = 1:size(voxels, 1)
    mask(voxels(i,1),voxels(i,2),voxels(i,3)) = 1;
end

mask = double(mask);

return


function vol=smoothbinvol(vol,layer)
%
% vol=smoothbinvol(vol,layer)
%
% perform a memory-limited 3D image smoothing
%
% author: Qianqian Fang <fangq at nmr.mgh.harvard.edu>
%
% input:
%     vol: a 3D volumetric image to be smoothed
%     layer: number of iterations for the smoothing
%
% output:
%     vol: the volumetric image after smoothing
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

dim=size(vol);
dxy=dim(1)*dim(2);
fulllen=prod(dim);

weight=1./6.;
step=4000;

% in case vol is a logical
vol=double(vol);
offs=[1,-1,dim(1), -dim(1),dxy, -dxy];

for i=1:layer
  % find all non-zero values
  idx=find(vol);
  % get the neighbors of all the non-zero values
  % this may cause wrapping -- TODO
  val=vol(idx);
  for k=1:6
    nextidx=idx+offs(k);
    % find all 1-valued voxels that are located within the domain
	  goodidx=find(nextidx>0 & nextidx<fulllen);
    % for all neighboring voxels, add a fraction from the non-0 voxels
    % problematic when running in parallel (racing)
    len=length(goodidx);
    % control granualarity with step
    if(len>step)
        for j=1:step:len-step
            vol(nextidx(goodidx(j:j+step-1)))=vol(nextidx(goodidx(j:j+step-1)))+weight*val(goodidx(j:j+step-1));
        end
        vol(nextidx(goodidx(j+step:end)))=vol(nextidx(goodidx(j+step:end)))+weight*val(goodidx(j+step:end));
    else
        vol(nextidx(goodidx))=vol(nextidx(goodidx))+weight*val(goodidx);
    end
    % the above line may change the values of the non-zero voxels, recover
    % them
  end
  vol(idx)=val;
end


function img = imDrawLine(img, pos1, pos2, varargin)
%IMDRAWLINE Draw a line between two points in the image
%
%   IMG2 = imDrawLine(IMG, P1, P2);
%   IMG2 = imDrawLine(IMG, X1, Y1, X2, Y2);
%
%   [X, Y] = imDrawLine(X1, X2, Y1, Y2) computes an approximation to the
%   line segment joining (X1, Y1) and (X2, Y2) with integer coordinates.
%   X1, X2, Y1, and Y2 should be integers. imDrawLine is reversible; that
%   is, imDrawLine(X1, X2, Y1, Y2) produces the same results as
%   FLIPUD(imDrawLine(X2, X1, Y2, Y1)).
%
%   Example
%   % Draw some lines to make a diamond
%     img = imCreate([16 16], 'uint8');
%     img = imDrawLine(img, [8 1], [16 8]);
%     img = imDrawLine(img, [16 8], [8 16]);
%     img = imDrawLine(img, [8 16], [1 8]);
%     img = imDrawLine(img, [1 8], [8 1]);
%     image(img);
%
%   % Overlay some lines on a grayscale image
%     % read image
%     img = imread('cameraman.tif');
%     % draw white line
%     img = imDrawLine(img, [10 30], [150 110], 255);
%     % draw also a black line
%     img = imDrawLine(img, [10 30], [100 210], 0);
%     % display result
%     imshow(img);
%
%   See also
%     bresenhamLine

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-11-25,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Input argument extraction

% process case of input given as imDrawLine(IMG, X1, Y1, X2, Y2)
if isscalar(pos1)
    if nargin < 5
        error('Please give coordinates either as 2 points, or as 4 coords');
    end
    pos1 = [pos1 pos2];
    pos2 = [varargin{1} varargin{2}];
    varargin(1:2) = [];
end

% extract color
color = 255;
if ~isempty(varargin)
    color = varargin{1};
end

%% Coordinates computation

% coordinates of pixels
[x, y] = bresenhamLine(pos1, pos2, varargin{:});

% remove line pixels outside image
xOut = x < 1 | x > size(img, 2);
yOut = y < 1 | y > size(img, 1);
x(xOut | yOut) = [];
y(xOut | yOut) = [];

%% Write the line into image

if size(img, 3) == 1
    % grayscale image
    for i = 1:length(x)
        img(y(i), x(i)) = color;
    end

else
    % color image
    for i = 1:length(x)
        img(y(i), x(i), :) = color(:);
    end
end


function transforms2d(varargin)
%TRANSFORMS2D Description of functions operating on transforms
%
%   By 'transform' we mean an affine transform. A planar affine transform
%   can be represented by a 3x3 matrix.
%
%   Example
%   % create a translation by the vector [10 20]:
%   T = createTranslation([10 20])
%   T =
%        1     0    10
%        0     1    20
%        0     0     1
%
%
%   See also:
%   createTranslation, createRotation, createRotation90, createScaling
%   createHomothecy, createLineReflection, createBasisTransform
%   transformPoint, transformVector, transformLine, transformEdge
%   rotateVector, fitAffineTransform2d
%   polynomialTransform2d, fitPolynomialTransform2d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2008-10-13,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

help('transforms2d');


function DEM = reclassify(DEM,varargin)

%RECLASSIFY generate univariate class intervals for an instance of GRIDobj
%
% Syntax
%
%     C = reclassify(DEM);
%     C = reclassify(DEM,'method',value)
%
% Description
%
%     reclassify bins continous values of an instance of GRIDobj by setting
%     class intervals based on different classification methods. The
%     default method is equal interval classification with 10 classes.
%
% Input Arguments
%
%     DEM       Grid (class = GRIDobj)
%
%     'Methods' and [values]
%
%     'equalintervals'   [number of classes]
%     'definedintervals' [vector of class breaks]
%     'equalquantiles'   [number of classes]
%     'definedquantiles' [vector of quantiles]
%                        e.g. [0.1 0.9] results in three classes
%     'kmeans'           [number of classes]
%                        uses the k_means algorithm of Yi Cao
%                        FEX submission 19344 included in this function
%                        ! may take a while for large grids !
%     'std'              [s = scale of standard deviation, e.g. 2]
%                        equal intervals with a width of std/s. Bins are
%                        centered around sample mean
%     'otsu'             [number of classes, must be (2 4 8 16 etc)]
%                        recursive Otsu thresholding (see function
%                        graythresh)
%
% Output argument
%
%     C        Classified grid (class = GRIDobj)
%
% Example
%
%     DEM = GRIDobj('srtm_bigtujunga30m_utm11.tif');
%     C   = reclassify(DEM,'equalquantiles',10);
%     imageschs(DEM,C)
%
% See also: k_means, graythresh
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 18. January, 2013

narginchk(1,3)

allowedmethods = {'equalintervals',...
                  'definedintervals',...
                  'definedquantiles',...
                  'equalquantiles',...
                  'kmeans',...
                  'standarddeviation',...
                  'std',...
                  'otsu'...
                 };
% nr of classes
if nargin == 1;
    method = 'equalinterval';
    num = 10;
else
    method = validatestring(varargin{1},allowedmethods);

    if nargin==3
        num = varargin{2};
    end
end

INAN = isnan(DEM.Z) | isinf(DEM.Z);
DEM.Z(INAN) = nan;
inan = any(INAN(:));

switch method
    case 'equalintervals'
        validateattributes(num,{'numeric'},{'scalar'});

        if inan
            DEM.Z(~INAN) = mat2gray(DEM.Z(~INAN));
            DEM.Z = grayslice(DEM.Z,num);
            if num == 256;
                DEM.Z = double(DEM.Z) + 1;
                DEM.Z(INAN) = nan;
            elseif num < 256;
                DEM.Z = DEM.Z + 1;
                DEM.Z(INAN) = 0;
            else
                DEM.Z(INAN) = nan;
            end

        else
            DEM.Z = mat2gray(DEM.Z);
            DEM.Z = grayslice(DEM.Z,num);

            if num <= 255
                DEM.Z = DEM.Z + 1;
            elseif num == 256
                DEM.Z = double(DEM.Z) + 1;
            end
        end
    case 'definedintervals'
        validateattributes(num,{'numeric'},{'vector'});
        num(end+1) = inf;
        num = [-inf; num(:)];
        siz = size(DEM.Z);
        [~,DEM.Z] = histc(DEM.Z(:),num);
        DEM.Z = reshape(DEM.Z,siz);
    case 'equalquantiles'
        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        z = DEM.Z(~INAN);
        z = z(:);
        z = sort(z,'ascend');
        q = (1:num)/num;
        q = ceil(q*numel(z));
        edges = z(q);
        edges(end) = inf;
        [~,DEM.Z] = histc(DEM.Z(:),[-inf; edges]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;
    case 'definedquantiles'
        validateattributes(num,{'numeric'},{'vector','>',0,'<=',1});
        p = num(:);
        if p(end) < 1;
            p(end+1) = 1;
        end

        z = DEM.Z(~INAN);
        z = z(:);
        n = numel(z);
        edges = interp1((0:n-1).'/(n-1), sort(z), p);

        edges(end) = inf;
        [~,DEM.Z] = histc(DEM.Z(:),[-inf; edges]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;

    case {'standarddeviation','std'}
        validateattributes(num,{'numeric'},{'vector','>',0,'<=',1});
        z = DEM.Z(~INAN);
        z = z(:);
        s = std(z(:));
        s = s*num;
        mz = mean(z);
        minz = min(z);
        maxz = max(z);

        edges1 = [mz-s/2 :-s: minz];
        if edges1(end) > minz
            edges1(end+1) = -inf;
        else
            edges1(end) = -inf;
        end
        edges2 = mz+s/2 :s: maxz;

        edges2(end) = inf;

        [~,DEM.Z] = histc(DEM.Z(:),[edges1(end:-1:1) edges2]);
        DEM.Z = reshape(DEM.Z,DEM.size);
        DEM.Z(INAN) = nan;

    case 'kmeans';

        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        z = DEM.Z(~INAN);
        z = z(:);

        IX = k_means(z,num);

        DEM.Z(:,:) = nan;
        DEM.Z(~INAN) = IX;
    case 'otsu';
        validateattributes(num,{'numeric'},{'scalar','integer','>',1});
        if ceil(log2(num)) ~= log2(num);
            error('TopToolbox:GRIDobj','log2(value) must be an integer')
        end

        INAN = ~INAN;
        z  = mat2gray(DEM.Z(INAN));
        DEM.Z(INAN) = cast(otsu(z,num),class(DEM.Z));
end

end

% subfunctions

function [gIdx,c]=k_means(X,k)
% K_MEANS    k-means clustring
%   IDX = k_means(X, K) partititions the N x P data matrix X into K
%   clusters through a fully vectorized algorithm, where N is the number of
%   data points and P is the number of dimensions (variables). The
%   partition minimizes the sum of point-to-cluster-centroid Euclidean
%   distances of all clusters. The returned N x 1 vector IDX contains the
%   cluster indices of each point.
%
%   IDX = k_means(X, C) works with the initial centroids, C, (K x P).
%
%   [IDX, C] = k_means(X, K) also returns the K cluster centroid locations
%   in the K x P matrix, C.
%
% See also kmeans

% Version 2.0, by Yi Cao at Cranfield University on 27 March 2008.

% Example 1: small data set
%{
N=200;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
[cidx, ctrs] = k_means(X, 2);
plot(X(cidx==1,1),X(cidx==1,2),'r.',X(cidx==2,1),X(cidx==2,2),'b.', ctrs(:,1),ctrs(:,2),'kx');
%}

% Example 2: large data set
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
tic
[cidx, ctrs] = k_means(X, 2);
toc
plot(X(cidx==1,1),X(cidx==1,2),'r.',X(cidx==2,1),X(cidx==2,2),'b.', ctrs(:,1),ctrs(:,2),'kx');
%}

% Example 3: large data set with 5 centroids
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
tic
[cidx, ctrs] = k_means(X, 5);
toc
plot(X(cidx==1,1),X(cidx==1,2),'.',...
X(cidx==2,1),X(cidx==2,2),'.',...
X(cidx==3,1),X(cidx==3,2),'.',...
X(cidx==4,1),X(cidx==4,2),'.',...
X(cidx==5,1),X(cidx==5,2),'.',...
ctrs(:,1),ctrs(:,2),'+','linewidth',2)
%}

% Example 4: Comparison with kmeans in Statistics Toolbox
%{
N=20000;
X = [randn(N,2)+ones(N,2); randn(N,2)-ones(N,2)];
rand('state',0);
tic
cidx = k_means(X, 20);
toc
% Compare with kmeans in Statistis Toolbox
rand('state',0);
tic,
cidx1 = kmeans(X, 20, 'Option', statset('MaxIter',200));
toc
%}

% Check input and output
narginchk(2,2);

[n,m]=size(X);

% Check if second input is centroids
if ~isscalar(k)
    c=k;
    k=size(c,1);
else
    c=X(ceil(rand(k,1)*n),:);
end

% allocating variables
g0=ones(n,1);
gIdx=zeros(n,1);
D=zeros(n,k);

% Main loop converge if previous partition is the same as current
while any(g0~=gIdx)
%     disp(sum(g0~=gIdx))
    g0=gIdx;
    % Loop for each centroid
    for t=1:k
        d=zeros(n,1);
        % Loop for each dimension
        for s=1:m
            d=d+(X(:,s)-c(t,s)).^2;
        end
        D(:,t)=d;
    end
    % Partition data to closest centroids
    [~,gIdx]=min(D,[],2);
    % Update centroids using means of partitions
    for t=1:k
        c(t,:)=mean(X(gIdx==t,:));
    end
%     for t=1:m
%         c(:,t)=accumarray(gIdx,X(:,t),[],@mean);
%     end
end

end

function ix = otsu(x,n)

if mod(n,2)~=0
    error('number of classes is odd');
end

I = x >= graythresh(x);
ix = I*n/2;% + 1;
% do until all classes are
if n > 2
ix(I) = otsu(x(I),n/2) + ix(I);
ix(~I) = otsu(x(~I),n/2) + ix(~I);
else
    ix = ix+1;

end

end


function C = minus(A,B)
%MINUS Binary subtraction for sparse tensors.
%
%   MINUS(A,B) is called for the syntax 'A - B' when A or B is a sparse
%   tensor. A and B must have the same size, unless one is a scalar. A
%   scalar can be subtracted from a sparse tensor of any size.
%
%   Examples
%   A = sptenrand([4 3 2],5); B = sptenrand([4 3 2],3);
%   A - B %<-- sparse
%   A - 5 %<-- dense
%   A - 0 %<-- dense
%   A - full(A) %<-- dense
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of a - 5 is dense!
% The result of a - 0 is dense!
% The result of a - full(a) is dense!
% The result of a - b (two sparse matrices) is sparse.

%% Case 1: One argument is a scalar
% Emulating the sparse matrix case here, which creates and returns
% a dense result, even if the scalar is zero.

% Case 1a: Second argument is a scalar or a dense tensor
if isscalar(B) || isa(B,'tensor')
    C = full(A) - B;
    return;
end

% Case 1b: First argument is a scalar or a dense tensor
if isscalar(A) || isa(A,'tensor')
    C = A - full(B);
    return;
end

%% Case 2: Both are sparse tensors
if ~isa(A,'sptensor') || ~isa(B,'sptensor') || ~isequal(size(A),size(B))
    error('Must be two sparse tensors of the same size');
end

C = sptensor([A.subs; B.subs], [A.vals; -B.vals], size(A));


function varargout = inthist(varargin)
% VL_INTHIST  Calculate Integral Histogram
%   INTHIST = VL_INTHIST(LABELS) computes the integral histogram for
%   the label map(s) LABELS.
%
%   LABELS is a [M,N,L] UINT32 array containing L label maps (one for
%   each MxN layer). Each label map associates to each of the MxN
%   pixels one of K labels (a value of 0 denotes no association) with
%   unitary mass.
%
%   INTHIST is the [M,N,K] integral histogram obtained by integrating
%   the label maps (an integral histogram is just an array of K
%   integral images, one for each of the K labels). All layers of
%   labels are accumulated to the same integral histogram.
%
%   The total number of labels K is obtained as the maximum value of
%   LABELS. VL_INTHIST(..., 'NUMLABELS', K) specifies the number of
%   labels explicitly.
%
%   VL_INTHIST(..., 'MASS', MASSES) specifies a mass MASSES for each
%   entry of LABELS. MASSES can be either of class UINT32 or DOUBLE,
%   and the class of INTHIST varies accordingly.
%
%   See also: VL_IMINTEGRAL(), VL_SAMPLEINTHIST(), VL_HELP().
[varargout{1:nargout}] = vl_inthist(varargin{:});


%go_calib_optim_iter
%
%Main calibration function. Computes the intrinsic andextrinsic parameters.
%Runs as a script.
%
%INPUT: x_1,x_2,x_3,...: Feature locations on the images
%       X_1,X_2,X_3,...: Corresponding grid coordinates
%
%OUTPUT: fc: Camera focal length
%        cc: Principal point coordinates
%        alpha_c: Skew coefficient
%        kc: Distortion coefficients
%        KK: The camera matrix (containing fc and cc)
%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
%
%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
%
%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
%
%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
%      corresponding entry in the active_images vector to zero.
%
%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
%that is so far implemented to work only with 2D rigs.
%In the future, a more general function will be there.
%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length

if ~exist('desactivated_images'),
    desactivated_images = [];
end;

if ~exist('est_aspect_ratio'),
    est_aspect_ratio = 1;
end;

if ~exist('est_fc');
    est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!)
end;

if ~exist('recompute_extrinsic'),
    recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters
    % at each iterstion.
end;

if ~exist('MaxIter'),
    MaxIter = 30; % Maximum number of iterations in the gradient descent
end;

if ~exist('check_cond'),
    check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically
end;

if ~exist('center_optim'),
    center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
end;

if exist('est_dist'),
    if length(est_dist) == 4,
        est_dist = [est_dist ; 0];
    end;
end;

if ~exist('est_dist'),
    est_dist = [1;1;1;1;0];
end;

if ~exist('est_alpha'),
    est_alpha = 0; % by default, do not estimate skew
end;

% Little fix in case of stupid values in the binary variables:
center_optim = double(~~center_optim);
est_alpha = double(~~est_alpha);
est_dist = double(~~est_dist);
est_fc = double(~~est_fc);
est_aspect_ratio = double(~~est_aspect_ratio);

fprintf(1,'\n');

if ~exist('nx')&~exist('ny'),
    fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n');
    nx = 640;
    ny = 480;
end;

check_active_images;

quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)

% Check 3D-ness of the calibration rig:
rig3D = 0;
for kk = ind_active,
    eval(['X_kk = X_' num2str(kk) ';']);
    if is3D(X_kk),
        rig3D = 1;
    end;
end;

if center_optim & (length(ind_active) < 2) & ~rig3D,
    fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n');
    center_optim = 0; %%% when using a single image, please, no principal point estimation!!!
    est_alpha = 0;
end;

if ~exist('dont_ask'),
    dont_ask = 0;
end;

if center_optim & (length(ind_active) < 5) & ~rig3D,
    fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n');
    %if ~dont_ask,
    %   quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) ');
    %   center_optim = isempty(quest);
    %end;
end;

% A quick fix for solving conflict
if ~isequal(est_fc,[1;1]),
    est_aspect_ratio=1;
end;
if ~est_aspect_ratio,
    est_fc=[1;1];
end;

if ~est_aspect_ratio,
    fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n');
else
    if isequal(est_fc,[1;1]),
        fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n');
    end;
end;

if ~isequal(est_fc,[1;1]),
    if isequal(est_fc,[1;0]),
        fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n');
    else
        if isequal(est_fc,[0;1]),
            fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n');
        else
            fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n');
        end;
    end;
end;

if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image
    fprintf(1,'Principal point not optimized (center_optim=0). ');
    if ~exist('cc'),
        fprintf(1,'It is kept at the center of the image.\n');
        cc = [(nx-1)/2;(ny-1)/2];
    else
        fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n');
    end;
else
    fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n');
end;

if ~center_optim & (est_alpha),
    fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n');
    est_alpha = 0;
end;

if ~est_alpha,
    fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n');
    alpha_c = 0;
else
    fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n');
end;

if ~prod(double(est_dist)),
    fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n');
    if ~est_dist(1),
        fprintf(1,'     Second order distortion not estimated (est_dist(1)=0).\n');
    end;
    if ~est_dist(2),
        fprintf(1,'     Fourth order distortion not estimated (est_dist(2)=0).\n');
    end;
    if ~est_dist(5),
        fprintf(1,'     Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n');
    end;
    if ~prod(double(est_dist(3:4))),
        fprintf(1,'     Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n');
    end;
end;

% Check 3D-ness of the calibration rig:
rig3D = 0;
for kk = ind_active,
    eval(['X_kk = X_' num2str(kk) ';']);
    if is3D(X_kk),
        rig3D = 1;
    end;
end;

% If the rig is 3D, then no choice: the only valid initialization is manual!
if rig3D,
    quick_init = 1;
end;

alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent

% Conditioning threshold for view rejection
thresh_cond = 1e6;

% Initialization of the intrinsic parameters (if necessary)

if ~exist('cc'),
    fprintf(1,'Initialization of the principal point at the center of the image.\n');
    cc = [(nx-1)/2;(ny-1)/2];
    alpha_smooth = 0.1; % slow convergence
end;

if exist('kc'),
    if length(kc) == 4;
        fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree');
        kc = [kc;0];
    end;
end;

if ~exist('alpha_c'),
    fprintf(1,'Initialization of the image skew to zero.\n');
    alpha_c = 0;
    alpha_smooth = 0.1; % slow convergence
end;

if ~exist('fc') && quick_init,
    FOV_angle = 35; % Initial camera field of view in degrees
    fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']);
    fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1);
    est_fc = [1;1];
    alpha_smooth = 0.1; % slow
end;

if ~exist('fc'),
    % Initialization of the intrinsic parameters:
    fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n')
    init_intrinsic_param; % The right way to go (if quick_init is not active)!
    alpha_smooth = 0.1; % slow convergence
    est_fc = [1;1];
end;

if ~exist('kc'),
    fprintf(1,'Initialization of the image distortion to zero.\n');
    kc = zeros(5,1);
    alpha_smooth = 0.1; % slow convergence
end;

if ~est_aspect_ratio,
    fc(1) = (fc(1)+fc(2))/2;
    fc(2) = fc(1);
end;

if ~prod(double(est_dist)),
    % If no distortion estimated, set to zero the variables that are not estimated
    kc = kc .* est_dist;
end;

if ~prod(double(est_fc)),
    fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n');
end;

%%% Initialization of the extrinsic parameters for global minimization:
comp_ext_calib;

%%% Initialization of the global parameter vector:

init_param = [fc;cc;alpha_c;kc;zeros(5,1)];

for kk = 1:n_ima,
    eval(['omckk = omc_' num2str(kk) ';']);
    eval(['Tckk = Tc_' num2str(kk) ';']);
    init_param = [init_param; omckk ; Tckk];
end;

%-------------------- Main Optimization:

fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));

param = init_param;
change = 1;

iter = 0;

fprintf(1,'Gradient descent iterations: ');

param_list = param;

while (change > 1e-9) && (iter < MaxIter),

    fprintf(1,'%d...',iter+1);

    % To speed up: pre-allocate the memory for the Jacobian JJ3.
    % For that, need to compute the total number of points.

    %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active
    %% images) through a one step steepest gradient descent.

    f = param(1:2);
    c = param(3:4);
    alpha = param(5);
    k = param(6:10);

    % Compute the size of the Jacobian matrix:
    N_points_views_active = N_points_views(ind_active);

    JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
    ex3 = zeros(15 + 6*n_ima,1);

    for kk = ind_active, %1:n_ima,
        %if active_images(kk),

        omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);

        Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

        if isnan(omckk(1)),
            fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);
            return;
        end;

        eval(['X_kk = X_' num2str(kk) ';']);
        eval(['x_kk = x_' num2str(kk) ';']);

        Np = N_points_views(kk);

        if ~est_aspect_ratio,
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha);
            dxdf = repmat(dxdf,[1 2]);
        else
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha);
        end;

        exkk = x_kk - x;

        A = [dxdf dxdc dxdalpha dxdk]';
        B = [dxdom dxdT]';

        JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');

        AB = sparse(A*B');
        JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';

        ex3(1:10) = ex3(1:10) + A*exkk(:);
        ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:);

        % Check if this view is ill-conditioned:
        if check_cond,
            JJ_kk = B'; %[dxdom dxdT];
            if (cond(JJ_kk)> thresh_cond),
                active_images(kk) = 0;
                fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk)
                desactivated_images = [desactivated_images kk];
                param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1);
            end;
        end;

        %end;

    end;

    % List of active images (necessary if changed):
    check_active_images;

    % The following vector helps to select the variables to update (for only active images):
    selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)];
    if ~est_aspect_ratio,
        if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]),
            selected_variables(2) = 0;
        end;
    end;
    ind_Jac = find(selected_variables)';

    JJ3 = JJ3(ind_Jac,ind_Jac);
    ex3 = ex3(ind_Jac);

    JJ2_inv = inv(JJ3); % not bad for sparse matrices!!

    % Smoothing coefficient:

    alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!

    param_innov = alpha_smooth2*JJ2_inv*ex3;

    param_up = param(ind_Jac) + param_innov;
    param(ind_Jac) = param_up;

    % New intrinsic parameters:

    fc_current = param(1:2);
    cc_current = param(3:4);

    if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)),
        fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n');
        center_optim = 0;
        cc_current = c;
    else
        cc_current = param(3:4);
    end;

    alpha_current = param(5);
    kc_current = param(6:10);

    if ~est_aspect_ratio & isequal(est_fc,[1;1]),
        fc_current(2) = fc_current(1);
        param(2) = param(1);
    end;

    % Change on the intrinsic parameters:
    change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]);

    %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!!
    %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)
    %% The complete gradient descent method is useful to precisely update the intrinsic parameters.

    if recompute_extrinsic,
        MaxIter2 = 20;
        for kk =ind_active, %1:n_ima,
            %if active_images(kk),
            omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
            Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
            eval(['X_kk = X_' num2str(kk) ';']);
            eval(['x_kk = x_' num2str(kk) ';']);
            [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);
            [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond);
            if check_cond,
                if (cond(JJ_kk)> thresh_cond),
                    active_images(kk) = 0;
                    fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk);
                    desactivated_images = [desactivated_images kk];
                    omckk = NaN*ones(3,1);
                    Tckk = NaN*ones(3,1);
                end;
            end;
            param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk;
            param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk;
            %end;
        end;
    end;

    param_list = [param_list param];
    iter = iter + 1;

end;

fprintf(1,'done\n');

%%%--------------------------- Computation of the error of estimation:

fprintf(1,'Estimation of uncertainties...');

check_active_images;

solution = param;

% Extraction of the paramters for computing the right reprojection error:

fc = solution(1:2);
cc = solution(3:4);
alpha_c = solution(5);
kc = solution(6:10);

for kk = 1:n_ima,

    if active_images(kk),

        omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***
        Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
        Rckk = rodrigues(omckk);

    else

        omckk = NaN*ones(3,1);
        Tckk = NaN*ones(3,1);
        Rckk = NaN*ones(3,3);

    end;

    eval(['omc_' num2str(kk) ' = omckk;']);
    eval(['Rc_' num2str(kk) ' = Rckk;']);
    eval(['Tc_' num2str(kk) ' = Tckk;']);

end;

% Recompute the error (in the vector ex):
comp_error_calib;

sigma_x = std(ex(:));

% Compute the size of the Jacobian matrix:
N_points_views_active = N_points_views(ind_active);

JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);

for kk = ind_active,

    omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
    Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

    eval(['X_kk = X_' num2str(kk) ';']);

    Np = N_points_views(kk);

    %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);

    if ~est_aspect_ratio,
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c);
        dxdf = repmat(dxdf,[1 2]);
    else
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
    end;

    A = [dxdf dxdc dxdalpha dxdk]';
    B = [dxdom dxdT]';

    JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');

    AB = sparse(A*B');
    JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';

end;

JJ3 = JJ3(ind_Jac,ind_Jac);

JJ2_inv = inv(JJ3); % not bad for sparse matrices!!

param_error = zeros(6*n_ima+15,1);
param_error(ind_Jac) =  3*sqrt(full(diag(JJ2_inv)))*sigma_x;

solution_error = param_error;

if ~est_aspect_ratio && isequal(est_fc,[1;1]),
    solution_error(2) = solution_error(1);
end;

%%% Extraction of the final intrinsic and extrinsic paramaters:

extract_parameters;

fprintf(1,'done\n');

fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n');
fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[fc;fc_error]);
fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[cc;cc_error]);
fprintf(1,'Skew:             alpha_c = [ %3.5f ] +/- [ %3.5f  ]   => angle of pixel axes = %3.5f +/- %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi);
fprintf(1,'Distortion:            kc = [ %3.5f   %3.5f   %3.5f   %3.5f  %5.5f ] +/- [ %3.5f   %3.5f   %3.5f   %3.5f  %5.5f ]\n',[kc;kc_error]);
fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f ]\n\n',err_std);
fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n')
%fprintf(1,'      For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n')

%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode.

alpha_c_min = alpha_c - alpha_c_error/2;
alpha_c_max = alpha_c + alpha_c_error/2;

if (alpha_c_min < 0) && (alpha_c_max > 0),
    fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n');
    fprintf(1,'                You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n');
end;

kc_min = kc - kc_error/2;
kc_max = kc + kc_error/2;

prob_kc = (kc_min < 0) & (kc_max > 0);

if ~(prob_kc(3) && prob_kc(4))
    prob_kc(3:4) = [0;0];
end;

if sum(prob_kc),
    fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n');
    fprintf(1,'                To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc);
end;

return;


function [pnew,timing,x_min,upper] = diagonalize_quadratic_program(p,timing,x_min,upper)
pnew = p;
pnew.V = [];
pnew.diagonalized = 0;

if ~p.options.bmibnb.diagonalize
    return
end

% Preprocessing has moved nonliner constraints
% to bounds, so don't be fooled...
if p.originallyNonlinearConstraints
    return
end

% No quadratic terms
if all(p.variabletype == 0)
    return
end

% Any polynomial terms or simple linear by some reason
if any(p.variabletype > 2) & ~all(p.variabletype == 0) | p.options.bmibnb.diagonalize==0
    return
end

if ~isempty(p.evalVariables)
    return
end

if ~isempty(p.binary_variables) | ~isempty(p.integer_variables)
    return
end

if isempty(p.F_struc)
    % Bound-constrained QP is faster in original
    % form as we can exploit concavity
    return
end

nonlinear = find(p.variabletype > 0);
linear = find(p.variabletype == 0);
if ~isempty(p.F_struc)
    % Nonlinear terms in constraints
    if nnz(p.F_struc(:,1 + nonlinear))> 0
        return
    end
end

% Find quadratic and linear terms
used_in_c = find(p.c);
quadraticterms = used_in_c(find(ismember(used_in_c,nonlinear)));
Q = zeros(length(p.c),length(p.c));
if ~isempty(quadraticterms)
    usedinquadratic = zeros(1,length(p.c));
    for i = 1:length(quadraticterms)
        Qij = p.c(quadraticterms(i));
        power_index = find(p.monomtable(quadraticterms(i),:));
        if length(power_index) == 1
            Q(power_index,power_index) = Qij;
        else
            Q(power_index(1),power_index(2)) = Qij/2;
            Q(power_index(2),power_index(1)) = Qij/2;
        end
    end
end
Qlin = Q(linear,linear);
clin = p.c(linear);

if nnz(Qlin-diag(diag(Qlin)))==0
    % already diagonal
    return
end

% Decompose Q
[V,D] = eig(full(Qlin));
V = real(V);
D = real(D);
V(abs(V)<1e-11) = 0;
D(abs(D)<1e-11) = 0;
lb = p.lb(linear);
ub = p.ub(linear);
Z = V';
newub = sum([Z>0].*Z.*repmat(ub',length(Z),1),2)+sum([Z<0].*Z.*repmat(lb',length(Z),1),2);
newlb = sum([Z>0].*Z.*repmat(lb',length(Z),1),2)+sum([Z<0].*Z.*repmat(ub',length(Z),1),2);
newub(isnan(newub)) = inf;
newlb(isnan(newlb)) = -inf;

% Create new problem
clin = V'*clin;

n = length(linear);
pnew.original_linear = linear;
pnew.original_n = length(p.c);
pnew.V = V;
pnew.c = [clin;diag(D)];
pnew.Q = spalloc(2*n,2*n,0);

% find constraint polytope
if size(p.F_struc,1)>0
    A = -p.F_struc(:,1 + linear);
    b = p.F_struc(:,1);
    pnew.F_struc = [b -A*V zeros(length(b),n)];
    Abounds = [V;-V];
    bbounds = [ub;-lb];
    keep = find(~isinf(bbounds));
    if ~isempty(keep)
        pnew.F_struc = [pnew.F_struc;bbounds(keep) -Abounds(keep,:) zeros(length(keep),n)];
        pnew.K.l = pnew.K.l + length(keep);
    end
end

pnew.variabletype = [zeros(1,n) ones(1,n)*2];
pnew.monomtable = [eye(n);2*eye(n)];
pnew.monomtable(2*n,2*n) = 0;
pnew.lb =-inf(2*n,1);
pnew.ub = inf(2*n,1);
pnew.lb(1:n) = newlb;
pnew.ub(1:n) = newub;
pnew.lb(n+1:end) = 0;
pnew.ub(n+1:end) = max(newlb.^2,newub.^2);
if length(p.x0)>0
    pnew.x0 = [(V'*p.x0(p.linears));(V'*p.x0(p.linears)).^2];
end
x_min = [V'*x_min(p.linears);(V'*x_min(p.linears)).^2];
pnew.diagonalized = 1;

% Ok ,we now got a new diagonalized model
% Sometimes it performs much worse though
% Make trial calls to judge it
if p.options.bmibnb.diagonalize == -1
    tstart = tic;
    % If the model lacks bounds, these relaxations will
    % fail at this point, as bound propagation hasn't been performed yet.
    % Add fake bounds and see which performs best on fake model
    p.socpcuts.F_struc = [];
    pnew.socpcuts.F_struc = [];
    p.delayedconvex = [];
    pnew.delayedconvex = [];
    p.upper=inf;
    pnew.upper=inf;
    [output1,cost1,~,timing] = solvelower(fakeBounds(defineQuadratics(fakeLowerModel(pnew))),p.options,p.solver.lowersolver.call,[],[],timing);
    [output2,cost2,~,timing] = solvelower(fakeBounds(fakeLowerModel(p)),p.options,p.solver.lowersolver.call,[],[],timing);
    [upper_cost1,x_u1] = quickCost(pnew,output1.Primal);
    [upper_cost2,x_u2] = quickCost(p,output2.Primal);
    timing.lowersolve = timing.lowersolve + toc(tstart);
    if output1.problem && output2.problem
        % both had issues. Let's assume diagonal is better
        if p.options.verbose>=0;display('* -Diagonalized QP');end
    elseif output1.problem && ~output2.problem
        % new model has issues. use old
        pnew = p;
        pnew.V = [];
        pnew.diagonalized = 0;
        upper = upper_cost2;x_min = x_u2;
        if p.options.verbose>=0;display('* -Diagonalized QP but switched back to original.');end
    elseif ~output1.problem && output2.problem
        % old has issue, so keep new
        upper = upper_cost1;x_min = x_u1;
        if p.options.verbose>=0;display('* -Diagonalized QP.');end
    else
        % Both solved, so which had the best lower bound
        if cost1 < cost2
            % New model weaker, keep old.
            % Take best upper though!
            x_min = x_u2;
            upper = upper_cost2;
            if upper_cost1 < upper
                upper = upper_cost1;
                x_min(p.linears)=V*x_u1(pnew.linears);
            end
            pnew = p;
            pnew.V = [];
            pnew.diagonalized = 0;
            if p.options.verbose>=0;display('* -Diagonalized QP but switched back to original.');end
        else
            % New model better
            % Take best upper though!
            x_min = x_u1;
            upper = upper_cost1;
            if upper_cost2 < upper
                upper = upper_cost2;
                x_min(pnew.linears)=V'*x_u2(p.linears);
            end
            if p.options.verbose>=0;display('* -Diagonalized QP.');end
        end
    end
else
	if p.options.verbose>=0;display('* -Diagonalized QP.');end
end

function pnew = fakeLowerModel(p)
pnew = p;
pnew.complementary=[];
pnew.shiftedQP=[];
pnew.originalModel.variabletype=[];
pnew.concavityEqualities=[];
pnew.EqualityConstraintState=[];
pnew.EqualityConstraintState=[];
pnew.InequalityConstraintState=[];

function [q,z] = quickCost(p,x)
q = 0;
z=[];
for i = 1:size(p.monomtable,1)
    j = find(p.monomtable(i,:));
    v = x(j);
    w = p.monomtable(i,j);
   	z =[z;prod(v(:).^w(:))];
    q = q + p.c(i)*z(i);
end

function p = defineQuadratics(p)
n = length(p.c)/2;
p.Quadratics = n+1:2*n;
p.nonlinears = n+1:2*n;
p.QuadraticsList = [zeros(n,2);(1:n)' (1:n)'];
p.bilinears = [(n+1:2*n)' (1:n)' (1:n)'];
%p.solver.lowersolver.constraint.inequalities.secondordercone.linear=0;

function p = fakeBounds(p)
U = 1e3;
p.ub(p.linears) = min(p.ub(p.linears),U);
p.lb(p.linears) = max(p.lb(p.linears),-U);
p.ub(p.Quadratics) = min(p.ub(p.Quadratics),U^2);
p.lb(p.Quadratics) = max(p.lb(p.Quadratics),-U^2);


%% Large NLP
clc
clear
%Number of decision vars AND number of linear constraints AND number of
%nonlinear constraints
n = 2000;

% Objective & Gradient
fun = @(x) -sum(x);
grad = @(x) -ones(n,1);

% Linear constraints
A = randn(n,n);
rl = zeros(n,1);
ru = zeros(n,1);
%%
% Nonlinear Constraint, Jacobian & Structure
nlcon = @(x) x(:).^4;
nljac = @(x) sparse(diag(4*x(:).^3));
jacstr = @() speye(n);
hessian = @(x,sigma,lambda) sparse(diag(12*x(:).^2));
hessianstructure = @() speye(n);
cl = -Inf(n,1);
cu = ones(n,1);

% Starting Guess
x0 = 1*ones(n,1);
% Build Function Structure
funcs.objective = fun;
funcs.gradient = grad;
funcs.constraints = nlcon;
funcs.jacobian = nljac;
funcs.jacobianstructure = jacstr;
% funcs.hessian = hessian;
% funcs.hessianstructure = hessianstructure;

% Build Options Structure
lb = -10*ones(n,1);
ub = 40*ones(n,1);
opts = [];
opts.lb = lb;
opts.ub = ub;
opts.rl = rl;
opts.ru = ru;
opts.cl = cl;
opts.cu = cu;
opts.A = sparse(A);
opts.ipopt.hessian_approximation = 'limited-memory';
opts.ipopt.linear_solver = 'pardiso';
% Call IPOPT
[~,output] = ipopt(x0,funcs,opts);

%% Modify to call BONMIN
clc
opts.display = 1;
opts.var_type = zeros(n,1); opts.var_type(1:10) = 1;
[~,output] = bonmin(x0,funcs,opts);


function [rgb,points3d,points3dMatrix]=read_3d_pts_general(depthInpaint,K,depthInpaintsize,imageName,crop)
    % K is [fx 0 cx; 0 fy cy; 0 0 1];
    % for uncrop image crop =[1,1];
    % imageName is the full path to image
    cx = K(1,3); cy = K(2,3);
    fx = K(1,1); fy = K(2,2);
    invalid = depthInpaint==0;
    if ~isempty(imageName)
        im = imread(imageName);
        rgb = im2double(im);
    else
        rgb =double(cat(3,zeros(depthInpaintsize(1),depthInpaintsize(2)),...
                    ones(depthInpaintsize(1),depthInpaintsize(2)),...
                    zeros(depthInpaintsize(1),depthInpaintsize(2))));
    end
    rgb = reshape(rgb, [], 3);
    %3D points
    [x,y] = meshgrid(1:depthInpaintsize(2), 1:depthInpaintsize(1));
    x3 = (x-cx).*depthInpaint*1/fx;
    y3 = (y-cy).*depthInpaint*1/fy;
    z3 = depthInpaint;
    points3dMatrix =cat(3,x3,z3,-y3);
    points3dMatrix(cat(3,invalid,invalid,invalid))=NaN;
    points3d = [x3(:) z3(:) -y3(:)];
    points3d(invalid(:),:) =NaN;
end


function [dLdp,iCpY,st] = spm_mci_grad_curve (assign,w,v,M,U,Y,fxtype)
% Compute gradient and curvature for MFX model
% FORMAT [dLdp,iCpY,st] = spm_mci_grad_curve (assign,w,v,M,U,Y,fxtype)
%
% assign    fields specify which are random/fixed effects
% w         random effects vector
% v         fixed effects vector
% M,U,Y     structure,inputs,data
% fxtype    'random' or 'fixed'
%
% dLdp      gradient
% iCpY      curvature (Fisher information)
% st        -1 for integration problem
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_grad_curve.m 6697 2016-01-27 14:57:28Z spm $

% Extract init and flow params from rfx or ffx vectors
[p_init,p_flow] = spm_mci_init_flow (assign,w,v,M);

Np=0;
st=0;

% Gradient and curvature of log likelihood
if strcmp(assign.init_par,fxtype)
    [G,sy_init,st] = spm_mci_sens_init (p_init,p_flow,M,U);
    sinit=1;
    Np=Np+size(sy_init,3);
else
    sinit=0;
end

if strcmp(assign.flow_par,fxtype)
    M.x0=p_init; % Initial conditions
    [G,sy_flow,st] = spm_mci_sens (p_flow,M,U);
    sflow=1;
    Np=Np+size(sy_flow,3);
else
    sflow=0;
end

if strcmp(assign.out_par,fxtype)
    M.x0=p_init; % Initial conditions
    % Compute sensitivity to output params
    [G,x,st] = spm_mci_fwd (p_flow,M,U);
    for n=1:M.N,
        [yout,dydx,dydoutp] = feval (M.g,x(n,:)',U(:,n),p_flow,M);
        sy_out(n,:,:)=dydoutp;
    end
    sout=1;
    Np=Np+size(sy_out,3);
else
    sout=0;
end

% Read data points and time indices
try, ind=Y.ind; catch, ind=1:M.N; end
Nt=length(ind);
y=Y.y;

if st==-1, disp('Integration Problem !'); return; end

% Prediction errors
g=G(ind,:);
e=Y.y-g;

% Compute gradient and precision
dLdp=zeros(1,Np);
iCpY=zeros(Np,Np);
for t=1:Nt,
    n=ind(t);
    sn=[];
    if sinit
        sn=squeeze(sy_init(n,:,:));
        if M.l==1, sn=sn'; end
    end
    if sflow
        sy=squeeze(sy_flow(n,:,:));
        if M.l==1, sy=sy'; end
        sn=[sn,sy];
    end
    if sout
        sy=squeeze(sy_out(n,:,:));
        if M.l==1, sy=sy'; end
        sn=[sn,sy];
    end
    dLdp=dLdp+e(t,:)*M.iCe*sn;
    iCpY=iCpY+sn'*M.iCe*sn;
end


function smallpot = marginalize_pot(bigpot, keepdom, sumoverdom, nodesizes)
% MARGINALIZE_POT Marginalize a mpot onto a smaller domain.
% smallpot = marginalize_pot(bigpot, keep)

keepsize = sum(nodesizes(keepdom));
[A1, A2, B1, B2, C11, C12, C21, C22] = partition_matrix_vec_3(bigpot.A, bigpot.B, bigpot.C, keepdom, sumoverdom, nodesizes);
smallpot = scgcpot(keepsize, bigpot.ctailsize, bigpot.p, A1, B1, C11);


classdef nnl2norm < nntest
  methods (Test)

    function basic(test)
      x = test.randn([5 5 3 3]) ;
      y = vl_nnl2norm(x) ;

      % check derivatives with numerical approximation
      dzdy = test.randn(size(y)) ;
      dzdx = vl_nnl2norm(x, dzdy) ;
      test.der(@(x) vl_nnl2norm(x), x, dzdy, dzdx, 1e-3*test.range) ;
    end

  end
end


function [calscores,params] = parallel_cal_augm(w,scores)
%

if nargin==0
    test_this();
    return;
end

if ~exist('scores','var') || isempty(scores)
    calscores = sprintf(['parallel calibration:',repmat(' %g',1,length(w))],w);
    return;
end

[m,n] = size(scores);
scores = [scores;zeros(1,n)];
wsz = 2*m;

[whead,wtail] = splitvec_fh(wsz,w);
params.get_w0 = @(wfuse) init_w0(wfuse);
params.tail = wtail;

waugm = augmentmatrix_fh(m,0,whead);
calscores = linTrans(waugm,@(w)map_this(w),@(w)transmap_this(w));

    function w0 = init_w0(wfuse)
        scal = wfuse(1:end-1);
        offs = wfuse(end);
        W = [scal*(m+1);((m+1)/m)*offs*ones(m,1)];
        w0 = W(:);
    end

    function y = map_this(w)
        w = reshape(w,m+1,2);
        y = bsxfun(@times,scores,w(:,1));
        y = bsxfun(@plus,y,w(:,2));
    end

    function w = transmap_this(y)
        y = reshape(y,m+1,n);
        w = [sum(y.*scores,2),sum(y,2)];
    end

end

function test_this()

    scores = randn(4,10);
    [sys,params] = parallel_cal_augm([],scores);
    w0 = params.get_w0();
    test_MV2DF(sys,w0);

end


function i4vec_indexed_heap_d_max_test ( )

%*****************************************************************************80
%
%% I4VEC_INDEXED_HEAP_D_MAX_TEST tests I4VEC_INDEXED_HEAP_D_MAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
  m = 20;
  n_max = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_INDEXED_HEAP_D_MAX_TEST\n' );
  fprintf ( 1, '  For an indexed I4VEC,\n' );
  fprintf ( 1, '  I4VEC_INDEXED_HEAP_D_MAX reports the maximum value.\n' );
%
%  Set the data array.  To keep things easy, we will use the indicator vector.
%
  a = i4vec_indicator1 ( m );
%
%  The index array will initially be a random subset of the numbers 1 to M,
%  in random order.
%
  n = 5;
  indx(1:11,1) = [ 9, 2, 8, 14, 5, 7, 15, 1, 19, 20, 3 ]';

  i4vec_print ( m, a, '  The data vector:' );
  i4vec_print ( n, indx, '  The index vector:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end
%
%  Create a descending heap from the indexed array.
%
  indx = i4vec_indexed_heap_d ( n, a, indx );

  i4vec_print ( n, indx, '  The index vector after heaping:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after heaping:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end
%
%  Insert five entries, and monitor the maximum.
%
  for i = 1 : 5

    indx_insert = indx(n+1);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Inserting value %d\n', a(indx_insert) );

    [ n, indx ] = i4vec_indexed_heap_d_insert ( n, a, indx, indx_insert );

    indx_max = i4vec_indexed_heap_d_max ( n, a, indx );

    fprintf ( 1, '  Current maximum is %d\n', a(indx_max) );

  end
  i4vec_print ( m, a, '  The data vector after insertions:' )
  i4vec_print ( n, indx, '  The index vector after insertions:' )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A(INDX) after insertions:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %4d\n', i, a(indx(i)) );
  end

  return
end


function test_suite = test_vectorAngle
% One-line description here, please.
%   output = testVectorAngle(input)
%
%   Example
%   testVectorAngle
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testSingle(testCase) %#ok<*DEFNU>

ang = vectorAngle([1 0]);
testCase.assertEqual(0, ang, 'AbsTol', .01);

ang = vectorAngle([0 1]);
testCase.assertEqual(pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([-1 0]);
testCase.assertEqual(pi, ang, 'AbsTol', .01);

ang = vectorAngle([0 -1]);
testCase.assertEqual(3*pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([-1 1]);
testCase.assertEqual(3*pi/4, ang, 'AbsTol', .01);

function testSinglePiCentered(testCase)

ang = vectorAngle([1 0], pi);
testCase.assertEqual(0, ang, 'AbsTol', .01);

ang = vectorAngle([0 1], pi);
testCase.assertEqual(pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([-1 0], pi);
testCase.assertEqual(pi, ang, 'AbsTol', .01);

ang = vectorAngle([0 -1], pi);
testCase.assertEqual(3*pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([-1 1], pi);
testCase.assertEqual(3*pi/4, ang, 'AbsTol', .01);

function testArray(testCase)

vecs = [1 0;0 1;-1 0;0 -1;1 1];
angs = [0;pi/2;pi;3*pi/2;pi/4];
testCase.assertEqual(angs, vectorAngle(vecs), 'AbsTol', .01);

function testArrayPiCentered(testCase)

vecs = [1 0;0 1;-1 0;0 -1;1 1];
angs = [0;pi/2;pi;3*pi/2;pi/4];
testCase.assertEqual(angs, vectorAngle(vecs, pi), 'AbsTol', .01);

function testSingleZeroCentered(testCase)

ang = vectorAngle([1 0], 0);
testCase.assertEqual(0, ang, 'AbsTol', .01);

ang = vectorAngle([0 1], 0);
testCase.assertEqual(pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([0 -1], 0);
testCase.assertEqual(-pi/2, ang, 'AbsTol', .01);

ang = vectorAngle([-1 1], 0);
testCase.assertEqual(3*pi/4, ang, 'AbsTol', .01);

function testArrayZeroCentered(testCase)

vecs = [1 0;0 1;0 -1;1 1;1 -1];
angs = [0;pi/2;-pi/2;pi/4;-pi/4];
testCase.assertEqual(angs, vectorAngle(vecs, 0), 'AbsTol', .01);

function testCoupleSingleSingle(testCase)

v1 = [1 0];
v2 = [0 1];
ang = pi /2 ;
testCase.assertEqual(ang, vectorAngle(v1, v2), 'AbsTol', .01);

function testCoupleSingleArray(testCase)

v1 = [1 0];
v2 = [0 1; 0 1; 1 1; -1 1];
ang = [pi / 2 ;pi / 2 ;pi / 4 ; 3 * pi / 4];
testCase.assertEqual(ang, vectorAngle(v1, v2), 'AbsTol', .01);

function testCoupleArraySingle(testCase)

v1 = [0 1; 0 1; 1 1; -1 1];
v2 = [-1 0];
ang = [pi / 2 ;pi / 2 ; 3 * pi / 4 ; pi / 4];
testCase.assertEqual(ang, vectorAngle(v1, v2), 'AbsTol', .01);

function testCoupleArrayArray(testCase)

v1 = [1 0; 0 1; 1 1; -1 1];
v2 = [0 1; 1 0; -1 1; -1 0];
ang = [pi / 2 ;3 * pi / 2 ;pi / 2 ; pi / 4];
testCase.assertEqual(ang, vectorAngle(v1, v2), 'AbsTol', .01);


close all

% % setup
for nr= 1%1:nRuns
    figure
    plot(xR_true(1,:,nr), xR_true(2,:,nr), 'b-.','Linewidth',1), hold on
    plot(xL_true(1,:,nr), xL_true(2,:,nr), 'b*','Linewidth',1)
    title('Robot trajectory and landmarks')
    legend('True Trajectory','True Landmarks')
    xlabel('x (m)','FontWeight','bold'), ylabel('y (m)','FontWeight','bold')
    axis equal
    print('-dpng','setup_sim')
end

% % robot % %

start= 1;  incr= 5;

% Robot NEES
figure, hold on
plot([start:incr:nSteps],neesR_avg_id(start:incr:end)','g:','Linewidth',3);
plot([start:incr:nSteps],neesR_avg_std(start:incr:end)','b-o','Linewidth',1);
plot([start:incr:nSteps],neesR_avg_fej(start:incr:end)','m-.','Linewidth',2);
plot([start:incr:nSteps],neesR_avg_ocekf_1(start:incr:end)','k--','Linewidth',2);

xlabel('Time (sec)','FontWeight','bold'), ylabel('Robot pose NEES','FontWeight','bold')
legend( 'Ideal-EKF','Std-EKF','FEJ-EKF','OC-EKF')%,'OC-EKF3','Robocentric')
print('-dpng','slam_robot_nees_oc3_sim')

% Robot RMSE
figure
subplot(2,1,1), hold on
plot([start:incr:nSteps],rmsRp_avg_id(start:incr:end)','g:','Linewidth',3);
plot([start:incr:nSteps],rmsRp_avg_std(start:incr:end)','b-o','Linewidth',1);
plot([start:incr:nSteps],rmsRp_avg_fej(start:incr:end)','m-.','Linewidth',2);
plot([start:incr:nSteps],rmsRp_avg_ocekf_1(start:incr:end)','k--','Linewidth',2);
ylabel('Position RMSE (m)','FontWeight','bold')
legend( 'Ideal-EKF','Std-EKF','FEJ-EKF','OC-EKF')%,'OC-EKF3','Robocentric') %#ok<*LEGINTPAR>
subplot(2,1,2), hold on
plot([start:incr:nSteps],rmsRth_avg_id(start:incr:end)','g:','Linewidth',3);
plot([start:incr:nSteps],rmsRth_avg_std(start:incr:end)','b-o','Linewidth',1);
plot([start:incr:nSteps],rmsRth_avg_fej(start:incr:end)','m-.','Linewidth',2);
plot([start:incr:nSteps],rmsRth_avg_ocekf_1(start:incr:end)','k--','Linewidth',2);
xlabel('Time (sec)','FontWeight','bold'),
ylabel('Heading RMSE (rad)','FontWeight','bold')
print('-dpng','slam_robot_rms_oc3_sim')

NEES_Robot = [mean(neesR_avg_id(1:end)),mean(neesR_avg_std(1:end)), mean(neesR_avg_fej(1:end)), mean(neesR_avg_ocekf_1(1:end)) ]
RMS_Position = [mean(rmsRp_avg_id(1:end)),mean(rmsRp_avg_std(1:end)), mean(rmsRp_avg_fej(1:end)),   mean(rmsRp_avg_ocekf_1(1:end)) ]
RMS_Heading = [mean(rmsRth_avg_id(1:end)),mean(rmsRth_avg_std(1:end)), mean(rmsRth_avg_fej(1:end)), mean(rmsRth_avg_ocekf_1(1:end))]

% % landmarks % %

% NEES
figure
bar([neesL_avg_id,...
    neesL_avg_std,...
    neesL_avg_fej,...
    neesL_avg_ocekf_1 ]);
ylabel('Avg. Landmark NEES','FontWeight','bold')
print('-dpng','lm_nees')

% RMSE
figure
bar([rmsL_avg_id,...
    rmsL_avg_std,...
    rmsL_avg_fej,...
    rmsL_avg_ocekf_1  ]);
ylabel('Avg. Landmark RMSE','FontWeight','bold')
print('-dpng','lm_rms')

NEES_L = [neesL_avg_id,neesL_avg_std,neesL_avg_fej, neesL_avg_ocekf_1]
RMS_L = [rmsL_avg_id,rmsL_avg_std,rmsL_avg_fej, rmsL_avg_ocekf_1 ]


function str = cvx_replicate_structure( str, sz )
%CVX_REPLICATE_STRUCTURE Apply matrix structure to N-D arrays.

szs  = size( str );
nmat = prod( sz );
omat = ones( 1, nmat );
[ r, c, v ] = find( str );
nelm = length( r );
oelm = ones( 1, nelm );
r = r( : );
nvec = ( 0 : nmat - 1 ) * szs( 1 );
r = r( :, omat ) + nvec( oelm, : );
c = c( : );
c = c( :, omat );
nvec = ( 0 : nmat - 1 ) * szs( 2 );
c = c( :, omat ) + nvec( oelm, : );
v = v( : );
v = v( :, omat );
str = sparse( r, c, v, nmat * szs( 1 ), nmat * szs( 2 ) );

% 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.