function y = vl_nnloss_modified(x, c, varargin)
%VL_NNLOSS CNN categorical or attribute loss.
%   Y = VL_NNLOSS(X, C) computes the loss incurred by the prediction
%   scores X given the categorical labels C.
%
%   The prediction scores X are organised as a field of prediction
%   vectors, represented by a H x W x D x N array. The first two
%   dimensions, H and W, are spatial and correspond to the height and
%   width of the field; the third dimension D is the number of
%   categories or classes; finally, the dimension N is the number of
%   data items (images) packed in the array.
%
%   While often one has H = W = 1, the case W, H > 1 is useful in
%   dense labelling problems such as image segmentation. In the latter
%   case, the loss is summed across pixels (contributions can be
%   weighed using the `InstanceWeights` option described below).
%
%   The array C contains the categorical labels. In the simplest case,
%   C is an array of integers in the range [1, D] with N elements
%   specifying one label for each of the N images. If H, W > 1, the
%   same label is implicitly applied to all spatial locations.
%
%   In the second form, C has dimension H x W x 1 x N and specifies a
%   categorical label for each spatial location.
%
%   In the third form, C has dimension H x W x D x N and specifies
%   attributes rather than categories. Here elements in C are either
%   +1 or -1 and C, where +1 denotes that an attribute is present and
%   -1 that it is not. The key difference is that multiple attributes
%   can be active at the same time, while categories are mutually
%   exclusive. By default, the loss is *summed* across attributes
%   (unless otherwise specified using the `InstanceWeights` option
%   described below).
%
%   DZDX = VL_NNLOSS(X, C, DZDY) computes the derivative of the block
%   projected onto the output derivative DZDY. DZDX and DZDY have the
%   same dimensions as X and Y respectively.
%
%   VL_NNLOSS() supports several loss functions, which can be selected
%   by using the option `type` described below. When each scalar c in
%   C is interpreted as a categorical label (first two forms above),
%   the following losses can be used:
%
%   Classification error:: `classerror`
%     L(X,c) = (argmax_q X(q) ~= c). Note that the classification
%     error derivative is flat; therefore this loss is useful for
%     assessment, but not for training a model.
%
%   Top-K classification error:: `topkerror`
%     L(X,c) = (rank X(c) in X <= K). The top rank is the one with
%     highest score. For K=1, this is the same as the
%     classification error. K is controlled by the `topK` option.
%
%   Log loss:: `log`
%     L(X,c) = - log(X(c)). This function assumes that X(c) is the
%     predicted probability of class c (hence the vector X must be non
%     negative and sum to one).
%
%   Softmax log loss (multinomial logistic loss):: `softmaxlog`
%     L(X,c) = - log(P(c)) where P(c) = exp(X(c)) / sum_q exp(X(q)).
%     This is the same as the `log` loss, but renormalizes the
%     predictions using the softmax function.
%
%   Multiclass hinge loss:: `mhinge`
%     L(X,c) = max{0, 1 - X(c)}. This function assumes that X(c) is
%     the score margin for class c against the other classes.  See
%     also the `mmhinge` loss below.
%
%   Multiclass structured hinge loss:: `mshinge`
%     L(X,c) = max{0, 1 - M(c)} where M(c) = X(c) - max_{q ~= c}
%     X(q). This is the same as the `mhinge` loss, but computes the
%     margin between the prediction scores first. This is also known
%     the Crammer-Singer loss, an example of a structured prediction
%     loss.
%
%   When C is a vector of binary attribures c in (+1,-1), each scalar
%   prediction score x is interpreted as voting for the presence or
%   absence of a particular attribute. The following losses can be
%   used:
%
%   Binary classification error:: `binaryerror`
%     L(x,c) = (sign(x - t) ~= c). t is a threshold that can be
%     specified using the `threshold` option and defaults to zero. If
%     x is a probability, it should be set to 0.5.
%
%   Binary log loss:: `binarylog`
%     L(x,c) = - log(c(x-0.5) + 0.5). x is assumed to be the
%     probability that the attribute is active (c=+1). Hence x must be
%     a number in the range [0,1]. This is the binary version of the
%     `log` loss.
%
%   Logistic log loss:: `logistic`
%     L(x,c) = log(1 + exp(- cx)). This is the same as the `binarylog`
%     loss, but implicitly normalizes the score x into a probability
%     using the logistic (sigmoid) function: p = sigmoid(x) = 1 / (1 +
%     exp(-x)). This is also equivalent to `softmaxlog` loss where
%     class c=+1 is assigned score x and class c=-1 is assigned score
%     0.
%
%   Hinge loss:: `hinge`
%     L(x,c) = max{0, 1 - cx}. This is the standard hinge loss for
%     binary classification. This is equivalent to the `mshinge` loss
%     if class c=+1 is assigned score x and class c=-1 is assigned
%     score 0.
%
%   VL_NNLOSS(...,'OPT', VALUE, ...) supports these additionals
%   options:
%
%   InstanceWeights:: []
%     Allows to weight the loss as L'(x,c) = WGT L(x,c), where WGT is
%     a per-instance weight extracted from the array
%     `InstanceWeights`. For categorical losses, this is either a H x
%     W x 1 or a H x W x 1 x N array. For attribute losses, this is
%     either a H x W x D or a H x W x D x N array.
%
%   TopK:: 5
%     Top-K value for the top-K error. Note that K should not
%     exceed the number of labels.
%
%   See also: VL_NNSOFTMAX().

% Copyright (C) 2014-15 Andrea Vedaldi.
% Copyright (C) 2016 Karel Lenc.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ~isempty(varargin) && ~ischar(varargin{1})  % passed in dzdy
    dzdy = varargin{1} ;
    varargin(1) = [] ;
else
    dzdy = [] ;
end

opts.marginMat = [] ;
opts.marginAlpha_ = 1 ;
opts.instanceWeights = [] ;
opts.classWeights = [] ;
opts.threshold = 0 ;
opts.loss = 'softmaxlog' ;
opts.topK = 5 ;
opts = vl_argparse(opts, varargin, 'nonrecursive') ;

inputSize = [size(x,1) size(x,2) size(x,3) size(x,4)] ;

% Form 1: C has one label per image. In this case, get C in form 2 or
% form 3.
c = gather(c) ;
if numel(c) == inputSize(4)
    c = reshape(c, [1 1 1 inputSize(4)]) ;
    c = repmat(c, inputSize(1:2)) ;
end

switch lower(opts.loss)
    case {'cosinesimilaritylogloss', 'cosinesimilaritymmloss', 'cosinesimilarityregloss', 'cosinesimilarityregloss_full', ...
            'cosinesimlaritylogloss', 'cosinesimlaritymmloss', 'cosinesimlarityregloss', 'cosinesimlarityregloss_full', ...
           'cosinesimlairtylogloss', 'cosinesimlairtymmloss', 'cosinesimlairtyregloss', 'cosinesimlairtyregloss_full', ...
           'cosinesimilarityabsregloss', 'cosinesimilarityabsregloss_full'}
        hasIgnoreLabel = 0;
    otherwise
        hasIgnoreLabel = any(c(:) == 0);
end

% --------------------------------------------------------------------
% Spatial weighting
% --------------------------------------------------------------------

% work around a bug in MATLAB, where native cast() would slow
% progressively
if isa(x, 'gpuArray')
    switch classUnderlying(x) ;
        case 'single', cast = @(z) single(z) ;
        case 'double', cast = @(z) double(z) ;
    end
    c = gpuArray(c);
else
    switch class(x)
        case 'single', cast = @(z) single(z) ;
        case 'double', cast = @(z) double(z) ;
    end
end

labelSize = [size(c,1) size(c,2) size(c,3) size(c,4)] ;
assert(isequal(labelSize(1:2), inputSize(1:2))) ;
assert(labelSize(4) == inputSize(4)) ;
instanceWeights = [] ;
switch lower(opts.loss)
    case {'classerror', 'topkerror', 'log', 'softmaxlog', 'mhinge', 'mshinge'}
        % there must be one categorical label per prediction vector
        assert(labelSize(3) == 1) ;

        if hasIgnoreLabel
            % null labels denote instances that should be skipped
            instanceWeights = cast(c(:,:,1,:) ~= 0) ;
        end

    case {'binaryerror', 'binarylog', 'logistic', 'hinge'}

        % there must be one categorical label per prediction scalar
        assert(labelSize(3) == inputSize(3)) ;

        if hasIgnoreLabel
            % null labels denote instances that should be skipped
            instanceWeights = cast(c ~= 0) ;
        end
    case {'cosinesimilaritylogloss', 'cosinesimilaritymmloss', 'cosinesimilarityregloss', 'cosinesimilarityregloss_full', ...
           'cosinesimlaritylogloss', 'cosinesimlaritymmloss', 'cosinesimlarityregloss', 'cosinesimlarityregloss_full', ...
           'cosinesimlairtylogloss', 'cosinesimlairtymmloss', 'cosinesimlairtyregloss', 'cosinesimlairtyregloss_full',...
           'cosinesimilarityabsregloss', 'cosinesimilarityabsregloss_full'}
        assert(labelSize(1) == inputSize(1)) ;
        assert(labelSize(2) == inputSize(2)) ;
        assert(labelSize(3) == inputSize(3)) ;
        assert(labelSize(4) == inputSize(4)) ;
        instanceWeights = opts.instanceWeights;
    otherwise
        error('Unknown loss ''%s''.', opts.loss) ;
end

if ~isempty(opts.instanceWeights)
    % important: this code needs to broadcast opts.instanceWeights to
    % an array of the same size as c
    if isempty(instanceWeights) && isempty(strfind(lower(opts.loss), 'cosinesimilarity'))  && isempty(strfind(lower(opts.loss), 'cosinesimlarity'))
        instanceWeights = bsxfun(@times, onesLike(c), opts.instanceWeights) ;
%         instanceWeights = c;
%         instanceWeights(instanceWeights~=11) = 3;
%         instanceWeights(instanceWeights==11) = 1;
%         instanceWeights = bsxfun(@times, instanceWeights, opts.instanceWeights) ;
    elseif isempty(instanceWeights) && ~isempty(strfind(lower(opts.loss), 'cosinesimilarity'))  && ~isempty(strfind(lower(opts.loss), 'cosinesimlarity'))
        instanceWeights = bsxfun(@times, instanceWeights, opts.instanceWeights);
    else
    end
end

% --------------------------------------------------------------------
% Do the work
% --------------------------------------------------------------------

switch lower(opts.loss)
    case {'log', 'softmaxlog', 'mhinge', 'mshinge'}
        % from category labels to indexes
        numPixelsPerImage = prod(inputSize(1:2)) ;
        numPixels = numPixelsPerImage * inputSize(4) ;
        imageVolume = numPixelsPerImage * inputSize(3) ;

        n = reshape(0:numPixels-1,labelSize) ;
        offset = 1 + mod(n, numPixelsPerImage) + ...
            imageVolume * fix(n / numPixelsPerImage) ;
        ci = offset + numPixelsPerImage * max(c - 1,0) ;
end

if nargin <= 2 || isempty(dzdy)
    switch lower(opts.loss)
        case 'classerror'
            [~,chat] = max(x,[],3) ;
            t = cast(c ~= chat) ;
        case 'topkerror'
            [~,predictions] = sort(x,3,'descend') ;
            t = 1 - sum(bsxfun(@eq, c, predictions(:,:,1:opts.topK,:)), 3) ;
        case 'log'
            t = - log(x(ci)) ;
        case 'softmaxlog'
            Xmax = max(x,[],3) ;
            ex = exp(bsxfun(@minus, x, Xmax)) ;
            t = Xmax + log(sum(ex,3)) - x(ci) ;
        case 'mhinge'
            t = max(0, 1 - x(ci)) ;
        case 'mshinge'
            Q = x ;
            Q(ci) = -inf ;
            t = max(0, 1 - x(ci) + max(Q,[],3)) ;
        case 'binaryerror'
            t = cast(sign(x - opts.threshold) ~= c) ;
        case 'binarylog'
            t = -log(c.*(x-0.5) + 0.5) ;
        case 'logistic'
            %t = log(1 + exp(-c.*X)) ;
            a = -c.*x ;
            b = max(0, a) ;
            t = b + log(exp(-b) + exp(a-b)) ;
        case 'hinge'
            t = max(0, 1 - c.*x) ;
        case 'cosinesimilaritylogloss'
            t = - (log(x).*c + log(1-x).*(1-c));
        case {'cosinesimilaritymmloss', 'cosinesimlaritymmloss', 'cosinesimlairtymmloss'}
            if isempty(opts.marginMat)
                t = max(0, x-opts.marginAlpha_) .* (1-c) ;
            else
                t = max(0, x-opts.marginMat) .* (1-c) ;
            end
        case {'cosinesimilarityregloss', 'cosinesimlarityregloss', 'cosinesimlairtyregloss'}
            t = c.* ((x-c).^2); % including positive pairs only
        case {'cosinesimilarityregloss_full', 'cosinesimlarityregloss_full', 'cosinesimlairtyregloss_full'}
            t = ((x-c).^2); % including positive and negative pairs
        case {'cosinesimilarityabsregloss', 'cosinesimlarityabsregloss', 'cosinesimlairtyabsregloss'}
            t = c.* (c-x); % including positive pairs only
        case {'cosinesimilarityabsregloss_full', 'cosinesimlarityabsregloss_full', 'cosinesimlairtyabsregloss_full'}
            t =  (c-x); % including positive pairs only
    end
    if ~isempty(instanceWeights)
        y = instanceWeights(:)' * t(:) ;
    else
        y = sum(t(:));
    end
else
    if ~isempty(instanceWeights)
        dzdy = dzdy * instanceWeights ;
    end
    switch lower(opts.loss)
        case {'classerror', 'topkerror'}
            y = zerosLike(x) ;
        case 'log'
            y = zerosLike(x) ;
            y(ci) = - dzdy ./ max(x(ci), 1e-8) ;
        case 'softmaxlog'
            Xmax = max(x,[],3) ;
            ex = exp(bsxfun(@minus, x, Xmax)) ;
            y = bsxfun(@rdivide, ex, sum(ex,3)) ;
            y(ci) = y(ci) - 1 ;
            y = bsxfun(@times, dzdy, y) ;
        case 'mhinge'
            % t = max(0, 1 - x(ci)) ;
            y = zerosLike(x) ;
            y(ci) = - dzdy .* (x(ci) < 1) ;
        case 'mshinge'
            Q = x ;
            Q(ci) = -inf ;
            [~, q] = max(Q,[],3) ;
            qi = offset + numPixelsPerImage * (q - 1) ;
            W = dzdy .* (x(ci) - x(qi) < 1) ;
            y = zerosLike(x) ;
            y(ci) = - W ;
            y(qi) = + W ;
        case 'binaryerror'
            y = zerosLike(x) ;
        case 'binarylog'
            y = - dzdy ./ (x + (c-1)*0.5) ;
        case 'logistic'
            % t = exp(-Y.*X) / (1 + exp(-Y.*X)) .* (-Y)
            % t = 1 / (1 + exp(Y.*X)) .* (-Y)
            y = - dzdy .* c ./ (1 + exp(c.*x)) ;
        case 'hinge'
            y = - dzdy .* c .* (c.*x < 1) ;
        case {'cosinesimilaritylogloss', 'cosinesimlaritylogloss', 'cosinesimlairtylogloss'}
            y = -dzdy .* ( c./x - (1-c) ./ (1-x) );
        case {'cosinesimilaritymmloss', 'cosinesimlaritymmloss', 'cosinesimlairtymmloss'}

            if isempty(opts.marginMat)
                % t = max(0, x-opts.marginAlpha_) .* (1-c) ;
                y = x - opts.marginAlpha_;
                y(y<0) = 0;
                y = y.* (1-c);
                y = dzdy .* y;
            else
                y = x - opts.marginMat;
                y(y<0) = 0;
                y = y.* (1-c);
                y = dzdy .* y;
            end
        case {'cosinesimilarityregloss', 'cosinesimlarityregloss', 'cosinesimlairtyregloss'}
            y = c.* dzdy .* (x-c);     % including positive pairs only
        case {'cosinesimilarityregloss_full', 'cosinesimlarityregloss_full', 'cosinesimlairtyregloss_full'}
            y = dzdy .* (x-c); % including positive and negative pairs
        case {'cosinesimilarityabsregloss', 'cosinesimlarityabsregloss', 'cosinesimlairtyabsregloss'}
            %t = c.* (c-x); % including positive pairs only
            y = c.* dzdy .* (-x);     % including positive pairs only
        case {'cosinesimilarityabsregloss_full', 'cosinesimlarityabsregloss_full', 'cosinesimlairtyabsregloss_full'}
            %t =  (c-x); % including positive pairs only
            y = dzdy .* (-x);     % including positive pairs only
    end
end

% --------------------------------------------------------------------
function y = zerosLike(x)
% --------------------------------------------------------------------
if isa(x,'gpuArray')
    y = gpuArray.zeros(size(x),classUnderlying(x)) ;
else
    y = zeros(size(x),'like',x) ;
end

% --------------------------------------------------------------------
function y = onesLike(x)
% --------------------------------------------------------------------
if isa(x,'gpuArray')
    y = gpuArray.ones(size(x),classUnderlying(x)) ;
else
    y = ones(size(x),'like',x) ;
end


% mohsst5_mapplot(w, data)

function hax = mohsst5_mapplot(w, varargin)

options = struct('colormap', 'centered', ...
                 'data', [], ...
                 'ytick', []);

[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

if nargin < 2 || isempty(options.data)
  data = mohsst5_loaddata();
else
  data = options.data;
end

if ~isvector(w)
  error('The loadings w should be a vector');
end

M = size(data.observations,1);

lands = colsum(~isnan(data.observations)) == 0;
if length(w) == 1727
  % Fill the land areas to the matrix
  tmp = nan(M,1);
  tmp(~lands) = w;
  w = tmp;
end

map_projection('global-ellipse');
hax = map_pcolor(data.longitude, ...
                 data.latitude, ...
                 reshape(w, [length(data.longitude) length(data.latitude)]));

switch options.colormap
 case 'centered'
  colormap_redblue();
  clim_centered();
 case 'positive'
  colormap_scale();
  clim_positive();
end

map_coast();
map_grid('yticklabels', [], 'xtick', [], 'ytick', options.ytick);

if nargout < 1
  clear hax;
end


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Returns discrete {0,1} matrix with the highest coefficients in each row
% and column from matrix M
%
% Input
%   M: matrix of coefficients
%
% Output
%   R: discrete {0,1} output matrix with
%

function R = clean(M)

    M = M - min(M(:)); % translate so that the new min is zero
    fils = size(M,1);
    cols = size(M,2);
    R = zeros(size(M));

    while sum(M(:)) > 0
        [v i] = max(M(:));
        [f,c] = ind2sub(size(M),i);
        R(i) = 1;
        M(f,:) = zeros(1,cols);
        M(:,c) = zeros(fils,1);
    end


% This demo shows examples of data-driven reversible jump (RJ) moves
%  performed on a toy dataset with Gaussian emissions (MANY dims per step)
% We compare and constrast  state-sequence based moves
%   and continuous parameter based moves

clear all;
close all;

doAR = 1;
doDataDriven = 1;
obsDim = 25;

% Generate Gaussian toy data for the BP-HMM
Ktrue = 8;
T = 500;
if doAR
dataP = {'SynthAR', 'nStates', Ktrue, 'obsDim', obsDim, 'T', T};
else
dataP = {'SynthGaussian', 'nStates', Ktrue, 'obsDim', obsDim, 'T', T};
end
mP = {};
initP = {'F.nTotal', 1};
aalgP = {'Niter', 30, ...
         'RJ.birthPropDistr', 'DataDriven', 'BP.doSampleMass', 0, ...
         'BP.doSampleConc', 0, 'HMM.doSampleHypers', 0, ...
          'RJ.minW', obsDim};

for doZMove = [0 1]
    algP = aalgP;
    algP(end+1:end+2) = {'doSampleUniqueZ', doZMove};
    algP(end+1:end+2) = {'doSampleFUnique', ~doZMove};

    outP  = {obsDim+(doZMove+1)*100,1};

    runBPHMM( dataP, mP, outP, algP, initP );
end

plotLogPr( obsDim+(1:2)*100, 1, {'Unique C', 'Unique Z'} );


function [B,L,U] = boundingbox(P,Options,x)
%BOUNDINGBOX Computes bounding box of a constraint
%
% If only one output is requested, only the symbolic model is returned
%  B = boundingbox(F)
%
% If three outputs are requested, numerical values are returned too
%  [B,L,U] = boundingbox(F)
%
% A second argument can be used to specificy solver settings
%  B = boundingbox(F,sdpsettings('solver','cplex'))
%
% A third argument can (should!) be used to obtain a bounding box in a
% particular set of variables, i.e. the bounding box of a projection.
% Unless you specify which variables you are computing the bounding box
% w.r.t, it will be very hard for you to understand which variables the
% values in L and U relate to
%  [B,L,U] = boundingbox(F,[],x)
% B will now be the box [L <= x <= U] (infinite bounds not included)

if nargin < 2
    Options = sdpsettings('verbose',0);
end

if nargin == 3
    [B,L,U] = boundingbox(lmi(P.Constraints),Options,x)
else
    [B,L,U] = boundingbox(lmi(P.Constraints),Options)
end
switch nargout
    case 0
        B
    case 1
        varargout{1} = B;
    case 2
        varargout{1} = B;
        varargout{2} = L;
    case 3
        varargout{1} = B;
        varargout{2} = L;
        varargout{3} = U;
    otherwise
end


function [irf_record,D_record,gamma_record,ETA_record,beta_gibbs,sigma_gibbs]...
    =IRFt6_Bayesian(betahat,IRFperiods,n,m,p,k,T,names,startdate,enddate,X,FEVDresperiods,Y,pref,IRFt,arvar,q,It,Bu,lambda1,lambda3,lambda4,strctident,favar)

%%Implementation of a Bayesian Proxy VAR based on the Codes published by Caldara and Herbst (2018)
%%Implementation by Ben Schumann

%% IV identification
% Load IV and make it comparable with the reduced form errors
[EPSIV,IVcut,~,sigmahatIV,sigma_hat,inv_sigma_hat,IV,txt,~,cut1,cut2,cut3,cut4]=...
    bear.loadIV(betahat,k,n,Y,X,T,p,names,startdate,enddate,strctident,pref);
% IV routine
[beta_draws,sigma_draws,IV_draws,C_draws]=...
    bear.irfIV_MH(EPSIV,IVcut,betahat,sigmahatIV,sigma_hat,inv_sigma_hat,IV,txt,cut1,cut2,cut3,cut4,names,It,Bu,n,arvar,lambda1,lambda3,lambda4,m,p,k,q,X,Y,T,startdate,enddate,pref,strctident,IRFperiods,IRFt);

%reorganize
%finaly tell bear how many draws we stored
It=size(IV_draws,2);
Bu=0; %set burn in to 0, as burn in is no longer requiered

%% check Restrictions
[irf_record,D_record,gamma_record,ETA_record,beta_gibbs,sigma_gibbs]...
        =bear.irfres(beta_draws,sigma_draws,C_draws,IV_draws,IRFperiods,n,m,p,k,T,Y,X,FEVDresperiods,strctident,pref,favar,IRFt,It,Bu);


close all;
clear;
clc;

% [lat, lon, alt, vel, att, pos_std, vel_std, att_std, ins_status, pos_type, time] = inspvaxa2pvax('./0115/BY.txt');
[lat, lon, alt, vel, att, pos_std, vel_std, att_std, ins_status, pos_type, time] = inspvaxa2pvax('./0122/BY_INSPVAX.txt');
[neu] = pos2xyz(lat, lon, alt);

%%
sg_xyz = neu(find(pos_type=='INS_PSRSP'), :);
dg_xyz = neu(find(pos_type=='INS_PSRDIFF'), :);
float_xyz = neu(find(pos_type=='INS_RTKFLOAT'), :);
fixed_xyz = neu(find(pos_type=='INS_RTKFIXED'), :);

figure;
hold on; grid on;
legend_str = string([]);
if ~isempty(sg_xyz)
    plot(sg_xyz(:, 1), sg_xyz(:, 2), 'r.', 'LineWidth', 3);
    legend_str = [legend_str; 'Standalone'];
end
if ~isempty(dg_xyz)
    plot(dg_xyz(:, 1), dg_xyz(:, 2), 'm.', 'LineWidth', 3);
    legend_str = [legend_str; 'DGNSS'];
end
if ~isempty(float_xyz)
    plot(float_xyz(:, 1), float_xyz(:, 2), 'b.', 'LineWidth', 3);
    legend_str = [legend_str; 'RTK Float'];
end
if ~isempty(fixed_xyz)
    plot(fixed_xyz(:, 1), fixed_xyz(:, 2), 'g.', 'LineWidth', 3);
    legend_str = [legend_str; 'RTK Fixed'];
end
plot(neu(:, 1), neu(:, 2), 'k');
axis equal;
xlabel('东向距离(m)');
ylabel('北向距离(m)');
legend(legend_str);

set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);

%%
figure('name', '高度与位置方差曲线');
subplot(2,1,1);
plot(neu(:, 3), 'LineWidth', 1.5); grid on;
xlim([0 length(ins_status)]);
xlabel('时间(s)');
ylabel('高度(m)');
title('高度曲线');
subplot(2,1,2);
plot(pos_std(:, 1), 'LineWidth', 1.5); hold on; grid on;
plot(pos_std(:, 2), 'LineWidth', 1.5);
plot(pos_std(:, 3), 'LineWidth', 1.5);
xlim([0 length(ins_status)]);
xlabel('时间(s)');
ylabel('位置方差(m)');
legend('东向', '北向', '天向', 'Orientation', 'horizontal');
title('位置方差曲线');

set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);

%%
figure('name', '水平速度曲线');
subplot(2,1,1);
plot(sqrt(vel(:, 1).^2 + vel(:, 2).^2)*3.6, 'LineWidth', 1.5); hold on; grid on;
xlim([0 length(ins_status)]);
ylabel('水平速度(km/h)');
title('水平速度');
subplot(2,1,2);
plot(sqrt(vel_std(:, 1).^2 + vel_std(:, 2).^2), 'LineWidth', 1.5); hold on; grid on;
xlim([0 length(ins_status)]);
xlabel('时间(s)');
ylabel('水平速度方差(m/s)');

set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);

%%
figure('name', '东北天速度曲线');
subplot(2,1,1);
plot(vel(:, 1), 'LineWidth', 1.5); hold on; grid on;
plot(vel(:, 2), 'LineWidth', 1.5);
plot(vel(:, 3), 'LineWidth', 1.5);
xlim([0 length(ins_status)]);
ylabel('东北天速度(m/s)');
title('东北天速度曲线');
legend('东向', '北向', '天向', 'Orientation', 'horizontal');
subplot(2,1,2);
plot(vel_std(:, 1), 'LineWidth', 1.5); hold on; grid on;
plot(vel_std(:, 2), 'LineWidth', 1.5);
plot(vel_std(:, 3), 'LineWidth', 1.5);
xlim([0 length(ins_status)]);
xlabel('时间(s)');
ylabel('东北天速度方差(m/s)');
legend('东向', '北向', '天向', 'Orientation', 'horizontal');

set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);

%%
figure('name', '姿态曲线');
subplot(3,1,1);
plot(att(:, 1), 'r', 'LineWidth', 1.5); hold on; grid on;
plot(att(:, 2), 'b', 'LineWidth', 1.5);
xlim([0 length(ins_status)]);
ylabel('水平姿态(°)');
legend('俯仰', '横滚', 'Orientation', 'horizontal');
subplot(3,1,2);
plot(att(:, 3), 'm', 'LineWidth', 1.5); grid on;
xlim([0 length(ins_status)]);
ylabel('航向(°)');
legend('航向', 'Orientation', 'horizontal');
subplot(3,1,3);
plot(att_std(:, 1), 'r', 'LineWidth', 1.5); hold on; grid on;
plot(att_std(:, 2), 'b', 'LineWidth', 1.5);
plot(att_std(:, 3), 'm', 'LineWidth', 1.5);
xlim([0 length(ins_status)]);
xlabel('时间(s)');
ylabel('方差(°)');
legend('俯仰', '横滚', '航向', 'Orientation', 'horizontal');

set(gcf, 'Units', 'normalized', 'Position', [0.025, 0.05, 0.95, 0.85]);

%%
wm = webmap('World Imagery');
wmline(lat, lon, 'Color', 'blue', 'Width', 3, 'OverlayName', 'GNSS');
% 线性可选颜色：
% 'red', 'green', 'blue', 'white', 'cyan', 'magenta', 'yellow', 'black'


classdef chebop2
%CHEBOP2   CHEBOP2 class for representing linear partial differential operators.
%
% Class used to solve linear PDEs defined on rectangular domains that have unique and
% globally smooth solutions.
%
% N = CHEBOP2(@(u) op(u)) constructs an operator N representing the operator
% given in @(u)op(u) acting on functions of two variables on [-1,1] by [-1,1].
%
% N = CHEBOP2(@(u) op(u), [a b c d]) constructs an operator N acting on
% functions of two variables defined on [a,b] by [c,d].
%
% N = CHEBOP2(@(x,y,u) op(x,y,u),...) constructs a variable coefficient PDE
% operator. If a partial differential operator is constant coefficient, we
% recommend the @(u) notation rather than @(x,y,u) as it is more efficient.
%
% Boundary conditions are imposed via the syntax N.lbc, N.rbc, N.ubc, and N.dbc.
%
% Example 1: (Poisson with zero Dirichlet conditions):
%    N = chebop2(@(u) diff(u,2,1) + diff(u,2,2));
%    N.bc = 0;
%    u = N \ 1;
%
% Example 2: (Helmholtz equation with gravity)
%    N = chebop2(@(x,y,u) laplacian(u) - 10*y.^2.*u, [-1 1 -3 0]);
%    N.bc = 1;
%    u = N \ 0;
%
% Example 3: (Klein-Gordon equation)
%    N = chebop2(@(u) diff(u,2,1) - diff(u,2,2) + 5*u,[-1 1 0 3]);
%    N.lbc = 0; N.rbc = 0;
%    N.dbc = @(x,u) [u - exp(-30*x.^2) ; diff(u)];
%    u = N \ 0;
%
% Example 4: (Poisson equation with general Dirichlet conditions and rhs)
%    N = chebop2( @(u) lap(u));
%    N.lbc = @(y) -y.^2;
%    N.rbc = @(y) y.^2;
%    N.ubc = @(x) x;
%    N.dbc = @(x) x;
%    u = N \ chebfun2(@(x,y) cos(x.*y));
%
% For further details about the PDE solver, see:
%
% A. Townsend and S. Olver, The automatic solution of partial differential
% equations using a global spectral method, J. Comput. Phys., 299 (2015),
% pp. 106-123.
%
% Warning: This PDE solver is an experimental new feature. It has not been
% publicly advertised.  Chebop2 cannot do nonlinear problems as more
% algorithmic advances are needed.

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

    %% PROPERTIES.
    properties ( GetAccess = 'public', SetAccess = 'public' )

        domain = [];  % Domain of the operator.
        op = [];      % The operator.
        ubc = [];     % Up boundary condition(s).
        lbc = [];     % Left boundary condition(s).
        rbc = [];     % Right boundary condition(s).
        dbc = [];     % Down boundary condition(s).
        dim = [];     % Size of the system (number of eqns).
        scale = [];   % Relative solution scale.
        coeffs = [];  % Matrix storing constant coefficients.
        rhs = [];     % Righthand side, if given by user.
        xorder = 0;   % Diff order in the x-variable.
        yorder = 0;   % Diff order in the y-variable.
        U             %
        S             % Low rank form of the partial differential operator.
        V             %

    end

    %% CONSTRUCTOR.
    methods

        function N = chebop2(varargin)
            % CHEBOP2 CONSTRUCTOR.

            % Get CHEBFUN2 preferences.
            pref = chebfunpref();
            tol = pref.cheb2Prefs.chebfun2eps;

            % If empty input arguments then return an empty CHEBOP2 object.
            if ( isempty(varargin) )
                return
            end

            % What domain is the operator defined on?
            if ( numel(varargin) > 1 )
                ends = varargin{2}; % Second argument should be a domain.
                if ( length(ends) == 4 )
                    % Valid domain?
                    if ( diff(ends(1:2)) > 0 && diff(ends(3:4)) > 0 )
                        dom = ends;
                    else
                        error('CHEBFUN:CHEBOP2:chebop2:emptyDomain', ...
                            'Empty domain.');
                    end
                else
                    error('CHEBFUN:CHEBOP2:chebop2:badDomain',...
                        'Argument should be a domain given by four doubles.')
                end
            else
                if ( isa(varargin{1}, 'function_handle') )
                    % Pick the default domain.
                    rect1 = [-1, 1];
                    rect2 = [-1, 1];
                    dom = [rect1, rect2];
                elseif ( isa( varargin{1}, 'double') )
                    % Set up identity operator on the domain.
                    N = chebop2(@(u) u, varargin{1});
                    return
                else
                    error('CHEBFUN:CHEBOP2:chebop2:badArg',...
                        'First argument is not an operator or domain.')
                end
            end

            % First argument in the constructor is the operator. If the
            % operator is univariate then it's a constant coefficient PDE,
            % otherwise assume it is a variable coefficient.
            if ( isa(varargin{1}, 'function_handle') )
                fh = varargin{1};

                if ( nargin(fh) == 1 )  % The PDE has constant coefficients.
                    % Trust that the user has formed the CHEBFUN2 objects
                    % outside of CHEBOP2.

                    % Extract out rhs:
                    x = chebfun2(@(x,y) x, dom);
                    RHS = fh(0*x);
                    % In this case, the RHS must be a constant chebfun2.
                    if ( norm( RHS ) > 0 )
                        fh = @(u) fh(u) - RHS;
                        N.rhs = -RHS;             % store for later.
                    else
                        N.rhs = 0;
                    end

                    u = adchebfun2(chebfun2(@(x,y) x.*y, dom));
                    v = fh(u);
                    % If the PDO has constant coefficients then convert to
                    % double:
                    try
                        A = cell2mat(v.jacobian).';
                    catch
                        % PDO has variable coefficients, keep them in a
                        % cell array:
                        A = v.jacobian;
                    end

                elseif ( nargin(fh) == 2 )
                    error('CHEBFUN:CHEBOP2:chebop2:badOp1', ...
                        'Did you intend to have @(x,y,u)?')
                elseif ( nargin(fh) == 3 )
                    % The coefficients of the PDE are now variable
                    % coefficient.

                    % Setup a chebfun2 on the right domain
                    u = adchebfun2(chebfun2(@(x,y) x.*y, dom));
                    x = chebfun2(@(x,y) x, dom);
                    y = chebfun2(@(x,y) y, dom);

                    % Extract out rhs:
                    RHS = fh(x, y, 0*x);
                    % The RHS is a chebfun2. Check if it is the zero. Do
                    % not change function handle unless we have to because
                    % we want the user to be able to see the PDO
                    % untouched if possible.
                    if ( norm( RHS ) > 0 )
                        fh = @(x, y, u) fh(x, y, u) - RHS;
                        N.rhs = -RHS;             % store for later.
                    else
                        N.rhs = 0;
                    end

                    % Apply it to the operator.
                    v = fh(x, y, u);
                    A = v.jacobian;  % Cell array of variable coefficients.

                    % If we have a variable coefficient PDO, then compute the
                    % separable representation immediately. We need it now.
                    [cellU, matS, cellV] = chebop2.separableFormat( A,...
                                                    size(A,2), size(A,1), dom );
                    N.U = cellU;
                    N.S = matS;
                    N.V = cellV;
                else
                    error('CHEBFUN:CHEBOP2:chebop2:badOp2',...
                        'Operator should be @(u) or @(x,y,u).')
                end

            else
                error('CHEBFUN:CHEBOP2:chebop2:badOp3',...
                    'First argument should be an operator')
            end

            % Often the coefficients are obtained with small rounding errors
            % and it is important to remove the very small non-zero ones to
            % have rank(A) correct.
            if ( iscell(A) )
                for jj = size(A, 1)
                    for kk = size(A, 2)
                        if ( isa(A{jj,kk}, 'double') && abs(A{jj,kk}) < 10*tol )
                            A{jj,kk} = 0;
                        end
                    end
                end
            else
                A(abs(A) < 10*tol) = 0;
            end

            % Construct CHEBOP2 object. The boundary conditions will be
            % given later.
            N.domain = dom;
            N.op = fh;
            N.coeffs = A;

            % Calculate xorder and yorder of PDE.
            % Find the differential order of the PDE operator.
            if ( iscell(A) )
                xdifforder = size(A, 2) - 1;
                ydifforder = size(A, 1) - 1;
            elseif ( min(size(A)) > 1 )
                xdifforder = find(sum(abs(A), 2) > 100*tol, 1, 'last') - 1;
                ydifforder = find(sum(abs(A)) > 100*tol, 1, 'last' ) - 1;
            else
                if ( size(A, 1) == 1 )
                    ydifforder = length(A) - 1;
                    xdifforder = 0;
                else
                    xdifforder = length(A) - 1;
                    ydifforder = 0;
                end
            end
            N.xorder = xdifforder;
            N.yorder = ydifforder;

            % Issue a warning to the user for the first CHEBOP2:
            warning('CHEBFUN:CHEBOP2:chebop2:experimental',...
                ['CHEBOP2 is a new experimental feature.\n'...
                'It has not been tested to the same extent as other'...
                'parts of the software.']);
            % Turn it off:
            warning('off', 'CHEBFUN:CHEBOP2:chebop2:experimental');

        end

    end

    %% STATIC HIDDEN METHODS.
    methods ( Static = true, Hidden = true )

        % Matrix equation solver for AX - XB = F (p and q are ADI shifts):
        X = adi( A, B, F, p, q );

        % Matrix equation solver for A*X-X*B = M*N.' (p and q are ADI shifts):
        [UX, DX, VX] = fadi( A, B, M, N, p, q );

        % ADI shift parameters for ADI method:
        [p, q] = adiShifts(a, b, c, d, tol);

        % Matrix equation solver for AXB^T + CXD^T = E. xsplit, ysplit = 1 if
        % the even and odd modes (coefficients) decouple.
        X = bartelsStewart(A, B, C, D, E, xsplit, ysplit);

        % Use automatic differentiation to pull out the coeffs of the
        % operator:
        deriv = chebfun2deriv(op);

        % This is used to discretize the linear constrains:
        [bcrow, bcvalue] = constructBC(bcArg, bcpos,...
            een, bcn, dom, scl, order);

        % Recover coefficient functions of a linear operator:
        p = recoverCoeffs(L);

        % Checks the type of the boundary conditions:
        [bctype, g] = checkBC(N, m, n);

        % This is used to discretize the PDE:
        [CC, rhs, bb, gg, Px, Py, xsplit, ysplit] =...
            discretize(N, f, m, n, flag);

        % Convert all the different user inputs for bc into uniform format:
        bc = createBC(bcArg, ends);

        % Method for deciding how to solve the matrix equation:
        X = denseSolve(N, f, m, n);

        % Compute the separable representation of a PDO:
        [cellU, S, cellV] = separableFormat(A, xorder, yorder, dom);

        % Setup Laplace operator on domain DOM:
        N = setupLaplace( dom );

        % Remove trailing coefficients.
        a = truncate(a, tol);

    end

end


function [figH,m,cbH] = showMontage(imVol, slices, cmap, crop, numCols, figNum, flip)
% Show a montage of the volume data in imVol
%
% [figH,m,cbH] = showMontage(imVol, [slices=[]], [cmap=gray(256)], [crop=[]], [numCols=[]], figNum=figure, [flip='none'])
%
% Example:
%  t1    = niftiRead('t1.nii.gz');
%  imVol = t1.data; % that is --> : [226x271x226 int16]
%
% flip options are 'none', 'axial'
%
% HISTORY:
% 2007.02.22 RFD wrote it.
%
% Bob (c) VISTASOFT Team
%

%% Handle the Inputs

% If a file was passed in then we try to read it with niftiRead
if ~isnumeric(imVol) && exist(imVol,'file')
    try
        ni = niftiRead(imVol);
        imVol = ni.data;
    catch
        warning('A file was passed in, but that file does not appear to be a a file we can read.');
        figH = [];
        m    = [];
        cbH  = [];
        return
    end

end

if(~exist('slices','var') || isempty(slices))
  slices = [];
end

if(~exist('cmap','var') || isempty(cmap))
  cmap = gray(256);
end

if(exist('crop','var') && ~isempty(crop))
    % newSz = [diff(crop')+1 size(imVol,3)];
    imVol = imVol(crop(1,1):crop(1,2),:,:);
    imVol = imVol(:,crop(2,1):crop(2,2),:);
end

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

if(~exist('figNum','var'))
    if exist('mrvNewGraphWin','file')
        figH = mrvNewGraphWin;
    else
        figH = figure;
    end
else
    figH = figure(figNum);
end

if(~exist('flip','var') || isempty(flip))
    flip = 'none';
end

if(strcmpi(flip(1),'a'))
    imVol = flip(permute(imVol,[2 1 3 4]),1);
end

%% Make the montage

colormap(cmap);
m = makeMontage(imVol,slices,[],numCols);
imagesc(m);
axis image;
cbH = colorbar;

if(nargout<2),   clear m;    end
if(nargout<1),   clear figH; end

return;


function [K, sk] = wienerKernCompute(kern, x, x2)

% WIENERKERNCOMPUTE Compute the WIENER kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the wiener
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the wiener
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : wienerKernParamInit, kernCompute, kernCreate, wienerKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2009

% KERN

  if any(x<0)
    error('WIENER kernel only valid for time greater than zero')
  end
  if nargin < 3
    K = repmat(x, 1, size(x, 1));
    sk = min(K, K');
  else
    if any(x2<0)
      error('WIENER kernel only valid for time greater than zero')
    end
    sk = min(repmat(x, 1, size(x2, 1)), repmat(x2', size(x, 1), 1));
  end
  K = kern.variance*sk;
end


% 3D molecular dynamics (GPU vectorized)
% by Brian Haidet for AlphaPhoenix
% published 12/15/2018
% CC non-commercial, attribution

% Don't copy this straight into a homework or something rediculous - I'm
% posting this so people can hopefully learn from it! It's not the cleanest
% code, but I added a few more comments in the areas that need
% explaining. and deleted all the extra bits of commented-out code that had
% built up. Feel free to ask more questions on youtube here!
% https://youtu.be/6DlRsPo-dxY

%print out to images
%close all;
v=VideoWriter(['outMovie' num2str(now()) '.mp4'],'MPEG-4');
v.FrameRate = 30;
open(v);

numFrames = length(dir('c:\MatlabOutput\CsClSimFrames\data'))

breakearly=35404-1
breakearly=3500

tic
iteration = 0;
while true
    if iteration==breakearly
        break;
    end
    iteration = iteration + 1;
    if exist(['c:\MatlabOutput\CsClSimFrames\data\data' num2str(iteration,'%05d') '.mat'],'file')
        load(['c:\MatlabOutput\CsClSimFrames\data\data' num2str(iteration,'%05d') '.mat'])
    else
        break;
    end
    clf;
    figure(1)
    set(gcf, 'Position', [0 0 1920, 1080]); %<- Set size
    pause(.02);
            plotter(iteration);
        drawnow()

%     plotter(iteration);
    writeVideo(v,getframe(gcf));
    disp(['Printing frame ' num2str(iteration) '/' num2str(numFrames) '   |   Est Completion Time: ' datestr(addtodate(now,floor((1000*toc/(iteration)*(numFrames-iteration))),'millisecond'))]);
end
close(v)


function ret = syntheticCalib(noiseType, noiseSigma, calibInfo)
%% Compare four calibration methods using synthetic data.
% Notations
% gt:
%     ground truth
% wb:
%     white board, also called calibration board where checkerboard is
%     attached to the center of it.
% cb: checkerboard
% sl:
%     structured light that is projected by the projector,
%     e.g, slPtsCamImg: structured light points in camera image space
% PR: proposed method
% DG: degraded proposed method (w/o BA)
% GH: generalized global homography method
% MT: Moreno & Taubin method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%%
disp('Calibrating...');

% for debug
verbose = 0;

format short

%% Noise description (noiseType)
% 0. No noise
% 1. Camera image noise
% 2. Projector image noise (before projection).
% 3. Calibration board noise.
% 4. All noises above

%% 1. Load ground truth parameters
% Load calibration data as synthetic params ground truth
gt = Calibration.loadGroundTruth('simulation_ground_truth.yml', calibInfo);

%% 2. Generate synthetic data using gt params
gt = Calibration.generateSyntheticData(gt, noiseType, noiseSigma, verbose);
synthData = gt.synthData;
PR = synthData.PR;
MT = synthData.MT;

%% 3. Calibrate camera using checkerboard points

% camParams is global homography and Moreno & Taubin method's camera
% paramter.
camParams = Calibration.calibrateInitGuess(synthData.cbPtsModel, synthData.cbPtsCamImg, calibInfo);

%% 4. Use calibrated cam params to undistort camera checkerboard and SL nodes

% # of calibration board poses
numSets = length(synthData.cbPtsCamImg);

% checkerboard points in projector image space
cbPtsPrjImgGH = cell(1, numSets);

% undistorted checkerboard and sl grid nodes in camera image space
cbPtsCamImgUndistort = cell(1, numSets);
slPtsCamImgUndistortPR = cell(1, numSets);

for i = 1:numSets
    % undistort checkerboard points in camera image
    cbPtsCamImgUndistort{i} = ImgProc.cvUndistortPoints(synthData.cbPtsCamImg{i}, camParams.camK, camParams.camKc);

    % undistort grid points in camera image
    slPtsCamImgUndistortPR{i} = ImgProc.cvUndistortPoints(PR.slPtsCamImg{i}, camParams.camK, camParams.camKc);

    % find a global homography between camera image space and projector
    % image space
    [Hcp, ~] = cv.findHomography(slPtsCamImgUndistortPR{i}, PR.slPtsPrjImg{i});

    % transform camera checkerboard points to projector image space using
    % global Hcp
    % NOTE: the cb corners have to be undistorted
    cbPtsPrjImgGH{i} = ImgProc.applyHomography(cbPtsCamImgUndistort{i}, Hcp);
end

%% 5. Compute SL nodes in world space and model space for PR and DG

% estimate grid points in world space using camera calibration tvecs and
% rvecs
slPtsModelPR = cell(1, numSets); % estimated grid points in model space
slPtsWorldPR = cell(1, numSets); % estimated grid points in world space

for i = 1:numSets
    % according to Zhang's method the homoraphy from cb model space is:
    % Hmc = lambda*K*[r1, r2, t], where r1, r2 are 1st and 2nd column of R
    R = cv.Rodrigues(camParams.rVecs{i});
    Hmc = camParams.camK * [R(:, 1:2), camParams.tVecs{i}];
    lambda = 1 / norm(inv(camParams.camK) * Hmc(:, 1));
    Hmc = lambda * Hmc;

    % transform camera image grid points to white board model space using
    % Hmc, then use calibrated tvecs and rvecs to transform grid points from
    % model space to world space.
    % NOTE: the cb corners have to be undistorted
    curSlPtsModelPR = ImgProc.applyHomography(slPtsCamImgUndistortPR{i}, inv(Hmc));
    slPtsModelPR{i} = [curSlPtsModelPR, zeros(length(curSlPtsModelPR), 1)];

    % transform grid points from model space to world space.
    slPtsWorldPR{i} = Calibration.rotateTranslatePoints(slPtsModelPR{i}, camParams.tVecs{i}', camParams.rVecs{i}, true);
end

%% 6. [Global Homography] Calibrate using checkerboard points and global homography

prjParamsGH = Calibration.calibrateInitGuess(synthData.cbPtsModel, cbPtsPrjImgGH, calibInfo);

% 2. Stereo calibration using OpenCV
stereoParamsGH = Calibration.calibrateStereoInitGuess(synthData.cbPtsModel, synthData.cbPtsCamImg, cbPtsPrjImgGH, camParams, prjParamsGH, calibInfo);
stereoParamsGH.sets = calibInfo.sets;
stereoParamsGH.dataName = calibInfo.dataName;

% 3. Calculate reprojection errors
stereoParamsGH = Calibration.calcReprojectionError(stereoParamsGH, synthData.cbPtsModel, synthData.cbPtsCamImg, cbPtsPrjImgGH);

if (verbose)
    Reconstruct.visualizePts3d(cell2mat(stereoParamsGH.worldPts'), stereoParamsGH.R, stereoParamsGH.T, 'Global homography calibration: Xm ');
end

%% 7. Subsample SL nodes to save computation time and RAM

for i = 1:length(slPtsModelPR)
    idx = round(linspace(1, length(slPtsModelPR{i}), 500));
    slPtsModelPR{i} = slPtsModelPR{i}(idx, :);
    PR.slPtsCamImg{i} = PR.slPtsCamImg{i}(idx, :);
    PR.slPtsPrjImg{i} = PR.slPtsPrjImg{i}(idx, :);

    % also do the same to slPtsWorldPR
    slPtsWorldPR{i} = slPtsWorldPR{i}(idx, :);
end

%% 8. [Degraded proposed] w/o BA, use DG for short
% 1. Use 'withoutBA' option to specify no bundle adjustment on Xm
stereoParamsDG = Calibration.stereoCalibrate(slPtsModelPR, PR.slPtsCamImg, PR.slPtsPrjImg, calibInfo.camImgSize, calibInfo.prjImgSize, 'withoutBA');
stereoParamsDG.sets = calibInfo.sets;
stereoParamsDG.dataName = calibInfo.dataName;

% 2. Calculate reprojection errors
stereoParamsDG = Calibration.calcReprojectionError(stereoParamsDG, stereoParamsDG.modelPts, stereoParamsDG.camImgPts, stereoParamsDG.prjImgPts);

% debug
if (verbose)
    Reconstruct.visualizePts3d(cell2mat(stereoParamsDG.worldPts'), stereoParamsDG.R, stereoParamsDG.T, 'Propsoed w/o BA calibration: Xm ');
end

%% 9. [proposed] with BA, use PR for short
% 1. Use 'BA' option to specify bundle adjustment on Xm
stereoParamsPR = Calibration.stereoCalibrate(slPtsModelPR, PR.slPtsCamImg, PR.slPtsPrjImg, calibInfo.camImgSize, calibInfo.prjImgSize, 'BA');
stereoParamsPR.sets = calibInfo.sets;
stereoParamsPR.dataName = calibInfo.dataName;

% 2. Calculate reprojection errors
stereoParamsPR = Calibration.calcReprojectionError(stereoParamsPR, stereoParamsPR.modelPts, stereoParamsPR.camImgPts, stereoParamsPR.prjImgPts);

% debug
if (verbose)
    Reconstruct.visualizePts3d(cell2mat(stereoParamsDG.worldPts'), stereoParamsDG.R, stereoParamsDG.T, 'Propsoed calibration: Xm ');
end

%% 10. Compute local homographies and warp corners for Moreno & Taubin method

% same cb corners as GH method
cbPtsModelMT = synthData.cbPtsModel;
cbPtsCamImgMT = synthData.cbPtsCamImg;

% warped checkerboard corners in projector image space (use local homographies)
cbPtsPrjImgMT = [];

% local homography windows size (default parameter)
winSize = 40/2;

% number of cb corners
numCorners = calibInfo.numCorners;

% for each sets (calibration board pose)
for i = 1:numSets
    cbPtsPrjImgMT{i} = zeros(numCorners, 2);

    % undistort grid points in camera image
    curSlPtsCamImgUndistortMT = ImgProc.cvUndistortPoints(MT.slPtsCamImg{i}, camParams.camK, camParams.camKc);
    curSlPtsPrjImgMT = MT.slPtsPrjImg{i};

    % for each cb corner
    for j = 1:numCorners

        % The current checkerboard point
        curCbPtsCamImgRoundMT = round(cbPtsCamImgUndistort{i}(j, :));

        % only use the SL points that are within winSize range of cb point
        % to estimate local homographies
        i1 = curSlPtsCamImgUndistortMT(:, 1) < curCbPtsCamImgRoundMT(1) + winSize;
        i2 = curSlPtsCamImgUndistortMT(:, 1) > curCbPtsCamImgRoundMT(1) - winSize;
        i3 = curSlPtsCamImgUndistortMT(:, 2) < curCbPtsCamImgRoundMT(2) + winSize;
        i4 = curSlPtsCamImgUndistortMT(:, 2) > curCbPtsCamImgRoundMT(2) - winSize;
        inlierIdx = i1 & i2 & i3 & i4;

        if (nnz(inlierIdx) < 4)
            warning('Not enough points to fit a homography, the camera calibration is not very accurate');
            cbPtsPrjImgMT{i}(j, :) = nan;
        else
            % 2d grid points in camera and projector image that are within the window
            curSlPtsCamImgWin = curSlPtsCamImgUndistortMT(inlierIdx, :);
            curSlPtsPrjImgWin = curSlPtsPrjImgMT(inlierIdx, :);

            % calculate local homography
            Hcp = cv.findHomography(curSlPtsCamImgWin, curSlPtsPrjImgWin);

            % transform camera checkerboard corners to projector image
            % space using Hcp
            cbPtsPrjImgMT{i}(j, :) = ImgProc.applyHomography(cbPtsCamImgUndistort{i}(j, :), Hcp);
        end
    end
end

% get rid of nan if fail to estimate a local homogrphy
for i = 1:numSets
    nanIdx = sum(isnan(cbPtsPrjImgMT{i}), 2) > 0;

    cbPtsModelMT{i}(nanIdx, :) = [];
    cbPtsCamImgMT{i}(nanIdx, :) = [];
    cbPtsPrjImgMT{i}(nanIdx, :) = [];
end

%% 11. [Moreno & Taubin method] use MT for short

% 1. Calibrate projector using local homography warped checkerboard corners
prjParamsMT = Calibration.calibrateInitGuess(cbPtsModelMT, cbPtsPrjImgMT, calibInfo);

% 2. Stereo calibration using OpenCV
stereoParamsMT = Calibration.calibrateStereoInitGuess(cbPtsModelMT, cbPtsCamImgMT, cbPtsPrjImgMT, camParams, prjParamsMT, calibInfo);
stereoParamsMT.sets = calibInfo.sets;
stereoParamsMT.dataName = calibInfo.dataName;

% 3. Calculate reprojection errors
stereoParamsMT = Calibration.calcReprojectionError(stereoParamsMT, cbPtsModelMT, cbPtsCamImgMT, cbPtsPrjImgMT);

% debug
if (verbose)
    Reconstruct.visualizePts3d(cell2mat(stereoParamsMT.worldPts'), stereoParamsMT.R, stereoParamsMT.T, 'Moreno & Taubin calibration: Xm ');
end

%% 12. [debug] Compare GH and MT warped checkerboard points in projector image space
if (verbose)
    % MT is + and GH is o
    figure;
    title('warped checkerboard corners in projector image, MT is + and GH is o');
    hold on;
    planeColors = hsv(numSets);

    for i = 1:numSets
        plot(cbPtsPrjImgGH{i}(:, 1), cbPtsPrjImgGH{i}(:, 2), 'o', 'Color', planeColors(i, :));
        plot(cbPtsPrjImgMT{i}(:, 1), cbPtsPrjImgMT{i}(:, 2), '+', 'Color', planeColors(i, :));
    end
end

%% 9. Visualize all error metrics
[errMT, stereoParamsMT] = Calibration.showErrors(stereoParamsMT, gt, 'Moreno & Taubin');
[errGH, stereoParamsGH] = Calibration.showErrors(stereoParamsGH, gt, 'Global homography');
[errDG, stereoParamsDG] = Calibration.showErrors(stereoParamsDG, gt, 'Proposed w/o BA');
[errPR, stereoParamsPR] = Calibration.showErrors(stereoParamsPR, gt, 'Proposed');

%% Convert err to table
ret = [errMT; errGH; errDG; errPR];
resultsTable = struct2table(ret);
disp(resultsTable);

end


function fvc = u3d_pre
% Syntax:
%     fvc = u3d_pre
%
% Description:
%    This function generates the input for the MESH_TO_LATEX function from
%    Alexandre Gramfort from your surface-graphs. The surface graphs 3d-model can be
%    displayed in u3d - format in pdf or if you don't delete the u3d-files
%    which are generated by MESH_TO_LATEX you can use deepview from
%    righthemisphere(http://www.righthemisphere.com/products/client-products/deep-view)
%    to embed the modell in Microsoft-Office products (Word, Excel, PowerPoint).
%
%    The output is a structure generated by the standard MATLAB function
%    surf2patch which can used in MESH_TO_LATEX:
%       fvc.vertices         -->    points
%       fvc.faces            -->    faces
%       fvc.facevertexcdata  -->    face_vertex_data
%
% Example:
%       [X,Y,Z] = peaks(30);
%       surf(X,Y,Z);
%       fvc = u3d_pre;
%       mesh_to_latex('tex/mesh',fvc.vertices,uint32(fvc.faces),fvc.facevertexcdata);
%
% Bugs and suggestions:
%    Please send to Sven Koerner: koerner(underline)sven(add)gmx.de
%
% See also:
%    MESH_TO_LATEX on the File Exchange
%    http://www.righthemisphere.com/products/client-products/deep-view
%
%
% License to use and modify this code is granted freely to all interested, as long as the original author is
% referenced and attributed as such. The original author maintains the right to be solely associated with this work.
%
% Programmed by Sven Koerner: koerner(underline)sven(add)gmx.de
% Date: 2010/04/14

% search for surface-graph
h = findobj('type','surface');

if ~isempty(h)
    % get defined data-points
    X = get(h(end,1), 'XData');
    Y = get(h(end,1), 'YData');
    Z = get(h(end,1), 'ZData');

    % scaled color to unscaled r
    cdata = get(h(end,1), 'CData');
    siz = size(cdata);
    cmap = colormap;
    nColors = size(cmap,1);
    cax = caxis;
    idx = ceil( (double(cdata) - cax(1)) / (cax(2)-cax(1)) * nColors);
    idx(idx<1) = 1;
    idx(idx>nColors) = nColors;
    %handle nans in idx
    nanmask = isnan(idx);
    idx(nanmask)=1; %temporarily replace w/ a valid colormap index
    realcolor = zeros(siz);
    for i = 1:3,
        c = cmap(idx,i);
        c = reshape(c,siz);
        realcolor(:,:,i) = c;
    end

    fvc = surf2patch(X,Y,Z,realcolor, 'triangles' );
else
    fvc.vertices            = [];
    fvc.faces               = [];
    fvc.facevertexcdata     = [];
    disp('No surface graph found.');
end;


function forest = SCiForest(X, t, psi, num_attributes, num_hyperplanes)
%iForest Construct a "forest" (set) of isolation trees
%   X: the data, each row is an instance
%   t: the number of trees to construct
%   psi: the sub-sampling size

height_limit = ceil(log2(psi));
[numdata ~] = size(X);
if nargin < 5
    num_hyperplanes = 10;
end
if nargin < 4
    num_attributes = 2;
end

for i=1:t
    sampleind = randsample(1:numdata, psi);
    X_sample = X(sampleind, :);
    forest(i) = SCiTree(X_sample, num_attributes, num_hyperplanes, 0, height_limit);
end

end


function [y,idx] = nanmin(a,dim,b)
% FORMAT: [Y,IDX] = NANMIN(A,DIM,[B])
%
%    Minimum ignoring NaNs
%
%    This function enhances the functionality of NANMIN as distributed in
%    the MATLAB Statistics Toolbox and is meant as a replacement (hence the
%    identical name).
%
%    If fact NANMIN simply rearranges the input arguments to MIN because
%    MIN already ignores NaNs.
%
%    NANMIN(A,DIM) calculates the minimum of A along the dimension DIM of
%    the N-D array X. If DIM is omitted NANMIN calculates the minimum along
%    the first non-singleton dimension of X.
%
%    NANMIN(A,[],B) returns the minimum of the N-D arrays A and B.  A and
%    B must be of the same size.
%
%    Comparing two matrices in a particular dimension is not supported,
%    e.g. NANMIN(A,2,B) is invalid.
%
%    [Y,IDX] = NANMIN(X,DIM) returns the index to the minimum in IDX.
%
%    Similar replacements exist for NANMAX, NANMEAN, NANSTD, NANMEDIAN and
%    NANSUM which are all part of the NaN-suite.
%
%    See also MIN

% -------------------------------------------------------------------------
%    author:      Jan Glscher
%    affiliation: Neuroimage Nord, University of Hamburg, Germany
%    email:       glaescher@uke.uni-hamburg.de
%
%    $Revision: 1.1 $ $Date: 2004/07/15 22:42:14 $

if nargin < 1
	error('Requires at least one input argument')
end

if nargin == 1
	if nargout > 1
		[y,idx] = min(a);
	else
		y = min(a);
	end
elseif nargin == 2
	if nargout > 1
		[y,idx] = min(a,[],dim);
	else
		y = min(a,[],dim);
	end
elseif nargin == 3
	if ~isempty(dim)
		error('Comparing two matrices along a particular dimension is not supported')
	else
		if nargout > 1
			[y,idx] = min(a,b);
		else
			y = min(a,b);
		end
	end
elseif nargin > 3
	error('Too many input arguments.')
end

% $Id: nanmin.m,v 1.1 2004/07/15 22:42:14 glaescher Exp glaescher $


function m = cross(m1,m2)
% pointwise cross product of two vector3d
%
% Syntax
%   v = cross(v1,v2)
%
% Input
%  v1,v2 - @vector3d
%
% Output
%  v - @vector3d

m = cross@vector3d(m1,m2);

% switch from recirprocal to direct and vice verca
m.dispStyle = MillerConvention(-MillerConvention(m.dispStyle));


% module = noise_module_laplace(noise_module, varargin)

function module = noise_module_laplace(varargin)

options = struct('init', struct(), ...
                 'update_u', true);
[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

U = [];
LogU = [];

module.initialize = @initialize;
module.update = @update;
module.get_struct = @get_struct;

  function [Tau] = initialize(M, N)

  U = nan(M,N);

  % Parse custom initialization
  init = struct('U', 1);
  [init, errmsg] = argparse(init, options.init);
  error(errmsg);

  % Initialize
  U(:,:) = init.U;
  Tau = U;
  end

  function S = get_struct()
  S.module = 'noise_module_independent_t';
  S.U = U;
  end

  function [Tau, LogTau, KL_Tau] = update(iter, E2, Obs)

  % OBS is the number of observations for each element of E2.

  [M,N] = size(E2);

  %
  % Update U
  %

  if index_selected(iter, options.update_u)

    % Update the distribution

    % a_U = bsxfun(@plus, nu, Obs) / 2;
    % b_U = bsxfun(@plus, nu, E2) / 2;
    % U = a_U ./ b_U;
    U(Obs) = E2(Obs) .^ (-0.5);
    LogU = nan(M,N); %psi(a_U) - log(b_U);
  end

% $$$   if iter == 4
% $$$     figure
% $$$     imagesc(U);
% $$$     error('debuggin')
% $$$   end
% $$$   if iter == 1
% $$$     figure
% $$$     imagesc(U);
% $$$     error('debuggin')
% $$$   end

  %minU = min(U(:));
  %maxU = max(U(:));

  % KL-term: <log q(U)> - <log p(U)>
  KL_U = NaN;
% $$$     0;
% $$$     KL_U = KL_U - sum(gamma_entropy(a_U(Obs),b_U(Obs)));
% $$$     switch options.nu
% $$$
% $$$      case 'pooled'
% $$$       KL_U = KL_U - sum(gamma_logpdf(U(Obs), nu/2, nu/2, LogU(Obs)));
% $$$
% $$$      case 'separate_rows'
% $$$       for m=1:M
% $$$         Nmv = Obs(m,:);
% $$$         KL_U = KL_U - sum(gamma_logpdf(U(m,Nmv),LogU(m,Nmv),nu(m)/2,nu(m)/2));
% $$$       end
% $$$
% $$$      case 'separate_columns'
% $$$       error('Not implemented yet')
% $$$
% $$$      otherwise
% $$$       error('Unknown model for nu');
% $$$
% $$$     end
% $$$
% $$$   else
% $$$
% $$$     KL_U = NaN;
% $$$
% $$$   end

  %
  % Compute the joint noise stuff
  %

  Tau = U;
  LogTau = LogU;
  KL_Tau = KL_U;

  end

end


function model = mogOptimise(model, display, iters)

% MOGOPTIMISE Optimise an MOG model.
% FORMAT
% DESC optimises an mixtures of Gaussians model via the
% expectation maximisation algorithm.
% ARG model : the model to be optimised.
% RETURN model : the optimised model.
%
% SEEALSO : mmpcaCreate, modelOptimise
%
% COPYRIGHT : Neil D. Lawrence, 2006

% MLTOOLS

diffll = 1;
iter = 0;
ll = mogLowerBound(model);
while abs(diffll)>1e-6 & iter<iters
  iter = iter + 1;
  model =  mogEstep(model);
  if display > 1
    [ll, oldll] = boundCheck(model, ll, 'E-step');
  end
  model = mogUpdatePrior(model);
  if display > 1
    [ll, oldll] = boundCheck(model, ll, 'Prior Update');
  end
  model = mogUpdateMean(model);
  if display > 1
    [ll, oldll] = boundCheck(model, ll, 'Mean Update');
  end
  model = mogUpdateCovariance(model);
  if display > 1
    [ll, oldll] = boundCheck(model, ll, 'Covariance Update');
  end
  if display > 1
  else
    oldll = ll;
    ll = mogLowerBound(model);
  end
  diffll = ll -oldll;
  if display
    fprintf('Iteration %d log-likelihood: %2.6f\n', iter, ll)
  end
end

function [ll, oldll] = boundCheck(model, oldll, step)

% BOUNDCHECK Helper function for checking bound.

ll = mogLowerBound(model);
diffll = ll - oldll;
if ll -oldll < 0
  warning(['Log likelihood went down by ' num2str(diffll) 'in ' ...
           'step: ' step ' in mogOptimise'])
end


function [Xopt,yopt,Sopt,obj] = recover_mosek_sol(res,n)
idxtril         = tril(true(n,n));
Xopt            = zeros(n,n);
Xopt(idxtril)   = res.sol.itr.barx;
Xopt            = Xopt + Xopt';
Xopt            = Xopt - 0.5*diag(diag(Xopt));
yopt            = res.sol.itr.y;
Sopt            = zeros(n,n);
Sopt(idxtril)   = res.sol.itr.bars;
Sopt            = Sopt + Sopt';
Sopt            = Sopt - 0.5*diag(diag(Sopt));
obj             = [res.sol.itr.pobjval;res.sol.itr.dobjval];
end


function monotonicity = DeriveMonotonicityFromStationary(properties,xL,xU)

monotonicity = 'none';
if isequal(properties.convexity,'convex')
    if xU <= properties.stationary(1)
        monotonicity = 'decreasing';
    elseif xL >= properties.stationary(1)
        monotonicity = 'increasing';
    end
elseif isequal(properties.convexity,'concave')
    if xU <= properties.stationary(1)
        monotonicity = 'increasing';
    elseif xL >= properties.stationary(1)
        monotonicity = 'decreasing';
    end
end


function acc=svmTest(W,H,text_feature,data,train_ratio,C)
    load([data,'/group.txt']);
    numOfNode = size(group,1);
    if strcmp(data,'citeseer')
        numOfGroup = 6;
    elseif strcmp(data,'cora')
        numOfGroup = 7;
    else
        numOfGroup = 19;
    end

    group(:,1) = group(:,1) + 1;
    if strcmp(data,'wiki')==0
        group(:,2) = group(:,2) + 1;
    end
    group = sparse(group(:,1),group(:,2),ones(size(group(:,1))),numOfNode,numOfGroup);

    grouptmp=group;
    acc=0;
    features = [W' text_feature*H'];    % representation learned by TADW
    for i=1:size(features,2)
        if (norm(features(:,i))>0)
            features(:,i) = features(:,i)/norm(features(:,i));
        end
    end

    for i=1:10  % do the procedure for 10 times and take the average
        rp = randperm(numOfNode);
        testId = rp(1:floor(numOfNode*(1-train_ratio)));

        groupTest = group(testId,:);
        group(testId,:)=[];

        trainId = [1:numOfNode]';
        trainId(testId,:)=[];

        result=SocioDim(features, group, trainId, testId, C);
        [res b] = evaluate(result,groupTest);
        acc=acc+res.micro_F1;
        group=grouptmp;
    end
    acc=acc/10;


function [p] = mesh_scalp_interp(p)

% mesh_scalp_interp - Interpolates scalar over scalp mesh from electrodes
%
% Usage: [p] = mesh_scalp_interp(p)
%
% p is the eeg_toolbox struct (see eeg_toolbox_defaults)
%
% This function returns the laplacian matrix of the scalp
% mesh, the interpolation matrix based on the min norm of
% the laplacian, and the voltage interpolated on the scalp.
% It also calls an electrode "coregistration" function and
% the results of that operation.
%
% The return matrices are all in p.mesh.data, including the
% interpolated timeseries values in p.mesh.data.timeseries{N},
% where N is the index of the scalp mesh in this case (see
% mesh_check to get N).
%
% This function is called by eeg_contours_engine.
%
% See also, mesh_fit_elec, mesh_laplacian, mesh_laplacian_interp
% for more information.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  Gnu GPL, no express or implied warranties
% History:  09/02, Darren.Weber_at_radiology.ucsf.edu
%                  extracted out of MESH_SCALP_INTERP, called by that
%                  function.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('p','var'),
    fprintf('Setting default parameters.\n');
   [p] = eeg_toolbox_defaults('create');
end

% Calculate voltage at scalp mesh vertices
if isempty(p.mesh.data),
    if ~isempty(p.mesh.file),
       [p] = mesh_open(p);
    else
        msg = sprintf('MESH_SCALP_INTERP: No meshes, load meshes first.');
        warning(msg);
    end
end

% get the scalp mesh
[p.mesh.current,meshExists] = mesh_check(p,'scalp');

if isempty(p.mesh.current),
    msg = sprintf('MESH_SCALP_INTERP: No ''scalp'' mesh, check p.mesh.data.meshtype.');
    error(msg);
end

% Find nearest vertices of scalp mesh to electrodes
% This will also reorder the scalp mesh vertices/faces
% so that the first 1:Nelec vertices are the vertices
% in the scalp that lie closest to the electrodes.
% Serious problems arise if there are any duplicate
% scalp vertices for any electrode, but this routine
% will continue regardless, at present.
p = mesh_fit_elec(p);

% Ensure scalp is the currently selected mesh
elecN = mesh_check(p,'elec');
scalpN = mesh_check(p,'scalp');
[p.mesh.current,meshExists] = mesh_check(p,'scalp');

% Get vertices and faces of scalp mesh, after they
% have been rearraned by mesh_fit_elec
scalpvert = p.mesh.data.vertices{scalpN};
scalpface = p.mesh.data.faces{scalpN};

% Get the scalp vertices and faces
% that correspond to the electrodes
elecvert  = p.mesh.data.vertices{elecN};
elecface  = p.mesh.data.faces{elecN};
% Get the index of each electrode vertex
% into the scalp mesh, should be first
% 1:Nelec indices of scalp vertices
elecindex = p.mesh.data.elecindex{scalpN};

fprintf('MESH_SCALP_INTERP: Calculating Scalp Interpolation\n');

% Could identify the elements of the scalp below
% the electrodes and discard them before calculating
% the interpolation matrices, or use them in the
% interpolation, but set their potential to zero
% later (see comment below).

% Calculate the Laplacian matrix of the scalp mesh (once off calculation)
if isfield(p.mesh.data,'lapmat'),
    if isempty(p.mesh.data.lapmat{scalpN}),
        p.mesh.data.lapmat{scalpN} = mesh_laplacian(scalpvert,scalpface);
    end
else
    p.mesh.data.lapmat{scalpN} = mesh_laplacian(scalpvert,scalpface);
end

% Calculate the electrode to scalp mesh interpolation matrix
% using a Laplacian minimum norm constraint (once off calculation)
if isfield(p.mesh.data,'lapint'),
    if isempty(p.mesh.data.lapint{scalpN}),
        [p.mesh.data.lapint{scalpN}, p.mesh.data.keepindex{scalpN}, p.mesh.data.repindex{scalpN}] = mesh_laplacian_interp(p.mesh.data.lapmat{scalpN}, elecindex);
    end
else
    [p.mesh.data.lapint{scalpN}, p.mesh.data.keepindex{scalpN}, p.mesh.data.repindex{scalpN}] = mesh_laplacian_interp(p.mesh.data.lapmat{scalpN}, elecindex);
end

fprintf('MESH_SCALP_INTERP: Multiplying Voltage by Interpolation matrix...\n');
tic;
% Interpolate voltage to all scalp mesh vertices (once off calc)
if isempty(p.mesh.data.repindex{scalpN}),
    p.mesh.data.Cdata{scalpN} = p.mesh.data.lapint{scalpN} * p.volt.data';
    %p.mesh.data.Cdata{scalpN} = p.volt.data * p.mesh.data.lapint{scalpN}';
else
    p.mesh.data.Cdata{scalpN} = p.mesh.data.lapint{scalpN} * p.volt.data(:,p.mesh.data.keepindex{scalpN})';
    %p.mesh.data.Cdata{scalpN} = p.volt.data(:,p.mesh.data.keepindex{scalpN}) * p.mesh.data.lapint{scalpN}';
end
t = toc;
fprintf('done (%6.2f sec).\n',t);

% Now a quick test of the interpolation: the values at the first
% Nelec vertices of the scalp mesh must be equal to the values
% of the original voltage data
TMP = p.mesh.data.Cdata{scalpN};
Nelec = size(p.volt.data,2);
if ~isequal(p.volt.data',TMP(1:Nelec,:)),
    msg = sprintf('MESH_SCALP_INTERP: Fatal Error in Mesh Voltage Interpolation, Reload all Data?\n');
    error(msg);
end

% At present, all scalp vertices are interpolated.  It might
% be useful at this point to determine all vertices that are
% "well below" the electrode array, say > 5cm from a nearest
% electrode and then set their voltage to zero.

return


function ans = calc_Dx(doseBinsV, volsHistV, x, volType)
% Returns the lowest dose in x% of structure'
% given the DVH data and the parameter percent.
%
%  MODIFICATION ALERT:  THIS FUNCTION IS UTILIZED BY THE DREXLER CODEBASE
%
%  Last modified: AJH 11/05
%
%  Usage: calc_Dx(doseBinsV, volsHistV, percent)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

if isstruct(x)  %for use with ROE
    volType = x.volType.val;
    x = x.x.val;
end

if ~exist('volType','var')
    %warning('Input volume assumed to be in percentage. Set volType=0 for absolute values.');
else
    if ~volType
    x = x/sum(volsHistV)*100;
    end
end

cumVolsV = cumsum(volsHistV);
	cumVols2V = cumVolsV(end) - cumVolsV;
	ind = min(find([ cumVols2V/cumVolsV(end) < x/100 ])); %CHANGED
	if isempty(ind)
        ans = 0;
	else
    	ans = doseBinsV(ind);
	end
return;


function [grains,grainId,mis2mean] = calcGrains(ebsd,varargin)
% grains reconstruction from 2d EBSD data
%
% Syntax
%
%   [grains, ebsd.grainId] = calcGrains(ebsd,'angle',10*degree)
%
%   % reconstruction low and high angle grain boundaries
%   lagb = 2*degree;
%   hagb = 10*degree;
%   grains = calcGrains(ebsd,'angle',[hagb lagb])
%
%   % allow grains to grow into not indexed regions
%   grains = calcGrains(ebsd('indexed'),'angle',10*degree)
%
%   % do not allow grains to grow into not indexed regions
%   grains = calcGrains(ebsd,'unitCell')
%
%   % follow non convex outer boundary
%   grains = calcGrains(ebsd,'boundary','tight')
%
%   % specify phase dependent thresholds
%   % thresholds follow the same order as ebsd.CSList and should have the same length
%   grains = calcGrains(ebsd,'angle',{angl_1 angle_2 angle_3})
%
%   % markovian clustering algorithm
%   p = 1.5;    % inflation power (default = 1.4)
%   maxIt = 10; % number of iterations (default = 4)
%   delta = 5*degree % variance of the threshold angle
%   grains = calcGrains(ebsd,'method','mcl',[p maxIt],'soft',[angle delta])
%
% Input
%  ebsd   - @EBSD
%
% Output
%  grains       - @grain2d
%  ebsd.grainId - grainId of each pixel
%
% Options
%  threshold, angle - array of threshold angles per phase of mis/disorientation in radians
%  boundary         - bounds the spatial domain ('convexhull', 'tight')
%  maxDist          - maximum distance to for two pixels to be in one grain (default inf)
%  fmc       - fast multiscale clustering method
%  mcl       - markovian clustering algorithm
%  custom    - use a custom property for grain separation
%
% Flags
%  unitCell - omit voronoi decomposition and treat a unitcell lattice
%  qhull    - use qHull for the voronin decomposition
%
% References
%
% * F.Bachmann, R. Hielscher, H. Schaeben, Grain detection from 2d and 3d
% EBSD data - Specification of the MTEX algorithm: Ultramicroscopy, 111,
% 1720-1733, 2011
%
% * C. McMahon, B. Soe, A. Loeb, A. Vemulkar, M. Ferry, L. Bassman,
%   Boundary identification in EBSD data with a generalization of fast
%   multiscale clustering, <https://doi.org/10.1016/j.ultramic.2013.04.009
%   Ultramicroscopy, 2013, 133:16-25>.
%
% See also
% GrainReconstruction GrainReconstructionAdvanced

% subdivide the domain into cells according to the measurement locations,
% i.e. by Voronoi teselation or unit cell
[V,F,I_FD] = spatialDecomposition([ebsd.prop.x(:), ebsd.prop.y(:)],ebsd.unitCell,varargin{:});
% V - list of vertices
% F - list of faces
% D - cell array of cells
% I_FD - incidence matrix faces to vertices

% determine which cells to connect
[A_Db,I_DG] = doSegmentation(I_FD,ebsd,varargin{:});
% A_db - neigbhouring cells with (inner) grain boundary
% I_DG - incidence matrix cells to grains

% compute grain ids
[grainId,~] = find(I_DG.');

% phaseId of each grain
phaseId = full(max(I_DG' * ...
  spdiags(ebsd.phaseId,0,numel(ebsd.phaseId),numel(ebsd.phaseId)),[],2));
phaseId(phaseId==0) = 1; % why this is needed?

% compute boundary this gives
% I_FDext - faces x cells external grain boundaries
% I_FDint - faces x cells internal grain boundaries
[I_FDext, I_FDint, Fext, Fint] = calcBoundary;

if check_option(varargin,'removeQuadruplePoints')
  qAdded = removeQuadruplePoints;
end

% setup grains
grains = grain2d( makeBoundary(Fext,I_FDext), ...
  calcPolygons(I_FDext * I_DG,Fext,V), ...
  [], ebsd.CSList, phaseId, ebsd.phaseMap, varargin{:});

grains.grainSize = full(sum(I_DG,1)).';
grains.innerBoundary = makeBoundary(Fint,I_FDint);
grains.scanUnit = ebsd.scanUnit;

% merge quadruple grains
if check_option(varargin,'removeQuadruplePoints') && qAdded > 0
  mergeQuadrupleGrains;
end

% calc mean orientations, GOS and mis2mean
% ----------------------------------------

[d,g] = find(I_DG);

grainRange    = [0;cumsum(grains.grainSize)];        %
firstD        = d(grainRange(2:end));
q             = quaternion(ebsd.rotations);
meanRotation  = q(firstD);
GOS = zeros(length(grains),1);

% choose between equivalent orientations in one grain such that all are
% close together
for pId = grains.indexedPhasesId
  ndx = ebsd.phaseId(d) == pId;
  if ~any(ndx), continue; end
  q(d(ndx)) = project2FundamentalRegion(q(d(ndx)),ebsd.CSList{pId},meanRotation(g(ndx)));
end

% TODO: this can be done more efficiently using accumarray

% compute mean orientation and GOS
doMeanCalc = find(grains.grainSize>1 & grains.isIndexed);
for k = 1:numel(doMeanCalc)
  qind = subSet(q,d(grainRange(doMeanCalc(k))+1:grainRange(doMeanCalc(k)+1)));
  mq = mean(qind,'robust');
  meanRotation = setSubSet(meanRotation,doMeanCalc(k),mq);
  GOS(doMeanCalc(k)) = mean(angle(mq,qind));
end

% save
grains.prop.GOS = GOS;
grains.prop.meanRotation = reshape(meanRotation,[],1);
mis2mean = inv(rotation(q(:))) .* grains.prop.meanRotation(grainId(:));

% assign variant and parent Ids for variant-based grain computation
if check_option(varargin,'variants')
    variantId = get_option(varargin,'variants');
    grains.prop.variantId = variantId(firstD,1);
    grains.prop.parentId = variantId(firstD,2);
end

  function [A_Db,I_DG] = doSegmentation(I_FD,ebsd,varargin)
    % segmentation
    %
    %
    % Output
    %  A_Db - adjecency matrix of grain boundaries
    %  A_Do - adjecency matrix inside grain connections

    % extract segmentation method
    grainBoundaryCiterions = dir([mtex_path '/EBSDAnalysis/@EBSD/private/gbc*.m']);
    grainBoundaryCiterions = {grainBoundaryCiterions.name};
    gbcFlags = regexprep(grainBoundaryCiterions,'gbc_(\w*)\.m','$1');

    gbc      = get_flag(varargin,gbcFlags,'angle');
    gbcValue = ensurecell(get_option(varargin,{gbc,'threshold','delta'},15*degree,{'double','cell'}));

    if numel(gbcValue) == 1 && length(ebsd.CSList) > 1
      gbcValue = repmat(gbcValue,size(ebsd.CSList));
    end

    % get pairs of neighbouring cells {D_l,D_r} in A_D
    A_D = I_FD'*I_FD==1;
    [Dl,Dr] = find(triu(A_D,1));

    if check_option(varargin,'maxDist')
      xyDist = sqrt((ebsd.prop.x(Dl)-ebsd.prop.x(Dr)).^2 + ...
        (ebsd.prop.y(Dl)-ebsd.prop.y(Dr)).^2);

      dx = sqrt(sum((max(ebsd.unitCell)-min(ebsd.unitCell)).^2));
      maxDist = get_option(varargin,'maxDist',3*dx);
      % maxDist = get_option(varargin,'maxDist',inf);
    else
      maxDist = 0;
    end

    connect = zeros(size(Dl));

    for p = 1:numel(ebsd.phaseMap)

      % neighboured cells Dl and Dr have the same phase
      if maxDist > 0
        ndx = ebsd.phaseId(Dl) == p & ebsd.phaseId(Dr) == p & xyDist < maxDist;
      else
        ndx = ebsd.phaseId(Dl) == p & ebsd.phaseId(Dr) == p;
      end

      connect(ndx) = true;

      % check, whether they are indexed
      ndx = ndx & ebsd.isIndexed(Dl) & ebsd.isIndexed(Dr);

      % now check for the grain boundary criterion
      if any(ndx)

        connect(ndx) = feval(['gbc_' gbc],...
          ebsd.rotations,ebsd.CSList{p},Dl(ndx),Dr(ndx),gbcValue{p},varargin{:});

      end
    end

    % adjacency of cells that have no common boundary
    ind = connect>0;
    A_Do = sparse(double(Dl(ind)),double(Dr(ind)),connect(ind),length(ebsd),length(ebsd));
    if check_option(varargin,'mcl')

      param = get_option(varargin,'mcl');
      if isempty(param), param = 1.4; end
      if length(param) == 1, param = [param,4]; end

      A_Do = mclComponents(A_Do,param(1),param(2));
      A_Db = sparse(double(Dl),double(Dr),true,length(ebsd),length(ebsd));
      A_Db(A_Do~=0) = false;

    else

      A_Db = sparse(double(Dl(connect<1)),double(Dr(connect<1)),true,...
        length(ebsd),length(ebsd));

    end
    A_Do = A_Do | A_Do.';

    % adjacency of cells that have a common boundary
    A_Db = A_Db | A_Db.';

    % compute I_DG connected components of A_Do
    % I_DG - incidence matrix cells to grains
    I_DG = sparse(1:length(ebsd),double(connectedComponents(A_Do)),1);

  end

  function [I_FDext, I_FDint, Fext, Fint] = calcBoundary
    % distinguish between interior and exterior grain boundaries

    % cells that have a subgrain boundary, i.e. a boundary with a cell
    % belonging to the same grain
    sub = ((A_Db * I_DG) & I_DG)';                 % grains x cell
    [i,j] = find( diag(any(sub,1))*double(A_Db) ); % all adjacence to those
    sub = any(sub(:,i) & sub(:,j),1);              % pairs in a grain

    % split grain boundaries A_Db into interior and exterior
    A_Db_int = sparse(i(sub),j(sub),1,size(I_DG,1),size(I_DG,1));
    A_Db_ext = A_Db - A_Db_int;                    % adjacent over grain boundray

    % create incidence graphs
    I_FDbg = diag( sum(I_FD,2)==1 ) * I_FD;
    D_Fbg  = diag(any(I_FDbg,2));

    [ix,iy] = find(A_Db_ext);
    D_Fext  = diag(sum(abs(I_FD(:,ix)) & abs(I_FD(:,iy)),2)>0);

    I_FDext = (D_Fext| D_Fbg)*I_FD;

    [ix,iy] = find(A_Db_int);
    D_Fsub  = diag(sum(abs(I_FD(:,ix)) & abs(I_FD(:,iy)),2)>0);
    I_FDint = D_Fsub*I_FD;

    % remove empty lines from I_FD, F, and V
    isExt = full(any(I_FDext,2));
    I_FDext = I_FDext.'; I_FDext = I_FDext(:,isExt).';

    isInt = full(any(I_FDint,2));
    I_FDint = I_FDint.'; I_FDint = I_FDint(:,isInt).';

    % remove vertices that are not needed anymore
    [inUse,~,F] = unique(F(isExt | isInt,:));
    V = V(inUse,:);
    F = reshape(F,[],2);
    Fext = F(isExt(isExt | isInt),:); % external boundary segments
    Fint = F(isInt(isExt | isInt),:); % internal boundary segments

  end

  function gB = makeBoundary(F,I_FD)

    % compute ebsdInd
    [eId,fId] = find(I_FD.');
    eId = eId(:); fId = fId(:);

    % replace fId that appears a second time by fId + length(F)+1
    % such that it refers to the second column
    d = diff([0;fId]);
    fId = cumsum(d>0) + (d==0)*size(F,1);

    %  ebsdInd - [Id1,Id2] list of adjecent EBSD pixels for each segment
    ebsdInd = zeros(size(F,1),2);
    ebsdInd(fId) = eId;

    % compute misrotations
    mori = rotation.nan(size(F,1),1);
    isNotBoundary = all(ebsdInd,2);
    mori(isNotBoundary) = ...
      inv(ebsd.rotations(ebsdInd(isNotBoundary,2))) ...
      .* ebsd.rotations(ebsdInd(isNotBoundary,1));

    gB = grainBoundary(V,F,ebsdInd,grainId,ebsd.phaseId,mori,ebsd.CSList,ebsd.phaseMap,ebsd.id);

  end

  function qAdded = removeQuadruplePoints

    quadPoints = find(accumarray(reshape(Fext(full(any(I_FDext,2)),:),[],1),1) == 4);
    qAdded = 0;

    if isempty(quadPoints), return; end

    % find the 4 edges connected to the quadpoints
    I_FV = sparse(repmat((1:size(Fext,1)).',1,2),Fext,ones(size(Fext)));

    quadPoints = find(sum(I_FV) == 4).';
    [iqF,~] = find(I_FV(:,quadPoints));

    % this is a length(quadPoints x 4 list of edges
    iqF = reshape(iqF,4,length(quadPoints)).';

    % find the 4 vertices adfacent to each quadruple point
    qV = [Fext(iqF.',1).';Fext(iqF.',2).'];
    qV = qV(qV ~= reshape(repmat(quadPoints.',8,1),2,[]));
    qV = reshape(qV,4,[]).';

    % compute angle with respect to quadruple point
    qOmega = reshape(atan2(V(qV,1) - V(repmat(quadPoints,1,4),1),...
      V(qV,2) - V(repmat(quadPoints,1,4),2)),[],4);

    % sort the angles
    [~,qOrder] = sort(qOmega,2);

    % find common pixels for pairs of edges - first we try 1/4 and 2/3
    s = size(iqF);
    orderSub = @(i) sub2ind(s,(1:s(1)).',qOrder(:,i));

    iqD = I_FDext(iqF(orderSub(1)),:) .* I_FDext(iqF(orderSub(4)),:) + ...
      I_FDext(iqF(orderSub(2)),:) .* I_FDext(iqF(orderSub(3)),:);

    % if not both have one common pixel
    switchOrder = full(sum(iqD,2))~= 2;

    % switch to 3/4 and 1/2
    qOrder(switchOrder,:) = qOrder(switchOrder,[4 1 2 3]);
    orderSub = @(i) sub2ind(s,(1:s(1)).',qOrder(:,i));

    iqD = I_FDext(iqF(orderSub(1)),:) .* I_FDext(iqF(orderSub(4)),:) + ...
      I_FDext(iqF(orderSub(2)),:) .* I_FDext(iqF(orderSub(3)),:);

    % some we will not be able to remove
    ignore = full(sum(iqD,2)) ~= 2;
    iqD(ignore,:) = [];
    quadPoints(ignore) = [];
    iqF(ignore,:) = [];
    qV(ignore,:) = [];
    qOrder(ignore,:) = [];
    s = size(iqF);
    orderSub = @(i) sub2ind(s,(1:s(1)).',qOrder(:,i));

    % add an additional vertex (with the same coordinates) for each quad point
    newVid = (size(V,1) + (1:length(quadPoints))).';
    V = [V;V(quadPoints,:)];

    % include new vertex into face list, i.e. replace quadpoint -> newVid
    Ftmp = Fext(iqF(orderSub(1)),:).';
    Ftmp(Ftmp == quadPoints.') = newVid;
    Fext(iqF(orderSub(1)),:) = Ftmp.';

    Ftmp = Fext(iqF(orderSub(2)),:).';
    Ftmp(Ftmp == quadPoints.') = newVid;
    Fext(iqF(orderSub(2)),:) = Ftmp.';

    %F(iqF(orderSub(1)),:) = [qV(orderSub(1)),newVid];
    %F(iqF(orderSub(2)),:) = [newVid,qV(orderSub(2))];
    sw = Fext(:,1) > Fext(:,2);
    Fext(sw,:) = fliplr(Fext(sw,:));

    [iqD,~] = find(iqD.'); iqD = reshape(iqD,2,[]).';

    % if we have different grains - we need a new boundary
    newBd = full(sum(I_DG(iqD(:,1),:) .* I_DG(iqD(:,2),:),2)) == 0;

    % add new edges
    Fext = [Fext; [quadPoints(newBd),newVid(newBd)]];
    qAdded = sum(newBd);

    % new rows to I_FDext
    I_FDext = [I_FDext; ...
      sparse(repmat((1:qAdded).',1,2), iqD(newBd,:), 1, ...
      qAdded,size(I_FDext,2))];

    % new empty rows to I_FDint
    %I_FDint = [I_FDint; sparse(qAdded,size(I_FDint,2))];

  end

  function mergeQuadrupleGrains

    gB = grains.boundary; gB = gB(length(gB)+1-(1:qAdded));
    toMerge = false(size(gB));

    for iPhase = ebsd.indexedPhasesId

      % restrict to the same phase
      iBd = all(gB.phaseId == iPhase,2);

      if ~any(iBd), continue; end

      % check for misorientation angle % TODO
      toMerge(iBd) = angle(gB(iBd).misorientation) < 5 * degree;

    end

    [grains, parentId] = merge(grains,gB(toMerge));

    % update I_DG
    I_PC = sparse(1:length(parentId),parentId,1);
    I_DG = I_DG * I_PC;

    % update grain ids
    [grainId,~] = find(I_DG.');
  end

end

function poly = calcPolygons(I_FG,F,V)
%
% Input:
%  I_FG - incidence matrix faces to grains
%  F    - list of faces
%  V    - list of vertices

poly = cell(size(I_FG,2),1);

if isempty(I_FG), return; end

% for all grains
for k=1:size(I_FG,2)

  % inner and outer boundaries are circles in the face graph
  EC = EulerCycles(F(I_FG(:,k)>0,:));

  % first cicle should be positive and all others negatively oriented
  for c = 1:numel(EC)
    if xor( c==1 , polySgnArea(V(EC{c},1),V(EC{c},2))>0 )
      EC{c} = fliplr(EC{c});
    end
  end

  % this is needed
  for c=2:numel(EC), EC{c} = [EC{c} EC{1}(1)]; end

  poly{k} = [EC{:}];

end

end


function [DataOut, Metric] = ReceiverSD(SDchips, G, Scrambler);
%
% ReceiverSD    Soft Decision Receiver. This function performs transmitted packet data recovering,
%                          based on Viterbi Decoder.
%
% 						Block Diagram:
%						SDchips -> [DeScrambler] -> [De-Interleaver] -> [Viterbi Decoder]
%
%
% 						Inputs: SDchips - Soft Decision RAKE receiver symbols
%                         G - Viterbi Encoder generation polynom
%                         Scrambler - Scrambler sequence
%
% 						Outputs: DataOut - Received data (binary form)
%                                       Metric - The  best metric of Viterbi Decoder
%

%====================== COMMON DEFINITIONS =================
%------- Viterbi Polynom ---------
if (nargin == 1)
   G = [1 1 1 1 0 1 0 1 1; 1 0 1 1 1 0 0 0 1];
end

%====================== R E C E I V E R =================

%----------- DeScrambler---------
% Rate = 19.2 KBps
SDchips = SDchips.*sign(1/2-Scrambler);

%-------- DeInterleaver ---------
INTERL = reshape(SDchips, 16, 24);		% IN-> rows, OUT-> columns
SDchips = reshape(INTERL', length(SDchips), 1);  % Rate = 19.2 KBps

%-------- SD Input Viterbi Decoder ----------
[DataOut Metric] = SoftVitDec(G, SDchips, 1);


%% brute_force_tune
% Code to tune parameters by brute force. This is for heading init.
% Obviously theoretically completely silly.
%
% Works sorta like RANSAC I guess?
%
% Adam Werries 2016, see Apache 2.0 license.

k_max = 60;
% Specify ranges
roll = linspace(-45,45,100);
pitch = linspace(-45,45,100);
yaw = linspace(-180,180,100);
% Repeat arrays
roll = repmat(roll, [1 k_max]);
pitch = repmat(pitch, [1 k_max]);
yaw = repmat(yaw, [1 k_max]);
% Generate random selections of each vector
num_items = length(roll);
roll_i = randperm(num_items);
pitch_i = randperm(num_items);
yaw_i = randperm(num_items);
rms_error_filter = zeros(1,num_items);
max_error_filter = zeros(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, Roll: %08.5f, Pitch: %08.5f, Yaw: %08.5f\n', i, roll(roll_i(i)), pitch(pitch_i(i)), yaw(yaw_i(i)));
    temp_cond = init_cond;
    temp_cond(7:9) = [deg2rad(roll(roll_i(i))) deg2rad(pitch(pitch_i(i))) deg2rad(yaw(yaw_i(i)))];
    [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(temp_cond, filter_time, epoch, lla, novatel, imu, LC_KF_config, est_IMU_bias);
    xyz = out_profile(:,2:4);
    llh = ecef2lla(xyz);
    [x,y] = deg2utm(llh(:,1),llh(:,2));
    x = x-min_x;
    y = y-min_y;

    distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
    rms_error_filter(i) = rms(distance);
    max_error_filter(i) = max(distance);
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.5f, rms is %08.5f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, Roll: %08.5f, Pitch: %08.5f, Yaw: %08.5f\n', i, roll(roll_i(i)), pitch(pitch_i(i)), yaw(yaw_i(i)));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.5f, max is %08.5f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, Roll: %08.5f, Pitch: %08.5f, Yaw: %08.5f\n', i, roll(roll_i(i)), pitch(pitch_i(i)), yaw(yaw_i(i)));
[minrms, i] = min((rms_error_filter+max_error_filter)/2);
fprintf('Best average of RMS and max: %08.4f, rms is  %08.4f, max is %08.4f\n', minrms, rms_error_filter(i), max_error_filter(i));
fprintf('Best iteration for rms: %d, Roll: %08.5f, Pitch: %08.5f, Yaw: %08.5f\n', i, roll(roll_i(i)), pitch(pitch_i(i)), yaw(yaw_i(i)));


function [y]=axpx(a,x)
%[y]=AXPX(A,X)
%This is a technical function that multiplies
%Matrix A by a vector and projects on X itself;
%This is for time-stepping

d=x.d;
n=a.n;
m=a.m;
rx=x.r;
psx=x.ps;
att=a.tt;
corea=att.core;
psa=att.ps;
ra=att.r;
%Do right-to-left orthogonalization and computation of psi
%matrices
corex=x.core;
pos1=psx(d+1);
psi=cell(d+1,1); %Psi-matrices
psi{d+1}=1; psi{1}=1;
cr1=corex(psx(d):psx(d+1)-1);
y=x;
corey=y.core;
psy=y.ps;
ry=y.r;
for i=d:-1:2
   %  fprintf('i=%d \n');
   cr2=corey(psy(i-1):psy(i)-1);
   cr1=reshape(cr1,[ry(i),n(i)*ry(i+1)]);
   cr2=reshape(cr2,[ry(i-1)*n(i-1),ry(i)]);
   cr1=cr1.';
   [q,rm]=qr(cr1,0); rn=size(q,2); rm=rm.';
   q=q.';
   ry(i)=rn;
   corey(pos1-ry(i+1)*n(i)*ry(i):pos1-1)=q(:);
      %Convolution is now performed for psi(i) using psi(i+1) and corea, and
   %(new) core q
   cra=corea(psa(i):psa(i+1)-1); cra=reshape(cra,[ra(i),n(i),m(i),ra(i+1)]);
   cry=reshape(conj(q),[ry(i),n(i),ry(i+1)]);
   crx=corex(psx(i):psx(i+1)-1);
   crx=reshape(crx,[rx(i),m(i),rx(i+1)]);
   pscur=psi{i+1}; pscur=reshape(pscur,[ra(i+1),rx(i+1),ry(i+1)]); %ra,rx,ry
   %First, convolve over rx(i+1)
   crx=reshape(crx,[rx(i)*m(i),rx(i+1)]);
    pscur=permute(pscur,[2,1,3]); pscur=reshape(pscur,[rx(i+1),ra(i+1)*ry(i+1)]);
   pscur=crx*pscur; %pscur is now rx(i)*m(i)*ra(i+1)*ry(i+1)
   %Convolve over m(i),ra(i+1),n(i),ry(i+1)
    pscur=reshape(pscur,[rx(i),m(i)*ra(i+1),ry(i+1)]);
    pscur=permute(pscur,[1,3,2]);
    pscur=reshape(pscur,[rx(i)*ry(i+1),m(i)*ra(i+1)]);
    cra=reshape(cra,[ra(i)*n(i),m(i)*ra(i+1)]); cra=cra.';
    pscur=pscur*cra;
    %pscur is now rx(i)*ry(i+1)*ra(i)*n(i), it is left to convolve over
    %n(i)*ry(i+1)
    pscur=reshape(pscur,[rx(i),ry(i+1),ra(i),n(i)]);
    pscur=permute(pscur,[3,1,4,2]);
    pscur=reshape(pscur,[ra(i)*rx(i),n(i)*ry(i+1)]);
    cry=reshape(cry,[ry(i),n(i)*ry(i+1)]); cry=cry.';
    pscur=pscur*cry; %pscur is ra*rx*ry
    pscur=reshape(pscur,[rx(i),ra(i),ry(i)]);
    psi{i}=pscur;
   %End of psi-block
   pos1=pos1-ry(i+1)*n(i)*ry(i);
   cr1=cr2*rm;
end
corey(pos1-ry(2)*n(1)*ry(1):pos1-1)=cr1(:);
pos1=pos1-ry(2)*n(1)*ry(1);
corey=corey(pos1:end); %Truncate unused elements
%Now compute the projection itself while recomputing psi-matrices
 pos1=1;
 psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
 rm=1;
 %keyboard;
  for i=1:d
          %Our convolution is
     %ps1(ra(i),rx(i),ry(i))*cra(ra(i),n(i),m(i),ra(i+1))
     %*ps2(ra(i+1),rx(i+1),ry(i+1))*crx(rx(i),m(i)*rx(i+1))->
     %cry(ry(i),n(i),ry(i+1)

     ps1=psi{i}; ps2=psi{i+1};
     cra=corea(psa(i):psa(i+1)-1);
     crx=corex(psx(i):psx(i+1)-1);
%     corey(psy(i):psy(i+1)-1)=crx(:);
%     y.core=corey;
%          psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
%     y.ps=psy;
     ps1=reshape(ps1,[ra(i),rx(i),ry(i)]);
     ps1=permute(ps1,[2,3,1]);
     ps1=reshape(ps1,[rx(i)*ry(i),ra(i)]);
     cra=reshape(cra,[ra(i),n(i)*m(i)*ra(i+1)]);
     cry=ps1*cra;      %cry is rx(i)*ry(i)*n(i)*m(i)*ra(i+1)
     %convolve over m(i),rx(i) with crx
     cry=reshape(cry,[rx(i),ry(i),n(i),m(i),ra(i+1)]);
     cry=permute(cry,[2,3,5,1,4]);
     cry=reshape(cry,[ry(i)*n(i)*ra(i+1),rx(i)*m(i)]);
     crx=reshape(crx,[rx(i)*m(i),rx(i+1)]);
     cry=cry*crx; cry0=cry;
     %cry is ry(i)*n(i)*ra(i+1)*rx(i+1)
     cry=reshape(cry,[ry(i)*n(i),ra(i+1)*rx(i+1)]);
     ps2=reshape(ps2,[ra(i+1)*rx(i+1),ry(i+1)]);
     %cry0=cry; %cry0 is ry(i)*n(i)*ra(i+1)*rx(i+1), convolution over ry(i)*n(i) will bring new psi --- not true; new psi will be brought with and
     %orthogonalization
     cry=cry*ps2;
      corey(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=cry(:);
      y.core=corey;
      psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
      y.ps=psy;
       %keyboard;

     cry=reshape(cry,[ry(i)*n(i),ry(i+1)]);
     [q,rm]=qr(cry,0);
     rn=size(q,2);
     ry(i+1)=rn;
     %q=q*rm;
     corey(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=q(:);
     pos1=pos1+ry(i)*n(i)*ry(i+1);
     %And now --- orthogonalization! (of cry) (he-he)
     %Now compute "new" psi
     %We need to compute
          %cry is rx(i)*ry(i)*n(i)*m(i)*ra(i+1)
     %convolve over m(i),rx(i) with crx
     %cry0 is ry(i)*n(i)*ra(i+1)*rx(i+1);
     cry0=reshape(cry0,[ry(i),n(i),ra(i+1),rx(i+1)]);
     cry0=permute(cry0,[3,4,1,2]);
     cry0=reshape(cry0,[ra(i+1)*rx(i+1),ry(i)*n(i)]);
     q=reshape(conj(q),[ry(i)*n(i),ry(i+1)]);
     psi{i+1}=cry0*q;

  end
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
  corey(psy(i):psy(i+1)-1)=corey(psy(i):psy(i+1)-1)*rm;
  y.core=corey;
  y.r=ry;
  y.ps=psy;
  y.n=n;
  y.d=d;
  return
end


function Tbig = multiply_by_pot(Tbig, Tsmall)
% MULTIPLY_BY_POT Tbig *= Tsmall
% Tbig = multiply_by_pot(Tbig, Tsmall)
%
% Tsmall's domain must be a subset of Tbig's domain.

smallp = extend_domain_table(Tsmall.p, Tsmall.domain, Tsmall.sizes, Tbig.domain, Tbig.sizes);
Tbig.p = Tbig.p .* smallp;

smallu = extend_domain_table(Tsmall.u, Tsmall.domain, Tsmall.sizes, Tbig.domain, Tbig.sizes);
Tbig.u = Tbig.u + smallu;


filename='Cantilever_triangle_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.4;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function h = cutfunc(x,a,b)
c1 = (x <= a);
c2 = (x>a & x<b);
c3 = (x >= b);
h = 0*c1+ (x-a)/(b-a).*c2 + 1*c3;
% c1 = (x <= a);
% c2 = (x>a & x<b);
% c3 = (x >= b);
% h = 0*c1+ 0.5*c2 + 1*c3;

%  for i=2:nx
% %
%      h(1,i)= (x(1,i)-1)/(-1);
% %
% %     h(1,1)=1;
%  end


function [Gu,Gs,Gn,f] = spm_csd_mtf_gu(P,M)
% Spectral desnities of innovations and noise for DCM for CSD
% FORMAT [Gu,Gs,Gn,f] = spm_csd_mtf_gu(P,M)
% FORMAT [Gu,Gs,Gn,f] = spm_csd_mtf_gu(P,f)
%
% P   - parameters
% M   - neural mass model structure (with M.Hz)
% f   - frequencies of interest (Hz)
%
% Gu  - neuronal innovations
% Gn  - channel noise (non-specific)
% Gs  - channel noise (specific)
%
% f   - frequency
%
% fluctuations and noise parameters: for n regions and c channels
%--------------------------------------------------------------------------
%    pE.a(2,n) - neuronal fluctuations        - amplitude and exponent
%    pE.b(2,c) - channel noise (non-specific) - amplitude and exponent
%    pE.c(2,c) - channel noise (specific)     - amplitude and exponent
%    pE.d(8,n) - neuronal fluctuations        - basis set coefficients
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd_mtf_gu.m 6856 2016-08-10 17:55:05Z karl $

% frequencies of interest
%--------------------------------------------------------------------------
try, f = M.Hz(:); catch, f = M(:); end

% Number of sources and fequencies
%--------------------------------------------------------------------------
if isfield(P,'d')
    ns = max(size(P.a,2),size(P.d,2));
else
    ns = size(P.a,2);
end
nf     = size(f,1);

% spectrum of neuronal innovations (Gu)
%==========================================================================
for i = 1:size(P.a,2)
    Gu(:,i) = exp(P.a(1,i))*f.^(-exp(P.a(2,i)));
end

if size(Gu,2) == 1, Gu = Gu*ones(1,ns); end

% add structured innovations - a discrete cosine set of order length(P.d)
%--------------------------------------------------------------------------
if isfield(P,'d')
    nd = size(P.d,1);
    X  = spm_dctmtx(nf,nd + 1);
    Mu = exp(X(:,2:end)*P.d);
else
    Mu = ones(nf,1);
end

if size(Mu,2) == 1, Mu = Mu*ones(1,ns); end

% return unless channel noise is required
%--------------------------------------------------------------------------
Gu     = Gu.*Mu;
if nargout < 2, return, end

% spectrum of channel noise (non-specific)
%==========================================================================
Gn  = exp(P.b(1) - 2)*f.^(-exp(P.b(2)));

% and spectrum of channel noise (specific: with the same exponent)
%--------------------------------------------------------------------------
for i = 1:size(P.c,2)
    Gs(:,i) = exp(P.c(1,i) - 2)*f.^(-exp(P.c(2,1)));
end


function [recall, rankloss, allRecalls, opts]= testFromFn(db, dbFeatFn, qFeatFn, opts, varargin)

    if nargin<4 || isempty(opts)
        % a bit hacky but fine..
        opts= struct(...
            'nTestRankSample', 0, ...
            'nTestSample', inf, ...
            'recallNs', [1:5, 10:5:100], ...
            'margin', 0.1, ...
            'nNegChoice', 1000, ...
            'cropToDim', 0 ...
            );
    end
    opts= vl_argparse(opts, varargin);

    relja_display('testFromFn:\n%s\n%s', dbFeatFn, qFeatFn);

    qFeat= fread( fopen(qFeatFn, 'rb'), inf, 'float32=>single');
    qFeat= reshape(qFeat, [], db.numQueries);
    nDims= size(qFeat, 1);
    dbFeat= fread( fopen(dbFeatFn, 'rb'), [nDims, db.numImages], 'float32=>single');
    assert(size(dbFeat,2)==db.numImages);

    if isfield(opts, 'cropToDim') && opts.cropToDim>0
        if opts.cropToDim > nDims
            warning('cropToDim (%d) larger than the dimensionality (%d) -- ignoring', opts.cropToDim, nDims);
        else
            qFeat= relja_l2normalize_col( qFeat(1:opts.cropToDim, :) );
            dbFeat= relja_l2normalize_col( dbFeat(1:opts.cropToDim, :) );
        end
    end

    if opts.nTestRankSample>0
        rankloss= testCoreRank(db, qFeat, dbFeat, opts.margin, opts.nNegChoice, 'nTestSample', opts.nTestRankSample);
    else
        rankloss= [];
    end
    [recall, allRecalls]= testCore(db, qFeat, dbFeat, 'nTestSample', opts.nTestSample, 'recallNs', opts.recallNs);
end


function mutationChildren = mutationNcK(parents,options,GenomeLength,FitnessFcn,state,thisScore,thisPopulation)
% Oren Rosen
% The MathWorks
% 8/29/2007
%
% This custom mutation function is written to work on a population of
% vectors of zeros and ones with the same amount of ones in each vector.
% The mutated child is formed by randomly permuting the elements of the
% parent.
% Note: Performance-wise this hasn't worked out to be that efficient. A
% better implementation may swap only two of the elements.

mutationChildren = zeros(length(parents),GenomeLength);
numVars = length(thisPopulation(1,:));

for i=1:length(parents)
    child = thisPopulation(parents(i),:);
    mutationChildren(i,:) = child( randperm(numVars) );
end