function [rq,rqSS,diagnostics] = realized_quantile_variance(price,time,timeType,samplingType,samplingInterval,quantiles,blockSize,symmetric,overlap,subsamples)
% Computes the Quantile Realized Variance of Christensen, Oomen and Podolskij (2008), the MinRV and
% MedRV estimator of Andersen, Dobrev and Shaumberg and the general symmetrized version suggested by
% Sheppard in a discussion of COP
%
% USAGE:
%   [RQ] = realized_quantile_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,QUANTILES,BLOCKSIZE)
%   [RQ,RQSS,DIAGNOSTICS] = realized_quantile_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,QUANTILES,BLOCKSIZE,SYMMETRIC,OVERLAP,SUBSAMPLES)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - m by 1 vector of times where TIME(i) corresponds to PRICE(i)
%   TIMETYPE         - String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight on the first day.
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - String describing the type of sampling to use when
%                        filtering PRICE
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVALticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL - Scalar integer or n by 1 vector whose meaning depends on the selected SAMPLINGTYPE
%   QUANTILES        - k by 1 vector of quantile values to use when computing RQ.
%                        When SYMMETRIC = false, 0.5<QUANTILES<=1.
%                        When SYMMETRIC = true, 0<QUANTILES<=1
%                        In either case, QUANTILES * BLOCKSIZE must be an integer.
%                        The simplest method to set the quantiles is to determine which of the
%                        ordered returns should be used in the QRV, and then to set
%                        QUANTILES = ORDER/BLOCKSIZE.  For example, when SYMETRIC = FALSE, and
%                        BLOCKSIZE = 20, QUANTILES = [13 15 18]/20 will use the returns in position
%                        13, 15 and 18 (as well as 8 6 and 3).  When SYMMETRIC = TRUE, QUANTILES =
%                        [5 8 10 13 15 18]/20 will use the absolute value of returns in positions 5,
%                        8, 10, 13, 15 and 18.  For a fixed block size, you should generally more
%                        quantiles when SYMMETRIC = TRUE.
%   BLOCKSIZE        - Number of returns to use in each block when computing the quantiles. NOTE: If
%                        OVERLAP = FALSE, then the number of returns produced by filtering according
%                        to SAMPLINGTYPE and SAMPLINGINTERVAL must be an integer multiple of
%                        BLOCKSIZE. Otherwise there is no constraint.
%   SYMMETRIC        - [OPTIONAL] Boolean indicating whether the base the estimator on the absolute value
%                        of returns (symmetric) of on the non-adjusted returns. Default value is
%                        FALSE (as used by COP).
%   OVERLAP          - [OPTIONAL] Boolean indicating whether to use all overlapping blocks (TRUE),
%                        or only non-overlapping blocks. Default value is TRUE.
%   SUBSAMPLES       - [OPTIONAL] Scalar integer indicating the number of subsample realized
%                        variance estimators to average with the original realized variance.
%                        Subsample realized variances are based on prices uniformly spaced between
%                        the times (Calendar sampling) or ticks (Business sampling).  SUBSAMPLES=1
%                        will compute a subsample realized variance using the mid-point of the
%                        price sample points, 2 will use 1/3 and 2/3, and so on. In general this
%                        number should be small so the subsample estimators will be "sparse".
%
% OUTPUTS:
%   RQ               - Quantile realized variance vector (k by 1) corresponding to the values in QUANTILES
%   RQSS             - Subsample based version of RQ.  RQSS = RQ if SUBSAMPLES = 0 or omitted.
%   DIAGNOSTICS      - Structure with fields:
%                        RQINDIV   - k by 1 vector of RQ estimates
%                        RQSSINDIV - k by 1 vector of subsample RQ estimates
%                        RQWEIGHTS - Weights used in combining the individual RQ estimates.  RQ
%                                      weights are computed from RQCOV as the minimum variance
%                                      combination
%                        RQCOV     - k by k non-scaled (integrated quarticity) covariance matrix.
%
% COMMENTS:
%   The cases where SAMPLESPERBIN is in {4 5 6 10 13 15 18 20 25 26 30 36 39 50 60 65 72 75 100 144}
%   (non-symmetric) or {2 3 4 5 6 8 9 10 12 13 15 18 20 24 25 26 30 36 39 40 50 60 65 72 75 100 144}
%   (symmetric) the scales and non-scaled covariances have been pre-computed using Monte Carlo
%   integration using 100,000,000 simulations. Choosing another number for SAMPLESPERBIN requires a
%   run of realized_quantile_variance_scale which can be slow if SAMPLESPERBIN is large. If using
%   another value, 1,000,000 simulations will be used in computing the weights. If repeatedly using
%   another value, pre-computing the appropriate scales and non-scaled covariance using
%   realzed_quantile_weight_simulation is recommended.
%
% EXAMPLES:
%   % Estimate the RQ from prices available from 9:30 to 16:00 using 1 minute returns, 30 samples per
%   % bin and quantiles [18 22 27]/30 ([0.6 0.733 0.9])
%   sampleTimes = seconds2wall(wall2seconds(93000):60:wall2seconds(160000))
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,[18 22 27]/30,30);
%
%   % The same but using 4 subsamples
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,[18 22 27]/30,30,[],[],4);
%
%   % Estimate the RQ from prices available from 9:30 to 16:00 using 1 minute returns, 30 samples per
%   % bin, the symmetric verion and quantiles [5 10 15 18 22 27]/30 ([0.15 0.33 0.5 0.6 0.733 0.9])
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,[5 10 15 18 22 27]/30,30,true);
%
%   % Estimate the MedRV of ABS
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,2/3,3,true);
%
%   % Estimate the MinRV of ABS
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,1/2,2,true);
%
%   % Estimate a MedRV-like RQV only using blocks of size 5
%   RQ=realized_quantile_variance(price,time,'wall','Fixed',sampleTimes,3/5,5,true);
%
%  See also REALIZED_MIN_MED_VARIANCE, REALIZED_KERNEL, REALIZED_VARIANCE,
%  REALIZED_BIPOWER_VARIATION, REALIZED_THRESHOLD_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% InputChecking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<7 || nargin>10
    error('Seven to ten inputs required.')
end
switch nargin
    case 7
        symmetric = false;
        overlap = true;
        subsamples = 1;
    case 8
        overlap = true;
        subsamples = 1;
    case 9
        subsamples = 1;
end

if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    error('PRICE must be a m by 1 vector.')
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    error('TIME must be sorted and increasing')
end
if size(time,2)>1 || length(time)~=length(price)
    error('TIME must be a m by 1 vector.')
end
% Inserted to protect against inputing integer times
time = double(time);

timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.');
end
samplingType=lower(samplingType);
if ~ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
    error('SAMPLINGTYPE must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.');
end

m=size(price,1);
t0=time(1);
tT=time(m);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer
    if ~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1
        error('SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected.')
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0) && any(samplingInterval<=tT))
        error('At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.')
    end
    if any(diff(samplingInterval)<=0)
        error('When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.')
    end
end

% SAMPLERPERBIN, positive integer >=2
if blockSize<1 || floor(blockSize)~=blockSize
    error('SAMPLESPERBIN must be a positive integer greater than or equal to 2 (and usually at least 20)')
end

% SYMMETRIC
if isempty(symmetric)
    symmetric = false;
end
if ~isscalar(symmetric)
    error('SYMMETRIC must be a scalar logical value.')
end
symmetric = logical(symmetric);

% QUANTILES
if symmetric
    if any(quantiles<=0) || any(quantiles>1) || length(quantiles)>blockSize || length(unique(quantiles*blockSize)) ~= length(quantiles)
        error('QUANTILES must be unique, greater than 0 and less than or equal to 1, and the number of quantiles must be smaller than SAMPLESPERBIN when SYMMETRIC = true.')
    end
else
    if any(quantiles<=0.5) || any(quantiles>1) || length(quantiles)>(blockSize/2) || length(unique(quantiles*blockSize)) ~= length(quantiles)
        error('QUANTILES must be unique, greater than 0.5 and less than or equal to 1, and the number of quantiles must be smaller than 0.5*SAMPLESPERBIN when SYMMETRIC = false.')
    end
end

% OVERLAP
if isempty(overlap)
    overlap = true;
end
if ~isscalar(overlap)
    error('OVERLAP must be a scalar logical value.')
end
overlap = logical(overlap);

% SUBSAMPLES
if isempty(subsamples)
    subsamples = 1;
end
if ~isscalar(subsamples) || subsamples<0 || floor(subsamples)~=subsamples
    error('SUBSAMPLES must be a non-negative integer.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 1. Compute returns
logPrice = log(price);
filteredLogPrice = realized_price_filter(logPrice,time,timeType,samplingType,samplingInterval);
returns = diff(filteredLogPrice);
% Check that the number of returns is compatible with SAMPLESPERBIN
n=length(returns);
if ~overlap
    if floor(n/blockSize)~=(n/blockSize)
        error(['The number of returns computed from the prices returned from realized_price_filter(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL) must be an integer multiple of SAMPLESPERBIN.  The number of return produced is ' num2str(n)]);
    end
end

if overlap
    binStart = 1:n-blockSize+1;
else
    binStart = 1:blockSize:n;
end
returns = returns * sqrt(n);
if symmetric
    returns = abs(returns);
end
q = length(quantiles);
rqindiv = zeros(length(binStart),q);

% Compute the indices to use
if symmetric
    indices = round(quantiles*blockSize);
else
    upperIndices = round(quantiles*blockSize);
    lowerIndices = round((1-quantiles)*blockSize+1);
end
% Loop over the bins

count = 1;
for j=binStart
    binreturns = returns(j:j+blockSize-1);
    binreturns = sort(binreturns)';
    if symmetric
        rqindiv(count,:)=binreturns(indices).^2;
    else
        rqindiv(count,:)=binreturns(lowerIndices).^2+binreturns(upperIndices).^2;
    end
    count = count + 1;
end
rqindiv = mean(rqindiv);

[rq,rqindiv,rqcov,rqweights] = realized_quantile_variance_core(rqindiv,blockSize,quantiles,symmetric);

diagnostics.rqindiv = rqindiv;
diagnostics.rqcov = rqcov;
diagnostics.rqweights = rqweights;

subsampledLogPrice = realized_subsample(logPrice,time,timeType,samplingType,samplingInterval,subsamples);
rqindiv = nan(subsamples * length(binStart),q);
count = 1;
for i=1:subsamples
    filteredLogPrice = subsampledLogPrice{i};
    returns = diff(filteredLogPrice);
    n = length(returns);
    returns = returns * sqrt(n);
    if symmetric
        returns = abs(returns);
    end
    binStart = 1:n-blockSize+1;
    for j=binStart
        binreturns = returns(j:j+blockSize-1);
        binreturns = sort(binreturns)';
         if symmetric
            rqindiv(count,:)=binreturns(indices).^2;
         else
            rqindiv(count,:)=binreturns(lowerIndices).^2+binreturns(upperIndices).^2;
        end
        count = count + 1;
    end
end
rqindiv = rqindiv(1:count-1,:);
rqindiv = mean(rqindiv);

[rqSS,rqindivSS] = realized_quantile_variance_core(rqindiv,blockSize,quantiles,symmetric);
diagnostics.rqindivSS = rqindivSS;

end

function [rq,rqindiv,rqcov,rqweights] = realized_quantile_variance_core(rqindiv,blockSize,quantiles,symmetric)

scales = load('realized_quantile_scales');
if symmetric
    simulationBlockSize = scales.symmetricSimulationSamplerperbin;
    simulationCovariance = scales.symmetricExpectedCovariance;
    simulationExpectedQuantiles = scales.symmetricExpectedQuantiles;
    simulationQuantile = scales.symmetricSimulationQuantile;
else
    simulationBlockSize = scales.asymmetricSimulationSamplerperbin;
    simulationCovariance = scales.asymmetricExpectedCovariance;
    simulationExpectedQuantiles = scales.asymmetricExpectedQuantiles;
    simulationQuantile = scales.asymmetricSimulationQuantile;

end
simulationBlockSize = cell2mat(simulationBlockSize);
if ismember(blockSize,simulationBlockSize )
    % If available load and use the pre-computed value

    [~,pl]=ismember(blockSize,simulationBlockSize);
    thisScale = simulationExpectedQuantiles{pl};
    thisQuantile = simulationQuantile{pl};
    thisCovariance = simulationCovariance{pl};
    q = length(quantiles);
    indicator = zeros(1,q);
    for i = 1:q
        [~,indicator(i)] = min(abs(thisQuantile-quantiles(i)));
    end
    rqExpectedSquaredQuantileValue = thisScale(indicator);
    % Find index
    rqcov     = thisCovariance(indicator,indicator);
    rqCovInv = rqcov \ eye(q);
    rqweights = rqCovInv*ones(q,1)/(ones(1,q)*rqCovInv*ones(q,1));
else
    % Need to simulate using 1,000,000 simulations
    if blockSize>100
        warning('oxfordRealized:realizedQuantileVariance','Computing the scales needed.  Since SAMPLESPERBIN is very large this may take a long time.  \nConsider pre-computing this value, especially if using this value of SAMPLESPERBIN many times.')
    else
        warning('oxfordRealized:realizedQuantileVariance','Computing the scales needed.  \nConsider pre-computing this value, especially if using this value of SAMPLESPERBIN many times.')
    end
    [rqExpectedSquaredQuantileValue,rqweights,rqcov]=realized_quantile_variance_scale(adjSamplesPerBin,quantiles,1000000);
end

rqindiv  = rqindiv ./ rqExpectedSquaredQuantileValue;
rq = rqweights'*rqindiv';
end


function pred_value = xval_simple_ols_featureselect(X, Y, pthreshold, holdout_method)
% Check: a very simple leave-one-out cross-validated regression
% pred_value = xval_simple_ols_loo(X, Y, p-value selection for univariate feature selection, holdout_method)
%
% pred_value: cross-validated predictions of outcome data
% X: n x variables matrix of predictors
% Y: n x 1 vector of outcomes
%
% Tor Wager, June 2010
%
% Go to any LASSO output directory and run this:
%
% maskInfo = iimg_read_img(fullfile(pwd, 'mask.img'), 2);
% dat{1} =  iimg_get_data(maskInfo, anticimages);
% pred_value = xval_simple_ols_loo(X, Y)

[N, k] = size(X);
pred_value = zeros(N, 1);

holdout_set = nested_select_holdout_set;

create_figure('test', 1, 2);

fprintf('Fold: %03d', 0);
for wh_fold = 1:length(holdout_set)

    fprintf('\b\b\b%3.0f ', wh_fold);

    % select training data
    Xi = X;
    Yi = Y;

    Yi(holdout_set{wh_fold}) = [];
    Xi(holdout_set{wh_fold}, :) = [];              % leave out the missing observation(s)

    Xtest = X(holdout_set{wh_fold}, :);
    Ytest = Y(holdout_set{wh_fold}, :);          % only used later when we test prediction

    ntrain = size(Xi, 1);
    nholdout = size(Xtest, 1);

    % select features based on univariate correlations
    [r, p, Tstat] = correlation_fast_series(Xi, Yi);
    wh_features = p <= pthreshold;
    nfeatures(wh_fold) = sum(wh_features);
    if nfeatures(wh_fold) == 0
        disp('Warning: no features pass threshold');
        wh_features = p <= prctile(p, 10);
    end

    Xi = Xi(:, wh_features);

    Xi = [ones(ntrain, 1) Xi]; % add intercept

    % Make prediction
    b = pinv(Xi) * Yi;
    pred_value(holdout_set{wh_fold}, 1) = [ones(nholdout, 1) Xtest(:, wh_features)] * b;

    create_figure('test', 1, 2, 1); subplot(1, 2, 1); plot(Xi*b, Yi, 'ko');
    plot(pred_value(holdout_set{wh_fold}, 1), Ytest, 'ro', 'MarkerFaceColor', 'r');
    subplot(1, 2, 2);
   title('Black circles = training set; Red = holdout obs');
    drawnow;

end

cm = colormap(jet(N));
figure; hold on;
for i = 1:N
    plot(pred_value(i), Y(i), 'ko', 'MarkerFaceColor', cm(i, :));
end
xlabel('Predicted outcome (xval)'); ylabel('Outcome');
title('Color = order in data series');

function holdout_set = nested_select_holdout_set
        % purpose: return holdout_set variable

        nobs_s = length(Y);

        switch lower(holdout_method)
            case 'loo'
                holdout_set = cell(1, nobs_s);
                for i = 1:nobs_s, holdout_set{i} = i; end

            case 'l4o_covbalanced'
                disp('Selecting holdout sets: Balancing on covariates and outcome, and also trying to ensure that each obs is selected equally often.');
                holdout_proportion = 4 ./ nobs_s; % prop for leave-4-out
                nfolds = 40;
                if isempty(cov_val)
                    wh_holdout = xval_select_holdout_set(Y, [], nfolds, holdout_proportion, verbose);
                else
                    wh_holdout = xval_select_holdout_set(Y, cov_val, nfolds, holdout_proportion, verbose);
                end
                holdout_set = cell(1, nfolds);
                for k = 1:nfolds
                    holdout_set{k} = wh_holdout(:, k);
                end

            case 'categorical_covs'

                holdout_set = xval_select_holdout_set_categoricalcovs(cov_val);

            case 'balanced4'
                nfolds = 4;
                holdout_set = cell(1, nfolds);
                [ys, indx] = sort(Y);
                for k = 1:nfolds

                    holdout_set{k} = indx(k:nfolds:end);
                    if isempty(holdout_set{k}), error('Holdout set construction error: Dataset too small?'); end

                end

            otherwise error('Unknown holdout method.  See help.');
        end
end

end % main function


% RES = corrDn(IM, FILT, EDGES, STEP, START, STOP)
%
% Compute correlation of matrices IM with FILT, followed by
% downsampling.  These arguments should be 1D or 2D matrices, and IM
% must be larger (in both dimensions) than FILT.  The origin of filt
% is assumed to be floor(size(filt)/2)+1.
%
% EDGES is a string determining boundary handling:
%    'circular' - Circular convolution
%    'reflect1' - Reflect about the edge pixels
%    'reflect2' - Reflect, doubling the edge pixels
%    'repeat'   - Repeat the edge pixels
%    'zero'     - Assume values of zero outside image boundary
%    'extend'   - Reflect and invert
%    'dont-compute' - Zero output when filter overhangs input boundaries
%
% Downsampling factors are determined by STEP (optional, default=[1 1]),
% which should be a 2-vector [y,x].
%
% The window over which the convolution occurs is specfied by START
% (optional, default=[1,1], and STOP (optional, default=size(IM)).
%
% NOTE: this operation corresponds to multiplication of a signal
% vector by a matrix whose rows contain copies of the FILT shifted by
% multiples of STEP.  See upConv.m for the operation corresponding to
% the transpose of this matrix.

% Eero Simoncelli, 6/96, revised 2/97.

function res = corrDn(im, filt, edges, step, start, stop)

%% NOTE: THIS CODE IS NOT ACTUALLY USED! (MEX FILE IS CALLED INSTEAD)

fprintf(1,'Warning: You should compile the MEX code for "corrDn", found in the MEX subdirectory.  It is MUCH faster.\n');

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('edges') == 1)
  if (strcmp(edges,'reflect1') ~= 1)
    warning('Using REFLECT1 edge-handling (use MEX code for other options).');
  end
end

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

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

if (exist('stop') ~= 1)
	stop = size(im);
end

%------------------------------------------------------------

% Reverse order of taps in filt, to do correlation instead of convolution
filt = filt(size(filt,1):-1:1,size(filt,2):-1:1);

tmp = rconv2(im,filt);
res = tmp(start(1):step(1):stop(1),start(2):step(2):stop(2));


function [b0vector b0angle] = getB0direction(vw)
% Return the direction of the b0 field from a scan as (1) a unit vector and
% (2) an angle in degrees
%
%   [b0vector b0angle]  = getB0direction([vw])
%
% Examples:
%   b0vector = getB0direction(vw)
%   [b0vector b0angle] = getB0direction
%
% vw can be INPLANE, GRAY, or VOLUME

mrGlobals

if ~exist('vw', 'var'), vw = getCurView; end

% get the scanner xform (image coords => mm coords)
scannerXform = viewGet(vw, 'scannerXform');

% Define a unit vector in the z-direction in scanner space. This is the B0
% direction (by definition).
origin = [0 0 0]; z = [0 0 1];

% Transform the vector into INPLANE image space
b0ip = scannerXform \ [origin 1; z 1]';
b0ip = b0ip(1:3,2) - b0ip(1:3,1);

% Make it a unit vector
b0ip = 1/norm(b0ip) * b0ip;

% xform the b0 vector to the appropriate view type
switch lower(viewGet(vw, 'viewType'))
    case 'inplane'
        % done
        b0vector = b0ip;
    case {'gray' 'volume'}
        xform = mrSESSION.alignment;
        b0Vol = xform * [[0 0 0 1]' [b0ip; 1]];
        % make it a vector of norm 1
        b0vector = b0Vol(1:3,2) - b0Vol(1:3,1);
        b0vector = 1/norm(b0vector) * b0vector;
        % xform to xyz
        b0vector = [-b0vector(3) b0vector(2) -b0vector(1)];
    otherwise
        error('Can''t get B0 direction in %s vw', viewGet(vw, 'viewType'))
end

% convert b0 vector into angle. This is the angle between z-vector in image
% space and and z-vector in scanner space.
costheta    = dot(b0vector, z)/norm(b0vector)*norm(z);
theta       = acos(costheta);
b0angle     = theta  / pi * 180;


%--------------------
function [EbN, Es] = PQ_excitCB (X2)

persistent W2 EIN

NF = 2048;
Version = 'Basic';
if (isempty (W2))
    Fs = 48000;
    f = linspace (0, Fs/2, NF/2+1);
    W2 = PQWOME (f);
    [Nc, fc] = PQCB (Version);
    EIN = PQIntNoise (fc);
end

% Allocate storage
XwN2 = zeros (1, NF/2+1);

% Outer and middle ear filtering
Xw2(1,:) = W2 .* X2(1,1:NF/2+1);
Xw2(2,:) = W2 .* X2(2,1:NF/2+1);

% Form the difference magnitude signal
for (k = 0:NF/2)
    XwN2(k+1) = (Xw2(1,k+1) - 2 * sqrt (Xw2(1,k+1) * Xw2(2,k+1)) ...
               + Xw2(2,k+1));
end

% Group into partial critical bands
Eb(1,:) = PQgroupCB (Xw2(1,:), Version);
Eb(2,:) = PQgroupCB (Xw2(2,:), Version);
EbN     = PQgroupCB (XwN2, Version);

% Add the internal noise term => "Pitch patterns"
E(1,:) = Eb(1,:) + EIN;
E(2,:) = Eb(2,:) + EIN;

% Critical band spreading => "Unsmeared excitation patterns"
Es(1,:) = PQspreadCB (E(1,:), Version);
Es(2,:) = PQspreadCB (E(2,:), Version);


%% Example: Random Category Registration problem and its semidefinite relaxations
%% Heng Yang, July 06, 2021

clc; clear; close all; restoredefaultpath

%% paths to dependencies
spotpath    = '../spotless';
mosekpath   = '../../mosek';
stridepath  = '../STRIDE';
manoptpath  = '../manopt';
sdpnalpath  = '../../SDPNAL+v1.0';
path.stridepath = stridepath;
path.mosekpath  = mosekpath;
path.manoptpath = manoptpath;
addpath('../utils')
addpath('./solvers')

%% choose if run GNC for STRIDE
rungnc      = true;

%% generate random point cloud registration problem
datatype                 = 'car';
problem.outlierRatio     = 0.1;
problem.noiseSigma       = 0.01;
problem.translationBound = 10.0;
switch datatype
    case 'random'
        %%%%%%%%%%%%%% RANDOM data %%%%%%%%%%%%%%
        problem.N                = 10;
        problem.K                = 3;
        problem                  = gen_category_registration(problem);
    case 'car'
        %%%%%%%%%%%%%% PASCAL Car %%%%%%%%%%%%%%
        problem.path             = './data/car.mat';
        problem                  = gen_catreg_pascal_car(problem);
end
lambda                   = 0.5;

%% generate SDP relaxations
addpath(genpath(spotpath))
SDP     = relax_category_registration_v2(problem,'lambda',lambda,'checkMonomials',false);
rmpath(genpath(spotpath))

%% Solve using STRIDE
% Primal initialization
if rungnc
    solution        = gnc_category_registration(problem,SDP,path,'lambda',lambda);
    [R_est,t_est]   = invert_transformation(solution.R_est,solution.t_est);

    v0       = lift_catreg_v2(R_est(:),...
                              t_est(:),...
                              solution.c_est(:),...
                              solution.theta_est(:),...
                              problem.cBound,...
                              problem.translationBound);
    X0          = rank_one_lift(v0);
    gnc.R_err   = getAngularError(problem.R_gt,solution.R_est);
    gnc.t_err   = getTranslationError(problem.t_gt,solution.t_est);
    gnc.c_err   = getTranslationError(problem.c_gt,solution.c_est);
    gnc.time    = solution.time;
    gnc.info    = solution;

else
    X0       = [];
end

% dual initialization
addpath(genpath(spotpath))
chordalSDP     = chordal_relax_category_registration_v2(problem,'lambda',lambda);
rmpath(genpath(spotpath))
prob = convert_sedumi2mosek(chordalSDP.sedumi.At,...
                            chordalSDP.sedumi.b,...
                            chordalSDP.sedumi.c,...
                            chordalSDP.sedumi.K);
addpath(genpath(mosekpath))
time0   = tic;
param.MSK_IPAR_INTPNT_MAX_ITERATIONS = 20; % set maximum iterations 20
[~,res] = mosekopt('minimize info',prob,param);
time_dualInit = toc(time0);
[Xchordal,ychordal,Schordal,~] = recover_mosek_sol_blk(res,chordalSDP.blk);
S_assm             = catreg_dual_from_chordal_dual_v2(Schordal,problem.N,problem.K);

% STRIDE main algorithm
addpath(genpath(stridepath))
addpath(genpath(manoptpath))

pgdopts.pgdStepSize     = 10;
pgdopts.SDPNALpath      = sdpnalpath;
pgdopts.maxiterPGD      = 5;
% ADMM parameters
pgdopts.tolADMM         = 1e-10;
pgdopts.maxiterADMM     = 1e4;
pgdopts.stopoptionADMM  = 0;

pgdopts.rrOpt           = 1:3;
pgdopts.rrFunName       = 'local_search_catreg_v2';
rrPar.blk = SDP.blk; rrPar.translationBound = problem.translationBound;
rrPar.N = problem.N; rrPar.K = problem.K; rrPar.cBound = problem.cBound;
pgdopts.rrPar           = rrPar;
pgdopts.maxiterLBFGS    = 1000;
pgdopts.maxiterSGS      = 1000;
pgdopts.lbfgseps        = false;
pgdopts.S0              = S_assm;

[outPGD,Xopt,yopt,Sopt]     = PGDSDP(SDP.blk, SDP.At, SDP.b, SDP.C, X0, pgdopts);
rmpath(genpath(manoptpath))

infostride                  = get_performance_catreg_v2(Xopt,yopt,Sopt,SDP,problem,stridepath);
infostride.totaltime        = outPGD.totaltime + time_dualInit;
infostride.time             = [outPGD.totaltime,time_dualInit];
if rungnc
    infostride.gnc  = gnc;
    infostride.totaltime = infostride.totaltime + gnc.time;
    infostride.time      = [gnc.time,infostride.time];
end

fprintf('\n\n\n\n\n')


classdef LossSmoothL1 < dagnn.Loss
%LossSmoothL1  Smooth L1 loss
%  `LossSmoothL1.forward({x, x0, w})` computes the smooth L1 distance
%  between `x` and `x0`, weighting the elements by `w`.
%
%  Here the smooth L1 loss between two vectors is defined as:
%
%     Loss = sum_i f(x_i - x0_i) w_i.
%
%  where f is the function (following the Faster R-CNN definition):
%
%              { 0.5 * sigma^2 * delta^2,         if |delta| < 1 / sigma^2,
%   f(delta) = {
%              { |delta| - 0.5 / sigma^2,         otherwise.
%
%  In practice, `x` and `x0` can pack multiple instances as 1 x 1 x C
%  x N arrays (or simply C x N arrays).

  properties
    sigma = 1.
  end

  methods
    function outputs = forward(obj, inputs, params)
      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      absDelta(linearRegion) = absDelta(linearRegion) - 0.5/sigma2 ;
      absDelta(~linearRegion) = 0.5 * sigma2 * absDelta(~linearRegion).^2 ;

      % Mutliply by instance weights and sum.
      outputs{1} = inputs{3}(:)' * absDelta(:) ;

      % Accumulate loss statistics.
      if obj.ignoreAverage, return; end;
      n = obj.numAveraged ;
      m = n + gather(sum(inputs{3}(:))) + 1e-9 ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
    % Function derivative:
    %
    %          { sigma^2 * x,             if |x| < 1 / sigma^2,
    %  f'(x) = {
    %          { sign(x),                 otherwise.

      sigma2 = obj.sigma^2 ;
      delta = inputs{1} - inputs{2} ;
      absDelta = abs(delta) ;

      linearRegion = (absDelta > 1. / sigma2) ;
      delta(linearRegion) = sign(delta(linearRegion));
      delta(~linearRegion) = sigma2 * delta(~linearRegion) ;

      derInputs = {inputs{3} .* delta .* derOutputs{1}, [], []} ;
      derParams = {} ;
    end

    function obj = LossSmoothL1(varargin)
      obj.load(varargin) ;
      obj.loss = 'smoothl1';
    end
  end
end


function [ccg,ic] = chXcorr(hc_L,hc_R,fs,varargin)
%CHXCORR Calculate cross-correlograms with a wide range of options.
%
%   CCG = IOSR.AUDITORY.CHXCORR(HC_L,HC_R,FS) cross-correlates the input
%   2-D matrices HC_L and HC_R over 10ms frame with a maximum lag of 1ms.
%   It is assumed that the number of frequency channels is min(size(HC_L))
%   and hence HC_L and HC_R can be in either orientation. The
%   cross-correlograms consist of cross-correlations for every frame and
%   frequency channel. CCG has dimensions [lag,frequency,frame]. The
%   function calculates running cross-correlations for every sample and
%   integrates these cross-correlations over each frame. The number of
%   frames frame_count is calculated thus:
%
%       frame_count = ...
%           floor((max(size(hc_L))-maxlag-1)/frame_length);
%
%   The underlying cross-correlation algorithm is based on that proposed by
%   Faller & Merimaa [1]. In this implmentation, the time constant of the
%   backward infinite exponential window is given by tau (in samples).
%
%   CCG = IOSR.AUDITORY.CHXCORR(HC_L,HC_R,FS,'PARAMETER',VALUE) allows a
%   number of options to be specified. The options are:
%
%   ({} indicates the default value)
%
%   'frame_length'   : {round(0.01*fs)} | scalar
%       The length of frames used to calculate for integrating
%       cross-correlations.
%   'noverlap'       : {1} | scalar
%       The number of frames over which to integrate the
%       cross-correlations. Note that the frame count is reduced
%       accordingly.
%   'maxlag'         : {round(0.001*fs)} | scalar
%       The maximum lag of the cross-correlation.
%   'tau'            : {round(0.01*fs)} | scalar
%       The time constant of the exponential window used to calculate
%       running cross-correlations. It is recommended that if norm_flag = 1
%       then tau >> 1. As can be seen below, as tau -> 1 then
%       [aL(m,i,n?1), aR(m,i,n?1)] -> 0, and hence c(m,i,n) -> 1.
%   'inhib'          : {[]} | array
%       Specificies an array with which to multiply the cross-correlations
%       before they are integrated. The value defaults to an empty array,
%       meaning that no inhibition will be applied.
%   'ic_t'           : {0} | scalar
%       Specifies the interaural coherence (IC) threshold. Only samples for
%       which the IC exceeds this threshold will be used to integrate
%       cross-correlations. The algorithm calculates Interaural Coherence
%       (IC) according to [1]. The value should be in the range [0,1].
%   'norm_flag'      : {0} | scalar
%       Specifies whether the cross-correlograms are calculated using
%       normalised cross-correlations. A non-zero value indicates that
%       normalised cross-correlations are used.
%   'inhib_mode'     : {'subtract'} | 'multiply'
%       Specify how the inhibition is applied. The default 'subtract' will
%       subtract inhib from the running cross-correlations (negative values
%       are set to zero); 'multiply' will multiply inhib with the running
%       cross-correlations.
%
%   [CCG,IC] = IOSR.AUDITORY.CHXCORR(...) returns the calculated IC to the
%   matrix IC. Although the matrix returned is the same size as hc_L, IC is
%   only calculated for samples 1:frame_count*frame_length, other values
%   will be set to 0.
%
%   Algorithm
%
%   The running normalised cross-correlation is calculated as [1]:
%
%   C(m,i,n) = c(m,i,n) / sqrt( aL(m,i,n) * aR(m,i,n)
%
%   where
%
%   c(m,i,n) = (1/tau) * HC_L(i, max(n+m,n)) * HC_R(i, max(n-m,n)) + ...
%       (1-1/tau) * c(m,i,n-1),
%
%   aL(m,i,n) = (1/tau) * (HC_L(i, max(n+m,n)))^2 + ...
%       (1-1/tau) * aL(m,i,n-1),
%
%   aR(m,i,n) = (1/tau) * (HC_R(i, max(n+m,n)))^2 + ...
%       (1-1/tau) * aR(m,i,n-1),
%
%   i is the frequency index, m is the lag index, and n is the sample
%   index. The running (non-normalised) cross-correlation is calculated is
%   simply c(m,i,n).
%
%   The interaural coherence is
%
%   IC(i,n) = max(C(m,i,n)) % (i.e. over m)
%
%   The cross-correlogram is calculated as the sum of cross- correlations
%   in a given frame:
%
%   CCG(m,i,j) = sum(C(m,i,J),3)
%
%   where
%
%   J = (j-1) * frame_length + 1 : j * frame_length
%
%   References
%
%   [1] C. Faller and J. Merimaa, "Source localization in complex listening
%       situations: Selection of binaural cues based on interaural
%       coherence", The Journal of the Acoustical Society of America, vol.
%       116, pp.3075-3089, Nov. 2004.
%
%   Further Reading
%
%   C. Hummersone, R. Mason, and T. Brookes, "A comparison of computational
%       precedence models for source separation in reverberant
%       environments", The Journal of the Audio Engineering Society, vol.
%       61(7/8), pp.508-520, July 2013.

%   Copyright 2016 University of Surrey.

    assert(nargin>=3, 'iosr:chXcorr:nargin', 'Number of input arguments must be greater than or equal to three.')

    if isparameter(varargin,'inhib_mode') && ~isparameter(varargin,'inhib')
        warning('iosr:instIld:inhibMode','''inhib_mode'' specified, but no inhibition array ''inhib''.');
    end

    % Check source file is compiled
    iosr.general.checkMexCompiled('-largeArrayDims',fullfile(fileparts(mfilename('fullpath')),'chXcorr_c.c'))

    options = struct(...
        'frame_length',round(0.01*fs),...
        'noverlap',1,...
        'maxlag',round(0.001*fs),...
        'tau',round(0.01*fs),...
        'inhib',[],...
        'ic_t',0,...
        'norm_flag',0,...
        'inhib_mode','subtract');

    % read parameter/value inputs
    if nargin > 3 % if parameters are specified
        % read the acceptable names
        optionNames = fieldnames(options);
        % count arguments
        nArgs = length(varargin);
        if round(nArgs/2)~=nArgs/2
           error('iosr:chXcorr:nameValuePair','CHXCORR needs propertyName/propertyValue pairs')
        end
        % overwrite defults
        for pair = reshape(varargin,2,[]) % pair is {propName;propValue}
           IX = strcmpi(pair{1},optionNames); % find match parameter names
           if any(IX)
              % do the overwrite
              options.(optionNames{IX}) = pair{2};
           else
              error('iosr:chXcorr:unknownOption','%s is not a recognized parameter name',pair{1})
           end
        end
    end

    % assign options to variables
    frame_length = options.frame_length;
    noverlap = options.noverlap;
    maxlag = options.maxlag;
    tau = options.tau;
    inhib_mode = options.inhib_mode;
    norm_flag = options.norm_flag;
    ic_t = options.ic_t;
    inhib = options.inhib;

    % check inputs
    assert(all(size(hc_L)==size(hc_R)), 'iosr:chXcorr:invalidInput', '''hc_L'' and ''hc_R'' must be the same size')
    assert(round(frame_length)==frame_length && isscalar(frame_length) && frame_length>0, 'iosr:chXcorr:invalidFrame', ...
        '''frame_length'' must be an integer greater than zero')
    assert(round(noverlap)==noverlap && isscalar(noverlap) && noverlap>0, 'iosr:chXcorr:invalidNoverlap', ...
        '''noverlap'' must be an integer greater than zero')
    assert(round(maxlag)==maxlag && isscalar(maxlag) && maxlag>0, 'iosr:chXcorr:invalidMaxlag', ...
        '''maxlag'' must be an integer greater than zero')
    assert(isscalar(tau) && tau>=1, 'iosr:chXcorr:invalidTau', '''tau'' must be a scalar greater than or equal to one')
    assert(isscalar(norm_flag), 'iosr:chXcorr:invalidNorm', '''norm_flag'' must be a scalar')
    assert(isscalar(ic_t) && ic_t>=0 && ic_t<=1, 'iosr:chXcorr:invalidIct', '''ic_t'' must be a scalar in the range [0,1]')
    assert(ischar(inhib_mode), 'iosr:chXcorr:invalidInhibMode', '''inhib_mode'' must be a char array (string)')

    % Calculate frame count
    frame_count = floor(max(size(hc_L))/(frame_length));
    frame_count = frame_count-noverlap+1;

    % Calculate number of frequency channels
    numchans = min(size(hc_L));
    numsamples = max(size(hc_L));

    % Check orientation of HC and inhib data (i.e. that frequency runs across the rows)
    dims = size(hc_L);
    hc_L = check_input(hc_L,2,numchans);
    hc_R = check_input(hc_R,2,numchans);

    % set a flag if data has been transposed in this way
    if dims(1)~=size(hc_L,1)
        rot = true;
    else
        rot = false;
    end

    % set inhibition mode ID
    switch inhib_mode
        case 'multiply'
            inhib_mode_ID = 1;
            if isempty(inhib)
                inhib = ones(size(hc_L));
            end
        case 'subtract'
            inhib_mode_ID = 2;
            if isempty(inhib)
                inhib = zeros(size(hc_L));
            end
        otherwise
            error('iosr:chXcorr:unknownInhibMode','''inhib_mode'' must be set to ''multiply'' or ''subtract''')
    end

    inhib = check_input(inhib,2,numchans);

    % Append HC and inhibition data with zeros for cross-correlation
    hc_L = [hc_L; zeros(maxlag+1,numchans)];
    hc_R = [hc_R; zeros(maxlag+1,numchans)];
    inhib = [inhib; zeros(maxlag+1,numchans)];

    assert(all(size(inhib)==size(hc_L)), 'iosr:chXcorr:invalidInhib', '''inhib'' must be a matrix the same size as ''hc_L'' or ''hc_R''')

    % Calculate cross-correlograms
    [ccg,ic] = iosr.auditory.chXcorr_c(hc_L,hc_R,frame_count,frame_length,noverlap,maxlag,tau,inhib,ic_t,norm_flag,inhib_mode_ID);

    % Correct orientation of IC data, if data was transposed, and crop to remove appended zeros
    ic = ic(1:numsamples,:);
    if rot
        ic = ic';
    end

end

function output = check_input(input,dim,target)
%CHECK_INPUT check input is correct orientation

    if size(input,dim)~=target
        output = input';
        assert(size(output,dim)==target, 'iosr:chXcorr:invalidInputs', 'Input invalid')
    else
        output = input;
    end

end

function set = isparameter(input,parameter)
%ISPARAMETER check for input parameter

    set = any(strcmpi(input(cellfun(@ischar,input)),parameter));

end


function varargout = cosh(varargin)
%COSH   Hyperbolic cosine of a DISKFUN.
%   COSH(F) returns the hyperbolic cosine of F.
%
% See also DISKFUN/SINH, DISKFUN/COS.

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

[varargout{1:nargout}] = cosh@separableApprox(varargin{:});

end


%% CS294A/CS294W Linear Decoder Exercise

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear decoder exericse. For this exercise, you will only need to modify
%  the code in sparseAutoencoderLinearCost.m. You will not need to modify
%  any code in this file.

addpath '../library/'
addpath '../library/minFunc/'

%%======================================================================
%% STEP 0: Initialization
%  Here we initialize some parameters used for the exercise.

imageChannels = 3;     % number of channels (rgb, so 3)

patchDim = 8;          % patch dimension
numPatches = 100000;   % number of patches

visibleSize = patchDim * patchDim * imageChannels;  % number of input units
outputSize = visibleSize;   % number of output units
hiddenSize = 400;           % number of hidden units

sparsityParam = 0.035; % desired average activation of the hidden units.
lambda = 3e-3;         % weight decay parameter
beta = 5;              % weight of sparsity penalty term

epsilon = 0.1;	       % epsilon for ZCA whitening

% %%======================================================================
% %% STEP 1: Create and modify sparseAutoencoderLinearCost.m to use a linear decoder,
% %          and check gradients
% %  You should copy sparseAutoencoderCost.m from your earlier exercise
% %  and rename it to sparseAutoencoderLinearCost.m.
% %  Then you need to rename the function from sparseAutoencoderCost to
% %  sparseAutoencoderLinearCost, and modify it so that the sparse autoencoder
% %  uses a linear decoder instead. Once that is done, you should check
% % your gradients to verify that they are correct.
%
% % NOTE: Modify sparseAutoencoderCost first!
%
% % To speed up gradient checking, we will use a reduced network and some
% % dummy patches
% hiddenSize = 5;
% visibleSize = 8;
% patches = rand([8 10]);
% theta = initializeParameters(hiddenSize, visibleSize);
%
% [cost, grad] = sparseAutoencoderLinearCost(theta, visibleSize, hiddenSize, ...
%                                            lambda, sparsityParam, beta, ...
%                                            patches);
%
% % Check gradients
% numGrad = computeNumericalGradient( @(x) sparseAutoencoderLinearCost(x, visibleSize, hiddenSize, ...
%                                                   lambda, sparsityParam, beta, ...
%                                                   patches), theta);
%
% % Use this to visually compare the gradients side by side
% disp([numGrad grad]);
%
% diff = norm(numGrad-grad)/norm(numGrad+grad);
% % Should be small. In our implementation, these values are usually less than 1e-9.
% disp(diff);
%
% assert(diff < 1e-9, 'Difference too large. Check your gradient computation again');
%
% % NOTE: Once your gradients check out, you should run step 0 again to
% %       reinitialize the parameters
% %}

%%======================================================================
%% STEP 2: Learn features on small patches
%  In this step, you will use your sparse autoencoder (which now uses a
%  linear decoder) to learn features on small patches sampled from related
%  images.

%% STEP 2a: Load patches
%  In this step, we load 100k patches sampled from the STL10 dataset and
%  visualize them.

patches = load('../data/STL10Patches100k.mat');
patches = patches.patches;

displayColorNetwork(patches(:, 1:100));

%% STEP 2b: Apply preprocessing
%  In this sub-step, we preprocess the sampled patches, in particular,
%  ZCA whitening them.
%
%  In a later exercise on convolution and pooling, you will need to replicate
%  exactly the preprocessing steps you apply to these patches before
%  using the autoencoder to learn features on them. Hence, we will save the
%  ZCA whitening and mean image matrices together with the learned features
%  later on.

% Scale data to range [0, 1]
patches = patches / 255;

% Subtract mean patch (hence zeroing the mean of the patches)
meanPatch = mean(patches, 2);
patches = bsxfun(@minus, patches, meanPatch);

% Apply ZCA whitening
sigma = patches * patches' / numPatches;
[u, s, v] = svd(sigma);
ZCAWhite = u * diag(1 ./ (diag(s) + epsilon)) * u';
patches = ZCAWhite * patches;

displayColorNetwork(patches(:, 1:100));

%% STEP 2c: Learn features
%  You will now use your sparse autoencoder (with linear decoder) to learn
%  features on the preprocessed patches. This should take around 45 minutes.

theta = initializeParameters(hiddenSize, visibleSize);

% Use minFunc to minimize the function

clear('options');
options.Method = 'lbfgs';
options.maxIter = 400;
options.display = 'on';

[optTheta, cost] = minFunc( @(p) sparseAutoencoderLinearCost(p, ...
                                   visibleSize, hiddenSize, ...
                                   lambda, sparsityParam, ...
                                   beta, patches), ...
                              theta, options);

% Save the learned features and the preprocessing matrices for use in
% the later exercise on convolution and pooling
fprintf('Saving learned features and preprocessing matrices...\n');
save('STL10Features.mat', 'optTheta', 'ZCAWhite', 'meanPatch');
fprintf('Saved\n');

%% STEP 2d: Visualize learned features

W = reshape(optTheta(1:visibleSize * hiddenSize), hiddenSize, visibleSize);
b = optTheta(2*hiddenSize*visibleSize+1:2*hiddenSize*visibleSize+hiddenSize);
displayColorNetwork(W');


function c = nancov(x,varargin)
%NANCOV Covariance matrix, ignoring NaNs.
%   C = NANCOV(X), if X is a vector, returns the sample variance of the
%   values in X, treating NaNs as missing values.  For matrices, where
%   each row is an observation and each column a variable, NANCOV(X) is
%   the covariance matrix computing using rows of X that do not contain
%   any NaN values.  NANCOV(X,Y), where X and Y are matrices with
%   the same number of elements, is equivalent to NANCOV([X(:) Y(:)]).
%
%   NANCOV(X) or NANCOV(X,Y) normalizes by (N-1) if N>1, where N is the
%   number of observations after removing missing values.  This makes
%   NANCOV(X) the best unbiased estimate of the covariance matrix if the
%   observations are from a normal distribution. For N=1, COV normalizes
%   by N.
%
%   NANCOV(X,1) or NANCOV(X,Y,1) normalizes by N and produces the second
%   moment matrix of the observations about their mean.  NANCOV(X,Y,0) is
%   the same as NANCOV(X,Y), and NANCOV(X,0) is the same as NANCOV(X).
%
%   C = NANCOV(...,'pairwise') computes C(I,J) using rows with no NaN
%   values in columns I or J.  The result may not be a positive definite
%   matrix. C = NANCOV(...,'complete') is the default, and it omits rows
%   with any NaN values, even if they are not in column I or J.
%
%   The mean is removed from each column before calculating the
%   result.
%
%   Example:  Generate random data having non-zero covariance between
%             column 4 and the other columns.
%       x = randn(30,4);       % uncorrelated data
%       x(:,4) = sum(x,2);     % introduce correlation
%       x(2,3) = NaN;          % introduce one missing value
%       c = nancov(x)          % compute sample covariance
%
%   Class support for inputs X,Y:
%      float: double, single
%
%   See also COV, VAR, NANVAR.

%   Copyright 1984-2011 The MathWorks, Inc.
%   $Revision: 1.1.8.3 $  $Date: 2011/02/09 19:35:29 $

if nargin<1
   error(message('stats:nancov:NotEnoughInputs'));
end

% Should we ignore NaNs by complete rows or pairwise?
dopairwise = false;
if numel(varargin)>0
   temp = varargin{end};
   if ischar(temp)
      j = find(strncmpi(temp, {'pairwise' 'complete'},length(temp)));
      if isempty(j)
         error(message('stats:nancov:InvalidArg', temp));
      end
      dopairwise = (j==1);
      varargin(end) = [];
   end
end

% Should we use the mle (divide by N) or unbiased estimate (N-1)?
domle = false;
if numel(varargin)>0
   temp = varargin{end};
   if isequal(temp,0) || isequal(temp,1)
      domle = (temp==1);
      varargin(end) = [];
   end
end

if numel(varargin)>1
   error(message('stats:nancov:TooManyArgs'));
end

scalarxy = false; % nancov(scalar,scalar) is an ambiguous case
if numel(varargin)>0
   y = varargin{1};

   % Two inputs, convert to equivalent single input
   x = x(:);
   y = y(:);
   if length(x)~=length(y)
      error(message('stats:nancov:XYmismatch'));
   end
   scalarxy = isscalar(x) && isscalar(y);
   x = [x y];
elseif ndims(x)>2
   error(message('stats:nancov:InputDim'));
end

if isvector(x) && ~scalarxy
  x = x(:);
end

xnan = isnan(x);
[m,n] = size(x);

if isempty(x);
  if (m==0 && n==0)
      c = NaN(class(x));
  else
      c = NaN(n,class(x));
  end
  return;
end

if ~dopairwise || ~any(any(xnan))    % no need to do pairwise
   nanrows = any(xnan,2);
   if any(nanrows)
       x = x(~nanrows,:);
   end
   c = localcov(x,domle);
else                                         % pairwise with some NaNs
   % Compute variance using complete data separately by column
   c = zeros(n,class(x));
   x(xnan) = 0;
   colsize = sum(~xnan,1);
   xmean = sum(x,1) ./ max(1,colsize);
   xmean(colsize==0) = NaN;
   xc = x - repmat(xmean,m,1);
   xc(xnan) = 0;
   xvar = sum(xc.^2,1);
   if domle
      denom = colsize;
   else
      denom = max(0,colsize-1);
   end
   xvar(denom>1) = xvar(denom>1) ./ denom(denom>1);
   xvar(denom==0) = NaN;
   c(1:n+1:end) = xvar;

   % Now compute off-diagonal entries
   jk = 1:2;
   for j = 2:n
      jk(1) = j;
      for k=1:j-1
         jk(2) = k;
         rowsjk = ~any(xnan(:,jk),2);
         njk = sum(rowsjk);
         if njk<=1
            cjk = NaN;
         else
            cjk = localcov(x(rowsjk,jk),domle);
            cjk = cjk(1,2);
         end
         c(j,k) = cjk;
      end
   end
   c = c + tril(c,-1)';
end

% ------------------------------------------------
function [c,n] = localcov(x,domle)
%LOCALCOV Compute cov with no error checking and assuming NaNs are removed

[m,n] = size(x);
if domle
   denom = m;
else
   denom = max(0,m-1);
end

if m==1   % and doing mle, be sure to get exact 0
   c = zeros(n,class(x));
elseif denom==0
   c = NaN(n,class(x));
else
   xc = x - repmat(mean(x),m,1);
   c = xc' * xc / denom;
end


function varargout = calcTensor(odf,T,varargin)
% compute the average tensor for an ODF
%
% Syntax
%   [TVoigt, TReuss, THill] = calcTensor(odf,T)
%   THill = calcTensor(odf,T,'Hill')
%   TGeo = calcTensor(odf,T,'geometric')
%
% Input
%  odf - @SO3Fun
%  T   - @tensor
%
% Output
%  T    - @tensor
%
% Options
%  Voigt     - Boigt mean
%  Reuss     - Reuss mean
%  Hill      - Hill mean
%  geometric - geometric mean
%
% See also
% tensor/mean EBSD/calcTensor

% decide between the quadrature based method and the harmonic method
if ~check_option(varargin,'quadrature')

  % the harmonic route is directly implemented into tensor/mean
  [varargout{1:nargout}] = mean(T,odf,varargin{:});

else % quadrature based method

  % define a grid
  res = get_option(varargin,'resolution',2.5*degree);
  S3G = equispacedSO3Grid(odf.CS,odf.SS,'resolution',res);

  % evaluate the ODF
  f = eval(odf,S3G,varargin{:});
  f = f ./ sum(f(:));

  % compute the means
  [varargout{1:nargout}] = mean(S3G*T,'weights',f(:),varargin{:});

end

end


function [nrm] = normals(pnt, tri, opt)

% NORMALS compute the surface normals of a triangular mesh
% for each triangle or for each vertex
%
% Use as
%   [nrm] = normals(pnt, tri, opt)
% where opt is either 'vertex' (default) or 'triangle'.
%
% See also PCNORMALS, PROJECTTRI

% Copyright (C) 2002-2007, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<3
  opt='vertex';
elseif (opt(1)=='v' || opt(1)=='V')
  opt='vertex';
elseif (opt(1)=='t' || opt(1)=='T')
  opt='triangle';
else
  ft_error('invalid optional argument');
end

npnt = size(pnt,1);
ntri = size(tri,1);

% shift to center
pnt(:,1) = pnt(:,1)-mean(pnt(:,1),1);
pnt(:,2) = pnt(:,2)-mean(pnt(:,2),1);
pnt(:,3) = pnt(:,3)-mean(pnt(:,3),1);

% compute triangle normals
% nrm_tri = zeros(ntri, 3);
% for i=1:ntri
%   v2 = pnt(tri(i,2),:) - pnt(tri(i,1),:);
%   v3 = pnt(tri(i,3),:) - pnt(tri(i,1),:);
%   nrm_tri(i,:) = cross(v2, v3);
% end

% vectorized version of the previous part
v2 = pnt(tri(:,2),:) - pnt(tri(:,1),:);
v3 = pnt(tri(:,3),:) - pnt(tri(:,1),:);
nrm_tri = cross(v2, v3);

if strcmp(opt, 'vertex')
  % compute vertex normals
  nrm_pnt = zeros(npnt, 3);
  for i=1:ntri
    nrm_pnt(tri(i,1),:) = nrm_pnt(tri(i,1),:) + nrm_tri(i,:);
    nrm_pnt(tri(i,2),:) = nrm_pnt(tri(i,2),:) + nrm_tri(i,:);
    nrm_pnt(tri(i,3),:) = nrm_pnt(tri(i,3),:) + nrm_tri(i,:);
  end
  % normalise the direction vectors to have length one
  nrm = nrm_pnt ./ (sqrt(sum(nrm_pnt.^2, 2)) * ones(1,3));
else
  % normalise the direction vectors to have length one
  nrm = nrm_tri ./ (sqrt(sum(nrm_tri.^2, 2)) * ones(1,3));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fast cross product to replace the MATLAB standard version
function [c] = cross(a,b)
c = [a(:,2).*b(:,3)-a(:,3).*b(:,2) a(:,3).*b(:,1)-a(:,1).*b(:,3) a(:,1).*b(:,2)-a(:,2).*b(:,1)];


function fk_space = autoGen_fk_space(q1,q2,q3)
%AUTOGEN_FK_SPACE
%    FK_SPACE = AUTOGEN_FK_SPACE(Q1,Q2,Q3)

%    This function was generated by the Symbolic Math Toolbox version 8.4.
%    01-Jun-2020 11:59:03

t2 = cos(q1);
t3 = cos(q2);
t4 = sin(q1);
t5 = q2+q3;
t6 = cos(t5);
t7 = sin(t5);
t8 = t3.*(4.0./2.5e+1);
t9 = t6.*(5.7e+1./2.0e+2);
t10 = t8+t9;
fk_space = reshape([t2.*t6,t4.*t6,-t7,0.0,-t4,t2,0.0,0.0,t2.*t7,t4.*t7,t6,0.0,t2.*t10,t4.*t10,t7.*(-5.7e+1./2.0e+2)-sin(q2).*(4.0./2.5e+1),1.0],[4,4]);


function [varargout, peval] = separcomp(dpix, peval, winit_pix, hinit)
% separcomp(dpix, p_script, savethis, winit, hinit)
% separate components
% V ~ WH
% V -> N_pix x N_t
% W -> N_pix x N_comp - xth pixel of the ith components
% H -> N_copm x N_t - contribution of the i-th component in the time t

[peval.nx, peval.ny, peval.nt] = size(dpix);
peval.numpix = peval.nx*peval.ny;
peval.meandata = mean(dpix(:));

dvec = reshape(dpix,peval.numpix, peval.nt);
dpix_dip = dip_image(dpix);

% background subtraction with empirical values:
peval.bg_clip = 'no';
peval.bg_fs_var = 5;
peval.bg_perc = 20;
peval.bg_ob_dist = 8;
if ~isfield(peval, 'bg')
    [out_nobg, peval.bg, bg_im]=backgroundoffset(dpix_dip, peval.bg_clip, peval.bg_fs_var, peval.bg_perc, peval.bg_ob_dist);
end

if ~isfield(peval, 'ncomp')
    peval.ncomp=estimate_ncpomp(dvec);
end

[winit, hinit] = initwh(winit_pix, hinit, peval); %initialization of w and h

if ~isfield(peval, 'w_fixvec') peval.w_fixvec=[]; end
if ~isfield(peval, 'h_fixvec') peval.h_fixvec=[]; end

if isempty(peval.w_fixvec)
    peval.w_fixvec = peval.ncomp + 1; %fixing background component
elseif ~(peval.w_fixvec(end) == peval.ncomp + 1)
    peval.w_fixvec = [peval.w_fixvec, peval.ncomp + 1];
end

if isempty(peval.h_fixvec)
    peval.h_fixvec = peval.ncomp + 1; %fixing background component
elseif ~(peval.h_fixvec(end) == peval.ncomp + 1)
    peval.h_fixvec = [peval.h_fixvec, peval.ncomp + 1];
end

verbose = 1;
% if strcmp (peval.method, 'nmf_classic') %classical nmf updates
%     [w,h,peval, dtrace, htrace]=nmf_classic(dvec,winit,hinit,peval,verbose);
% elseif strcmp (peval.method, 'nmf_conjgrad_penalty') %classical nmf updates
%     [w,h,peval, dtrace, htrace]=nmf_conjgrad_penalty(dvec,winit,hinit,peval,verbose);
% end
eval (['[w,h,peval, dtrace, htrace]=' peval.method '(dvec,winit,hinit,peval,verbose);']);

varargout = struct('w',w,'h',h, 'dtrace', dtrace, 'htrace', htrace);


%% INS惯导解算
clear;
clc;
close all;

%%
Fs = 100;  %采样频率
N = 1000; %采样次数

dt = 1 / Fs;
gyr = [0.01, 0.02, 0.03]; %单位rad
acc = [0, 0, 9.8]; % 单位m/s^(2)

% 捷联惯导解算
p = zeros(3, 1);
v = zeros(3, 1);
q= [1 0 0 0]';

for i=1:N
    [p ,v, q] = ch_nav_equ_local_tan(p, v, q, acc', gyr' , dt, [0, 0, -9.8]');
    h_pos(i,:) = p;
    h_vel(i,:) = v;
    h_eul(i,:) = ch_q2eul(q);
end

figure;
subplot(2,2,1);
ch_plot_pos3d(h_pos);
subplot(2,2,2);
ch_plot_pos2d(h_pos);
subplot(2,2,3);
ch_plot_att(h_eul);

fprintf('纯积分测试: 陀螺(rad/s):%.3f %.3f %.3f\n', gyr(1), gyr(2), gyr(3));
fprintf('纯积分测试: 加计(m/s^(2)):%.3f %.3f %.3f\n', acc(1), acc(2), acc(3));

fprintf('解算:%d次 总时间:%.3fs\n', N, N /Fs);
fprintf('最终误差(m): %.3f %.3f %.3f\n', h_pos(end, 1),  h_pos(end, 2),  h_pos(end, 3));


function d = Mtv(solver,I,i)
% forward operator
%
% Input
%  I - intensities
%  alpha - scaling factors
%
% Output
%  d - result
%

% extend specimen symmetry
lss = numProper(solver.SS);
I_ext = repmat(I.',lss,1)./lss;

% compute Fourier coefficients
nfsftmex('set_f', solver.nfft_r(i), I_ext);
nfsftmex('adjoint', solver.nfft_r(i));
fhat = nfsftmex('get_f_hat_linear', solver.nfft_r(i));

% convolution with kernel function
fhat = 4*pi*fhat .* solver.A;

% evaluate Fourier series at pole figure points g h_i
nfsftmex('set_f_hat_linear', solver.nfft_gh(i), fhat);
nfsftmex('trafo', solver.nfft_gh(i));
d = real(nfsftmex('get_f', solver.nfft_gh(i)));
d = reshape(d,length(solver.c),[]) * solver.refl{i}.';

end


  function proj = ellipsoid_proj(cg, params, varargin)
%|function proj = ellipsoid_proj(cg, params, varargin)
%|
%| Compute set of 2d line-integral projection views of ellipsoid(s).
%| Works for both parallel-beam and cone-beam geometry.
%|
%| in
%|	cg			ct_geom()
%|	params [ne 9]		ellipsoid parameters:
%|			[x_center y_center z_center  x_radius y_radius z_radius
%|				xy_angle_degrees z_angle_degrees  amplitude]
%| options
%|	oversample		over-sampling factor for emulating "strips"
%|				(to account for finite detector size)
%|
%| out
%|	proj	[ns nt na]	projection views
%|
%| Copyright 2003-10-22, Patty Laskowsky, Nicole Caparanis, Taka Masuda,
%| and Jeff Fessler, University of Michigan

if nargin == 1 && streq(cg, 'test'), ellipsoid_proj_test, return, end
if nargin < 2, ir_usage, end

arg.oversample = 1;
arg = vararg_pair(arg, varargin);

proj = ellipsoid_proj_do(params, cg.s, cg.t, cg.ar, cg.source_zs, ...
		cg.dso, cg.dod, cg.dfs, arg.oversample);

end % ellipsoid_proj()

% ellipsoid_proj_do()
function proj = ellipsoid_proj_do(params, ss, tt, ...
		beta, ... % [radians]
		source_zs, dso, dod, dfs, oversample)

if size(params, 2) ~= 9, error '9 parameters per ellipsoid', end

if oversample > 1
	ds = ss(2) - ss(1);
	dt = tt(2) - tt(1);
	if any(abs(diff(ss) / ds - 1) > 1e-6) ...
	|| any(abs(diff(tt) / dt - 1) > 1e-6)
		error 'uniform spacing required for oversampling'
	end
	No = oversample;
	% determine new finer sampling positions
	ss = outer_sum([-(No-1):2:(No-1)]'/(2*No)*ds, ss(:)'); % [No ns]
	tt = outer_sum([-(No-1):2:(No-1)]'/(2*No)*dt, tt(:)'); % [No nt]
	proj = ellipsoid_proj_do(params, ss(:), tt(:), beta, source_zs, dso, dod, dfs, 1);
	proj = downsample3(proj, [No No 1]);
return
end

% determine equivalent parallel-beam projection coordinates, at beta=0
ns = length(ss);
nt = length(tt);
[sss ttt] = ndgrid(ss, tt);

if isinf(dso) % parallel beam
	uu = sss;
	vv = ttt;
	azim0 = 0;
	polar = 0;

elseif isinf(dfs) % cone-beam with flat detector
	[uu vv azim0 polar] = ir_coord_cb_flat_to_par(sss, ttt, dso, dod);

elseif dfs == 0 % cone-beam with arc detector
	[uu vv azim0 polar] = ir_coord_cb_arc_to_par(sss, ttt, dso, dod);

else
	fail 'not done'
end

clear sss ttt

cpolar = cos(polar);
spolar = sin(polar);
proj = zeros(ns, nt, numel(beta));

% loop over ellipsoids
for ip = 1:size(params,1)
	par = params(ip,:);

	cx = par(1);	rx = par(4);
	cy = par(2);	ry = par(5);
	cz = par(3);	rz = par(6);
	xang = deg2rad(par(7)); % xy-plane rotation
	zang = deg2rad(par(8)); % z-plane rotation
	if zang, error 'z rotation not done', end
	val = par(9);

	for ib = 1:length(beta)
%		az = beta(ib) + azim0 - xang; % assume source rotate in xy plane
		az = beta(ib) + azim0; % correction due to Lei Zhu of Stanford

		% shift property of 3D transform:
		cz_eff = cz - source_zs(ib); % center relative to source
		ushift = cx * cos(az) + cy * sin(az);
		vshift = (cx * sin(az) - cy * cos(az)) .* spolar + cz_eff * cpolar;

		az = az - xang; % correction due to Lei Zhu of Stanford
		p1 = (uu-ushift) .* cos(az) + (vv-vshift) .* sin(az) .* spolar;
		p2 = (uu-ushift) .* sin(az) - (vv-vshift) .* cos(az) .* spolar;
		p3 = (vv-vshift) .* cpolar;

		e1 = -sin(az) .* cpolar;
		e2 = cos(az) .* cpolar;
		e3 = spolar;

		A = e1.^2 / rx^2 + e2.^2 / ry^2 + e3.^2 / rz^2;
		B = p1 .* e1 / rx^2 + p2 .* e2 / ry^2 + p3 .* e3 / rz^2;
		C = p1.^2 / rx^2 + p2.^2 / ry^2 + p3.^2 / rz^2 - 1;

		proj(:,:,ib) = proj(:,:,ib) + 2 * val * sqrt(B.^2 - A.*C) ./ A;
	end
end

% trick: anywhere proj of a single ellipsoid is imaginary, the real part is 0.
proj = real(proj);
end % ellipsoid_proj_do()

% ellipsoid_proj_test()
function ellipsoid_proj_test

ell = [20 0*50 -40 200 100 50 90 0 10;
        0 50 100 80 80 20 0 0 10];

fun_proj = @(cg) ellipsoid_proj(cg, ell, 'oversample', 2); % analytical
fun_im = @(ig) ellipsoid_im(ig, ell, 'oversample', 2, 'checkfov', true);

ir_proj3_compare1(fun_proj, fun_im, 'chat', 1);

end % ellipsoid_proj_test()


% Test file for bndfun/poly.m

function pass = test_poly(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Set the domain
dom = [-2 7];

%%
% Check a few simple examples.

f = bndfun(@(x) zeros(size(x)), struct('domain', dom), pref);
p = poly(f);
pass(1) = (norm(p, inf) <= get(f, 'vscale')*eps);

f = bndfun(@(x) 3*ones(size(x)), struct('domain', dom), pref);
p = poly(f);
pass(2) = (norm(p - 3, inf) < get(f, 'vscale')*eps);

f = bndfun(@(x) 6.4*x - 3i, struct('domain', dom), pref);
p = poly(f);
pass(3) = (norm(p - [6.4 (-3i)], inf) < get(f, 'vscale')*eps);

f = bndfun(@(x) 2i*x.^5 - 3.2*x.^4 + 2*x.^2 - (1.2 + 3i), ...
    struct('domain', dom), pref);
p = poly(f);
pass(4) = (norm(p - [2i (-3.2) 0 2 0 -(1.2 + 3i)], inf) ...
    < get(f, 'vscale')*eps);

%%
% Verify operation for array-valued bndfun objects.

f = bndfun(@(x) [3*ones(size(x)), (6.4*x - 3i), (4*x.^2 - 2i*x + 3.7)], ...
    struct('domain', dom), pref);
p = poly(f);
p_exact = [0 0     3;
           0 6.4   (-3i);
           4 (-2i) 3.7];
pass(5) = (norm(p(:) - p_exact(:), inf) < ...
    max(get(f, 'vscale')*eps));

end


function [RET, REL, RETREL, ndx] = LMprecisionRecall(retrievedBoundingBox, annotation, objectname, mindetectablesize)
% This function returns one point in the precision-recall curve.
% Provided a set of detected bounding boxes, it indicates how may objects
% have been detected (RETREL).
%
% The standard measures for IR are recall and precision. Assuming that:
%
%    * RET is the set of all items the system has retrieved for a specific inquiry;
%    * REL is the set of relevant items for a specific inquiry;
%    * RETREL is the set of the retrieved relevant items
%
% then precision and recall measures are obtained as follows:
%
%    precision = RETREL / RET
%    recall = RETREL / REL

% Search the target object in the annotation
j = LMobjectindex(annotation, objectname);

% Do not consider deleted objects:
j = j(find(strcmp({annotation.object(j).deleted}, '0')));

Ninstances = length(j);

% Extract the bounding boxes for each target present in the image
BoundingBox = []; REL = 0;
for i = 1:Ninstances
    [X,Y] = getLMpolygon(annotation.object(j(i)).polygon);
    BoundingBox = [BoundingBox; min(X) max(X) min(Y) max(Y)];

    % If we detect an object that we thought was too small, we should not
    % penalize performance for this. So, we will consider that the only
    % relevant targets are the ones with a size larger than the minimal
    % detectable size.
    if max(Y)-min(Y)>mindetectablesize(1) & max(X)-min(X)>mindetectablesize(2) & max(Y)>0 & max(X)>0
        REL = REL + 1;
    end
end

RET = size(retrievedBoundingBox, 1);

if REL == 0
    RETREL = 0;
    return
end

cxO = mean(BoundingBox(:,1:2),2);
cyO = mean(BoundingBox(:,3:4),2);
DxO = diff(BoundingBox(:,1:2),1,2);
DyO = diff(BoundingBox(:,3:4),1,2);

cxR = mean(retrievedBoundingBox(:,1:2),2);
cyR = mean(retrievedBoundingBox(:,3:4),2);
DxR = diff(retrievedBoundingBox(:,1:2),1,2);
DyR = diff(retrievedBoundingBox(:,3:4),1,2);

ndx = [];
for i = 1:RET
    d = sqrt(((cxR(i) - cxO)./DxO).^2 + ((cyR(i) - cyO)./DyO).^2)<.5 ...
        & max(DxO/DxR(i), DxR(i)./DxO)<1.5 ...
        & max(DyO/DyR(i), DyR(i)./DyO)<1.5;
    n = find(d);
    if length(n)>0
        ndx(i) = n(1);
    else
        ndx(i) = 0;
    end
end
RETREL = sum(unique(ndx)>0); % each object can be detected only once

% If we detect an object that we thought was too small, we should not
% penalize performance for this. So, if we detected one object that was not
% considered before within the relevan set, then we will move it into the
% relevant set:
REL = max(REL, RETREL);


%% rigidTransformationMatrixDirect
% Below is a demonstration of the features of the |rigidTransformationMatrixDirect| function

%%
clear; close all; clc;

%% Syntax
% |[M]=rigidTransformationMatrixDirect(V1,V2);|

%% Description
% This function computes the rigid transformation matrix (translation, and
% rotation). For the two point matches input sets V1 and V2.

%% Examples

%%
% Plot settings
fontSize=15;
faceAlpha=1;
edgeColor='k';

%% Example:

%% Determine the rigid transformation to overlay two surfaces
% Below is a demonstration of the features of the |rigidTransformationMatrixDirect| function

% Load example patch data
[F,Vd]=parasaurolophus;

%Translation
OR=[-1 2 -3]; %Translations
T  = [1 0 0 OR(1);...
      0 1 0 OR(2);...
      0 0 1 OR(3);...
      0 0 0 1];
%Rotation
a=[-0.25*pi 0.75*pi 0.1*pi]; %Euler angles
R  = eye(4,4); R(1:3,1:3)=euler2DCM(a);

%Scaling
v=[2 3 1]; %Scaling factors
S  = [v(1) 0    0    0;...
      0    v(2) 0    0;...
      0    0    v(3) 0;...
      0    0    0    1];

M_true  = T * R; %The true transformation matrix

%Point set 1
V1=Vd+0.5;

%Point set 2
V2=tform(M_true,V1);

%%
% Plotting input data

hf=cFigure;
title('The untransformed surfaces','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);

hp=patch('Faces',F,'Vertices',V1,'FaceColor','g','FaceAlpha',faceAlpha);

hp=patch('Faces',F,'Vertices',V2,'FaceColor','r','FaceAlpha',faceAlpha,'edgeColor',edgeColor);

camlight headlight;
set(gca,'FontSize',fontSize);
view(3); axis tight;  axis equal; box on;
drawnow;

%%
% Get the transformation matrix for the point matched data using |rigidTransformationMatrixDirect|

[M_fit]=rigidTransformationMatrixDirect(V1,V2);

V1f=tform((M_fit),V1);

%%
% Plotting results

hf=cFigure;
title('The green surfaces transformed towards the red','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);

hp=patch('Faces',F,'Vertices',V2,'FaceColor','r','FaceAlpha',0.5,'edgeColor',edgeColor);

hp=patch('Faces',F,'Vertices',V1f,'FaceColor','none','FaceAlpha',0.5,'edgeColor','g');
% hp=patch('Faces',F,'Vertices',V2ff,'FaceColor','none','FaceAlpha',0.5*2,'edgeColor','b');

set(gca,'FontSize',fontSize);
view(3); axis tight;  axis equal; box on;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function sump=sum(p,dim)
% sympoly/sum: sum a sympoly array along a given dimension
% usage: sump=sum(p);
% usage: sump=sum(p,dim);
%
% arguments:
%    p - sympoly array object
%  dim - (OPTIONAL) dimension to sum over
%        DEFAULT ==1, unless sp is a row vector
%
%  sump - sympoly object containing the sum reduced array

s = size(p);
np = length(s);

% default for dim is 1, UNLESS p is a row vector.
if (nargin<2) || isempty(dim)
  if (s(1) == 1) && (np==2)
    % a row vector
    dim = 2;
  else
    % any other shape array
    dim = 1;
  end
end

% for dim == 1 or dim == 2, do the sum using a dot product
if (dim == 1) && (np == 2)
  % sum down rows
  sump = ones(1,s(1))*p;

elseif (dim == 2) && (np == 2)
  % sum across columns
  sump = p*ones(s(2),1);

else
  % its an n-d array
  ss = s;
  ss(dim) = 1;

  si = cell(1,np);
  for i = 1:np
    si{i} = 1:s(i);
  end

  if any(ss~=1)
    sump = repmat(sympoly(0),ss);
  else
    sump = sympoly(0);
  end
  for i = 1:s(dim)
    si{dim} = i;
    sump = sump + p(si{:});
  end

end


function [w, infos] = cd_lasso_elasticnet(problem, options)
% Coordinate descent algorithm for LASSO problem.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Apr. 18, 2017

    % set dimensions and samples
    d = problem.dim();
    n = problem.samples();
    A = problem.A();

    % extract options
    if ~isfield(options, 'tol_optgap')
        tol_optgap = 1.0e-12;
    else
        tol_optgap = options.tol_optgap;
    end

    if ~isfield(options, 'tol_gnorm')
        tol_gnorm = 1.0e-12;
    else
        tol_gnorm = options.tol_gnorm;
    end

    if ~isfield(options, 'max_iter')
        max_iter = 100;
    else
        max_iter = options.max_iter;
    end

    if ~isfield(options, 'verbose')
        verbose = false;
    else
        verbose = options.verbose;
    end

    if ~isfield(options, 'w_init')
        w = randn(d,1);
    else
        w = options.w_init;
    end

    if ~isfield(options, 'f_opt')
        f_opt = -Inf;
    else
        f_opt = options.f_opt;
    end

    if ~isfield(options, 'store_w')
        store_w = false;
    else
        store_w = options.store_w;
    end

    if ~isfield(options, 'sub_mode')
        sub_mode = 'lasso';
    else
        sub_mode = options.sub_mode;
    end

    % initialise
    iter = 0;
    if strcmp(sub_mode, 'lasso')
        AtA = problem.AtA();
        squred_norm_col = diag(AtA);
    else
        AtA_l2 = problem.AtA_l2();
        squred_norm_col = diag(AtA_l2);
    end
    prox_th = ones(d, 1)./squred_norm_col;

    % store first infos
    clear infos;
    infos.iter = iter;
    infos.time = 0;
    infos.grad_calc_count = 0;
    f_val = problem.cost(w);
    infos.cost = f_val;
    optgap = f_val - f_opt;
    infos.optgap = optgap;
    grad = problem.full_grad(w);
    gnorm = norm(grad);
    infos.gnorm = gnorm;
    if ismethod(problem, 'reg')
        infos.reg = problem.reg(w);
    end
    if store_w
        infos.w = w;
    end

    % set start time
    start_time = tic();

    % print info
    if verbose
        fprintf('CD (%s): Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', sub_mode, iter, f_val, gnorm, optgap);
    end

    % main loop
    while (optgap > tol_optgap) && (gnorm > tol_gnorm) && (iter < max_iter)

        % update i-th coordinate
        if strcmp(sub_mode, 'lasso')
            for i = 1:d
                w_except_i = w;
                w_except_i(i) = 0;
                residual = problem.residual(w_except_i);

                snc = squred_norm_col(i);
                w(i) = problem.prox(A(:, i)'*residual/snc, prox_th(i));
            end
        else
            for i = 1:d
                w_except_i = w;
                w_except_i(i) = 0;
                residual = problem.residual(w_except_i, i);

                snc = squred_norm_col(i);
                w(i) = problem.prox(residual/snc, prox_th(i));
            end

        end

        % calculate gradient
        grad = problem.full_grad(w);

        % update iter
        iter = iter + 1;
        % calculate error
        f_val = problem.cost(w);
        optgap = f_val - f_opt;
        % calculate norm of gradient
        gnorm = norm(grad);

        % measure elapsed time
        elapsed_time = toc(start_time);

        % store infoa
        infos.iter = [infos.iter iter];
        infos.time = [infos.time elapsed_time];
        infos.grad_calc_count = [infos.grad_calc_count iter*n];
        infos.optgap = [infos.optgap optgap];
        infos.cost = [infos.cost f_val];
        infos.gnorm = [infos.gnorm gnorm];
        if ismethod(problem, 'reg')
            reg = problem.reg(w);
            infos.reg = [infos.reg reg];
        end
        if store_w
            infos.w = [infos.w w];
        end

        % print info
        if verbose
            fprintf('CD (%s): Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', sub_mode, iter, f_val, gnorm, optgap);
        end
    end

    if gnorm < tol_gnorm
        fprintf('Gradient norm tolerance reached: tol_gnorm = %g\n', tol_gnorm);
    elseif optgap < tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', tol_optgap);
    elseif iter == max_iter
        fprintf('Max iter reached: max_iter = %g\n', max_iter);
    end

end


%UMFPACK_SIMPLE a simple demo
%
% Example:
%   umfpack_simple
%
% Copyright 1995-2007 by Timothy A. Davis.
%
% UMFPACK License:
%
%     Your use or distribution of UMFPACK or any modified version of
%     UMFPACK implies that you agree to this License.  UMFPACK is
%     is free software; you can redistribute it and/or
%     modify it under the terms of the GNU General Public
%     License as published by the Free Software Foundation; either
%     version 2 of the License, or (at your option) any later version.

% Availability: http://www.cise.ufl.edu/research/sparse/umfpack
%
% See also: umfpack, umfpack2, umfpack_details

help umfpack_simple

format short

A = [
 2  3  0  0  0
 3  0  4  0  6
 0 -1 -3  2  0
 0  0  1  0  0
 0  4  2  0  1
] ;
fprintf ('A = \n') ; disp (A) ;

A = sparse (A) ;

b = [8 45 -3 3 19]' ;
fprintf ('b = \n') ; disp (b) ;

fprintf ('Solution to Ax=b via UMFPACK:\n') ;
fprintf ('x1 = umfpack2 (A, ''\\'', b)\n') ;

x1 = umfpack2 (A, '\', b) ;
fprintf ('x1 = \n') ; disp (x1) ;

fprintf ('Solution to Ax=b via MATLAB:\n') ;
fprintf ('x2 = A\\b\n') ;

x2 = A\b ;
fprintf ('x2 = \n') ; disp (x2) ;

fprintf ('norm (x1-x2) should be small: %g\n', norm (x1-x2)) ;

fprintf ('Type ''umfpack_demo'' for a full demo of UMFPACK\n') ;


% Chapter 14 - Poincare Maps and Nonautonomous Systems in the Plane.
% Programs 14a - Solving an initial value problem.
% Copyright Birkhauser 2013. Stephen Lynch.

% Solve a differential equation (Example 1).
r=dsolve('Dr=-r^2','r(0)=1');

% List the first eight returns on the segment {y=0, 0<x<1}.
% There may be small inaccuracies due to the numerical solution.
deq=inline('[-(r(1))^2]','t','r');
options=odeset('RelTol',1e-6,'AbsTol',1e-6);
[t,returns]=ode45(deq,0:2*pi:16*pi,1);
returns

% End of Programs 14a.


function [csubl, csubd] = aerodyn(alpha)

% aerodynamic properties

% input

%  alpha = angle-of-attack (radians)

% output

%  csubl = lift coefficient (non-dimensional)
%  csubd = drag coefficient (non-dimensional)

% STS aero - J. Betts model

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rtd = 180.0 / pi;

cl0 = -0.20704d0;

cl1 = 0.029244d0;

cd0 = 0.07854d0;

cd1 = -6.1592d-3;

cd2 = 6.21408d-4;

alphad = rtd * alpha;

csubl = cl0 + cl1 * alphad;

csubd = cd0 + (cd1 + cd2 * alphad) * alphad;


%
% Bayesian Optimization of Combinatorial Structures
%
% Copyright (C) 2018 R. Baptista & M. Poloczek
%
% BOCS 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.
%
% BOCS 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 BOCS.  If not, see <http://www.gnu.org/licenses/>.
%
% Copyright (C) 2018 MIT & University of Arizona
% Authors: Ricardo Baptista & Matthias Poloczek
% E-mails: rsb@mit.edu & poloczek@email.arizona.edu
%

clear; close all; clc
addpath(genpath('../algorithms'))
addpath(genpath('../stat_model'))
addpath(genpath('../test_problems/IsingModel'))
addpath(genpath('../tools'))

% Save inputs in struct
inputs = struct;
inputs.n_vars     = 12;
inputs.evalBudget = 100;
inputs.n_init     = 20;
inputs.lambda     = 1e-4;
inputs.estimator  = 'horseshoe';

% Generate random 3x3 graphical model
Theta   = rand_ising_grid(9);
Moments = ising_model_moments(Theta);

% Save objective function and regularization term
inputs.model    = @(x) KL_divergence_ising(Theta, Moments, x);
inputs.penalty  = @(x) inputs.lambda*sum(x,2);

% Generate initial samples for statistical models
inputs.x_vals   = sample_models(inputs.n_init, inputs.n_vars);
inputs.y_vals   = inputs.model(inputs.x_vals);

% Run BOCS-SDP and BOCS-SA (order 2)
B_SA  = BOCS(inputs.model, inputs.penalty, inputs, 2, 'SA');
B_SDP = BOCS(inputs.model, inputs.penalty, inputs, 2, 'sdp');

% Plot results
figure
hold on
plot(1:length(B_SA.objVals),  cummin(B_SA.objVals), '-r');
plot(1:length(B_SDP.objVals), cummin(B_SDP.objVals), '-.b');
set(gca,'YScale','log')
xlabel('$t$','interpreter','latex')
ylabel('Best $f(x)$','interpreter','latex')
legend({'BOCS - SA','BOCS - SDP'},'location','northeast')
hold off


function [F] = mci_ramsay_fx (x,U,P,M)
% State equation for Ramsay model
% FORMAT [F] = mci_ramsay_fx (x,U,P,M)
%
% x     State vector
%       x(1) Voltage variable
%       x(2) Recovery variable
% U     inputs
% P     vector of model parameters - 2 params only
% M     model
%
% F     dx/dt
%
% J Ramsay et al (2007) Parameter estimation for differential equations:
% a generalised smoothing approach. J Roy Stat Soc B, 69(5):741-796.
%
% See also section 10 (page 26) and contribution by W.Penny on page 75 of:
%
% Girolami and Calderhead (2011) Riemann manifold Langevin and Hamiltonian
% Monte Carlo methods. J Roy Stat Soc B,73(2):123-214.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_ramsay_fx.m 6548 2015-09-11 12:39:47Z will $

c=3;

P(1)=exp(P(1));
P(2)=exp(P(2));

F=zeros(2,1);
F(1)=c*(x(1)-(1/3)*x(1)^3+x(2));
F(2)=-(1/c)*(x(1)-P(1)+P(2)*x(2));


function [mxc,mlat,OUT] = xc_stats(xc,xl,varargin)
% [mxc,mlat,OUT] = xc_stats(xc,xl,varargin)
% calculate t-statistic values for a k x k x n 3-D matrix of
% correlation (xc) and latency (xl) values across n subjects.
%
% tor wager

names = [];
if length(varargin) > 0, names = varargin{1};,end

 if isempty(xl), mlat = [];, end

% ------------------------------------------------------------
% stats on correlation values
% ------------------------------------------------------------

warning off, clear stelatency, clear tlat, clear stecorr, clear tcorr
% standard errors and t-values, element by element

% only do this on the upper triangle, to be faster

for j = 1:size(xc,2)-1
    for k = j + 1 : size(xc,2)

        if ~isempty(xl)
            tmp = squeeze(xl(j,k,:));
            z = tmp;
            [stelatency(j,k),tlat(j,k)] = ste(z);   % t-test on latencies; maybe should be poisson dist?
        end

        tmp = squeeze(xc(j,k,:));
        z = .5 * log( (1+tmp) ./ (1-tmp) );     % Fisher's r-to-z transform
        [stecorr(j,k),tcorr(j,k)] = ste(z);     % correl in rand fx analysis across ss
    end
end
warning on

% makes symmetric matrices from upper tri

if ~isempty(xl), stelatency(end+1,:) = 0; ,end
stecorr(end+1,:) = 0;
tcorr(end+1,:) = 0;
if ~isempty(xl),
    tlat(end+1,:) = 0;
    stelatency = stelatency + stelatency';
end

stecorr = stecorr + stecorr' + Inf * eye(size(stecorr,1));
tcorr = tcorr + tcorr';     % t-scores for significance/reliability of cross-correlations across subjects

if ~isempty(xl)
    tlat = tlat + tlat';        % same for time lags across subjects
end

% group means

if ~isempty(xl)
    mlat = mean(xl,3);          % mean of latency data (cross-lag, group average)
end
mxc = mean(xc,3);           % mean cross-correlations among k regions, group average

% ------------------------------------------------------------
% print and save output
% ------------------------------------------------------------

OUT.desc1 = 'xl is latency mtx for each subject, xc is individual correlation mtx';
if ~isempty(xl), OUT.xl = xl;, end
OUT.xc = xc;
OUT.desc2 = 'mlat and mxc: matrices of means across subjects for latency and correl'
OUT.desc3 = 'tlat/tcorr are t-values across z-transformed correlations';
OUT.mxc = mxc;

if ~isempty(xl)
    OUT.mlat = mlat;
    OUT.tlat = tlat;
    OUT.tcorr = tcorr;
end

% Output
if ~isempty(xl)
    fprintf(1,'Latency\n');
    str = correlation_to_text(mlat,Inf,names);
end

fprintf(1,'correlation T-values, p<.05 corrected, assuming normal distribution of latency diffs\n');
% Alpha correction - bonferroni.
numobs=(size(mxc,1)*(size(mxc,1)-1))/2;
corrp=1-(0.05/ (2 * (   numobs   )));       % 2-tailed corr p
crit_t = tinv_t(corrp,size(xc,1)-3);          % critical t-value for corrected significance
crit_tu = tinv_t(1-(.05/2),size(xc,1)-3);         % critical t-value for uncorrected significance

if ~isempty(xl)
    str = correlation_to_text(tlat,crit_t,names);
end

OUT.crit_t = crit_t;
OUT.crit_tu = crit_tu;
OUT.crit_tdesc = 'crit_t crit t value corrected, _tu uncorrected';
OUT.corrp = corrp;
OUT.corrpdesc = 'critical corrected p-value (bonferroni)';

fprintf(1,'Average correlation across subjects\n');
[str, sigu] = correlation_to_text(mxc,Inf,names);

fprintf(1,'Corr T-values, p<.05 uncorrected, 2-tailed, assuming independent samples over time\n');
[str, sigu] = correlation_to_text(tcorr,crit_tu,names);
OUT.sigmat_uncorrected = sigu;

fprintf(1,'Corr T-values, p<.05 corrected, 2-tailed, assuming independent samples over time\n');
[str, sigmat] = correlation_to_text(tcorr,crit_t,names);

OUT.sigmat = sigmat;

return


function t=getDCTTransform(im,N)
s=dctmtx(N);
t=s*im*s';


function gIX = KmeansSubdivide(numK2,gIX,M_0)
gIX_old = gIX;
U = unique(gIX);
        for i = 1:length(U),
            IX = find(gIX_old == U(i));
            M_sub = M_0(IX,:);

            if numK2<length(IX),
                [gIX_sub,C] = kmeans(M_sub,numK2,'distance','correlation');
            else
                [gIX_sub,C] = kmeans(M_sub,length(IX),'distance','correlation');
            end
            gIX(IX) = (i-1)*numK2+gIX_sub;
        end
end


function dx = lorenz(t,x,Beta)
dx = [
Beta(1)*(x(2)-x(1));
x(1)*(Beta(2)-x(3))-x(2);
x(1)*x(2)-Beta(3)*x(3);
];


function grad = compute_NN_grad(W, X, resp, hout, nonlinearity, nonzero_grad)

% nonlinearity = 1 -- hyperbolic tangent
% nonlinearity = 2 -- logistic sigmoid (subtract 0.5 from last layer's outputs)
% nonlinearity = 3 -- logistic sigmoid + hyperbolic tangent (last layer)

ncases = size(resp{1},2);
if isa(X, 'gsingle')
  gputype = 'gsingle';
  onesncases = gones(ncases,1,gputype);
elseif isa(X, 'gdouble')
  gputype = 'gsingle';
  onesncases = gones(ncases,1,gputype);
else
  onesncases = ones(ncases,1);
end

nlayer = numel(W);

if (nonlinearity == 1)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad).^2);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' ones(snonzero_grad, 1)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}.^2);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

elseif (nonlinearity == 2)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)) .* resp{i}(:,nonzero_grad);

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' gones(snonzero_grad,1,gputype)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      backward{i} = tmp .* (1 - resp{i}) .* resp{i};

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

elseif (nonlinearity == 3)

  if (exist('nonzero_grad', 'var'))
    snonzero_grad = sum(nonzero_grad);
    if (snonzero_grad == 0)
      for i=nlayer:-1:1
	grad{i} = zeros(size(W{i}));
      end
      return;
    end

    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout(:, nonzero_grad);
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      if (i == nlayer)
	backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)^2);
      else
	backward{i} = tmp .* (1 - resp{i}(:,nonzero_grad)) .* resp{i}(:,nonzero_grad);
      end

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}(:,nonzero_grad)' gones(snonzero_grad,1,gputype)];
	else
	  grad{i} = backward{i} * resp{i-1}(:,nonzero_grad)';
	end
      else
	grad{i} = backward{i} * X(:, nonzero_grad)';
      end
    end
  else
    for i=nlayer:-1:1
      if (i == nlayer)
	tmp = hout;
      else
	tmp = (W{i+1}' * backward{i+1});
      end

      if (size(tmp, 1) ~= size(resp{i}, 1))
	tmp = tmp(1:end-1, :);
      end

      if (i == nlayer)
	backward{i} = tmp .* (1 - resp{i}.^2);
      else
	backward{i} = tmp .* (1 - resp{i}) .* resp{i};
      end

      if (i-1) >= 1
	if (size(W{i}, 2) == size(resp{i-1}, 1) + 1)
	  grad{i} = backward{i} * [resp{i-1}' onesncases];
	else
	  grad{i} = backward{i} * resp{i-1}';
	end
      else
	grad{i} = backward{i} * X';
      end
    end
  end

end


function s=sprintsi(x,d,w)
%SPRINTSI Print X with SI multiplier S=(X,D,W)
% D is number of decimal places (+ve) or significant digits (-ve) [default=-3]
% |W| is total width including multiplier
% if W<=0 then trailing 0's will be eliminated
%
% Example: sprintsi(2345,-2) gives '2.3 k'

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: sprintsi.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3 w=0; end;
if nargin<2 d=-3; end;
f='afpnum kMGT';
e=max(-18,min(12,floor(log10(abs(x)))));
k=floor(e/3);
dp=max([0 d 3*k-d-e-1]);
if w<=0 & dp
   w=abs(w);
   dp=max(find([1 mod(mod(round(x*10^(dp-3*k)),10^dp),10.^(dp:-1:1))]))-1;
end
if(k)
   s=sprintf(sprintf('%%%d.%df %c',w-2,dp,f(k+7)),x*1e-3^k);
else
   s=sprintf(sprintf('%%%d.%df ',w-1,dp),x*1e-3^k);
end


function pass = test_minus( pref )
% This tests the basic arithmetic operations on chebfun2 objects.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e5 * pref.cheb2Prefs.chebfun2eps;
j = 1;

D = [-1 1 -1 1; -2 2 -2 2; -1 pi 0 2*pi];

for r = 1 : size(D,1)
    f = chebfun2(@(x,y) cos(x.*y), D(r,:));
    g = chebfun2(@(x,y) x + y + x.*y, D(r,:));

    FminusG = chebfun2(@(x,y) cos(x.*y) - (x + y + x.*y), D(r,:) );

    tolr = norm(D(r,:),inf)*tol;

    pass(j) = ( norm( (f-g) - FminusG ) < 10*tolr ); j = j + 1;

end

end


% GP_COV_SUM - Covariance function as a sum of two covariance functions.
%
% Usage:
%
%   COVFUNC = GP_COV_SUM(COVFUNC1, COVFUNC2)
%
% The returned covariance function is called as
%
%   K = COVFUNC(THETA)
%   [K, DK] = COVFUNC(THETA)
%
% where THETA is a vector of parameters. The parameters of COVFUNC1 and
% COVFUNC2 are concatenated into a single parameter vector.
%
% If no arguments are given, dimensionality information is returned:
%
%   [N_THETA, N1, N2] = COVFUNC()
%
% N_THETA : Number of non-fixed parameters
% N1      : Number of rows in the covariance matrix
% N2      : Number of columns in the covariance matrix
%
% See also GP_COV, GP_COV_PRODUCT.

% Last modified 2010-01-25
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function func = gp_cov_sum(varargin)

func = @get_covariance;

if nargin < 2
  error('Must give at least two covariance functions')
end

covfuncs = varargin ;

% Number of covariance functions
n_funcs = nargin ;

% Number of parameters for each covariance function
n_theta = zeros(n_funcs,1) ;
% Dimensionalities of each covariance function
M = zeros(n_funcs,1) ;
N = zeros(n_funcs,1) ;
% Extract the values from the covariance functions
for i = 1:n_funcs
  [n_theta(i), M(i), N(i)] = varargin{i}();
end

% Indices of the hyperparameters for each covariance function
ind_theta = cell(n_funcs,1);
for i = 1:n_funcs
  ind_theta{i} = (1+sum(n_theta(1:(i-1)))):(sum(n_theta(1:i))) ;
end

%[n_theta1, M1, N1] = covfunc1();
%[n_theta2, M2, N2] = covfunc2();

if ~all(M == M(1)) || ~all(N == N(1))
  error('Can''t sum covariance matrices with different dimensionalities');
end
M = M(1) ;
N = N(1) ;

  function varargout = get_covariance(theta)

  if nargout == 0
    nout = 1 ;
  else
    nout = nargout ;
  end

  varargout = cell(nout,1);

  out = cell(nout,1);
  varargout = cell(nout,1);

  % Return only dimension information if requested
  if nargin == 0
    if nout >= 1
      varargout{1} = sum(n_theta); % number of parameters
      if nout >= 2
        varargout{2} = M; % dimensionalities
        if nout >= 3
          varargout{3} = N; % dimensionalities
        end
      end
    end
    return
  end

  varargout{1} = 0 ;
  varargout{2} = cell(sum(n_theta),1) ;

  for i = 1:n_funcs
    % Compute the covariance matrix
    [out{:}] = covfuncs{i}(theta(ind_theta{i}));
    varargout{1} = varargout{1} + out{1};

    % Compute the derivative
    if nout >= 2
      varargout{2}(ind_theta{i}) = out{2}(:);
    end
  end

  end

end


function [Xmat,idxisa] = workvolume(cam,room,imres,idxcams)

STEP = 0.1;

if nargin < 4
	idxcams = [1:size(cam,2)];
end

if nargin < 3
	imres = repmat([640 480],size(idxcams,2),1);
end
imres = imres(idxcams,:);

if nargin < 2
	% room [x_min, x_max, y_min, y_max, z_min, z_max]
	room = [-3 3 -3 3 0 3];
end

% compose Pmat containing all P matrices
Pmat = [];
for i=idxcams,
	Pmat = [Pmat; cam(i).P];
end

% create points

zcoor = room(5):STEP:room(6);
znum = size(zcoor,2);
ycoor = room(3):STEP:room(4);
ynum = size(ycoor,2);
xcoor = room(1):STEP:room(2);
xnum = size(xcoor,2);

buff  = repmat(ycoor,znum,1);
yvec  = reshape(buff,prod(size(buff)),1);
zvec  = repmat(zcoor',ynum,1);

buff = repmat(xcoor,znum*ynum,1);
xvec = reshape(buff,prod(size(buff)),1);

Xmat = [xvec,repmat([yvec,zvec],xnum,1)];
Xmat = [Xmat,ones(size(Xmat(:,1)))];

clear buff

size(Xmat)
umat = Pmat*Xmat';

% normalize projected points in umat
scalemat = [];
for i=1:size(idxcams,2),
	scalemat = [scalemat; repmat(umat(3*i,:),3,1)];
end
umat = umat./scalemat;
clear scalemat;
mask = zeros(size(umat));
mask(1:3:end,:) = umat(1:3:end,:)<repmat(imres(:,1),1,size(mask,2));
mask(2:3:end,:) = umat(2:3:end,:)<repmat(imres(:,2),1,size(mask,2));
mask(3:3:end,:) = 1;
mask = mask.*(umat>0);
idxisa = find(sum(mask)==size(mask,1));

return;