text:
function [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
%   [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%
% --- Input
%  X  : Input data  ( M x T )
%  Y  : Output data ( N x T )
%  N  =  # of output
%  M  =  # of input
%  T  =  # of data
%
%  Model : Structure for estimated model
%  Model.SY0 :  Output data variance                 ( 1 x 1 )
%  Model.A0  :  (Output data var)/(Input data var)   ( 1 x 1 )
%
%  parm  : Structure for learning parameter
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.SW  :  Weight variance        ( M x M )
%  Model.W   :  Weight matrix          ( N x M )
%  Model.A   :  Prior weight variance  ( N x M ) ARD hyper parameter
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = - (Log error)
%  Info.H   = - (# of effective weight parameters)
%
% 2007/1/26 Made by M. Sato

% Constants
MINVAL  = 1.0e-15;
MinCond = 1.0e-10;

% # of total training iteration
Ntrain = parm.Ntrain;

Nskip  = 100;   % skip steps for display info
a_min  = 1e-10; % Minimum value for weight pruning
Fdiff  = 1e-10; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end

% Dimension
[M ,Tx ,Nx ]= size(X); % input dim
[N ,Ty ,Ny ]= size(Y); % output dim

if Nx~=Ny, error('Trial number is different for input & output'); end
if Tx~=Ty, error('Time sample is different for input & output'); end

% Reshape into 2D matrix
T = Tx*Nx; % # of data
X = reshape(X, [M T]);
Y = reshape(Y, [N T]);

% # of stable VB-update in initial training
if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
%	Npre_train = 0;
	if T >= 2*M*D
		Npre_train = 0;
	elseif T >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end
if Npre_train > Ntrain, Npre_train = Ntrain; end;

fprintf('linear map sparse covariance_pinv start\n')
fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Nx)
fprintf('--- Number of training sample = %d\n',Tx)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

%
% --- Initialization
%  A  : Initial variable to use 1st update
%     : 1 x M

% Input/Output variance
sx = mean(repadd(X, - mean(X,2)).^2, 2);
sy = mean(repadd(Y, - mean(Y,2)).^2, 2);

A0  = 1./mean(sx);
SY0 = mean(sy);

if isempty(Model)
	A   = repmat(A0, [1,M]);
	W   = zeros(N,M);
	SY  = SY0;
else
	A   = Model.A ;	 % 1 x M
	W   = Model.W ;  % N x M
	SY  = mean(Model.SY);  % 1 x 1
end

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

%  --- Initialization by other method ---
% Model.mode = 'scalar': ARD term = alpha * W^2
% Model.mode = 'cov'   : ARD term = alpha * W^2 * SY(^-1)
%
if isfield(Model, 'mode') &&  strcmp(Model.mode,'scalar')==1,
	fprintf('Old result is used as initial value\n')
	fprintf('Old method = %s\n', Model.method)
	A  = sum(A,1)./(sum(SY));
end

A = max(A,MINVAL);

% Original input dimension
M_ALL = M;

if isfield(Model,'ix_act')
	% Active index
	IX_act = Model.ix_act;

	X = X(IX_act,:); 	% M x T
	M = length(IX_act);
else
	IX_act = 1:M;
end

% Initial active index
M_all  = M;
A_all  = A/max(A) ;
ix_act_old = 1:M;

ix_act = find( A_all > a_min );   % effective indices
Mnew   = length(ix_act);  		% # of effective input

if Mnew < M,
    % convert to relative index
    jx_act = trans_index(ix_act,ix_act_old,M_all);

    M   = Mnew;
    A   = A(jx_act) ;  			% 1 x M
    W   = W(:,jx_act) ;  		% N x M
	X 	= X(jx_act,:);			% M x T
end

% Input variance
%XX  = (X * X')/T;   	% M x M
%YX  = (Y * X')/T;       % N x M
%YY  = sum(Y.^2,2)/T;    % N x 1
% Covariance matrix (not normalised)
YX  = (Y * X');       % N x M
YY  = sum(Y.^2,2);    % N x 1

fprintf('a_min = %g\n', a_min)
fprintf('SY0   = %g\n', SY0)
fprintf('SY    = %g\n', SY)

% Working variable
if T <= M
	XX  = [];
	Cinv= zeros(T,T);
	SW  = zeros(T,T);
else
	XX  = (X * X');   	  % M x M
	Cinv= zeros(M,M);
	SW  = zeros(M,M);
end

G_A = zeros(1,M);       % 1 x M
log_a = 0;
A_old = A;

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
MM  = zeros(Ntrain,1);
Err = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') & ~isempty(parm.Debug) & parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(M_all, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% ARD hyper variance parameter
	% A = 1/alpha

	if T < M
	    % Weight variance
	    % inv(X*X' + 1./A) = A - A * X * inv(X'*A*X + 1) * X' * A
		%  C = ( X' *A* X + eye(T) );
		XA   = repmultiply(X' , A); % T x M
	    Cinv = XA * X + eye(T);  % T x T

    	if rcond(Cinv) > MinCond,
			Cinv  = inv( Cinv );  % M x M
		else
			Cinv  = pinv( Cinv );
		end

		% Weight update
	    % inv(X*X' + 1./A) = A - A * X * Cinv * X' * A
		% W0 = YX .* A;
		% W  = W0 - (((W0 * X) * Cinv) * X') .* A;
		% W  = W0 - ((W0 * X) * Cinv) * (X'.* A);
		W  = repmultiply(YX , A);
		W  = W - ((W * X) * Cinv) * XA;
%		W  = W - ((W * X) / C ) * XA;
%		W  = W - repmultiply( ((W * X) * Cinv) * X' , A);

		%  C = ( X' *A* X + eye(T) );
		%  G_A = diag( X * inv(C) * X' *A )
		% G_A  = A .* sum(X .* (X / C), 2)';
		G_A  = A .* sum(X .* (X * Cinv), 2)';

		% Log variance
		log_sw  = log_det(Cinv) ;
		if mod(k, Nskip)==0, fprintf('- '); end
	else
		if isempty(XX)
			% covariance matrix in reduced space
			XX = X * X';
			% save original index
			IX_act = IX_act(ix_act);
			% new active index in reduced space
			ix_act = 1:M;
			M_all  = M;
			A_all  = A;
		end
    	SW  = XX + diag(1./A);

    	if rcond(SW) > MinCond,
			SW  = inv( SW );  % M x M
		else
			SW  = pinv( SW );
		end

		% Weight update
		W  = YX * SW;

		% SW  = X*X' + diag(1./A)
		% G_A = diag(inv(SW) * X*X')
		%     = 1 - diag(inv(SW)) ./A
		% G_A = diag( XX /SW );
		G_A = sum( SW .* XX ,1);
		%G_A = 1 - diag(SW)' ./A;

		log_sw  = log_det(SW) - sum(log(A));
		if mod(k, Nskip)==0, fprintf('+ '); end
	end

	WW = sum(W.^2, 1);
    % Noise variance update
%    SY = (sum(YY) - sum(sum(W.*YX)))/(N*T);
%
%    if (SY/SY0) <= MINVAL,
	    % Error
	    dY  = Y - W * X;        % N x T
	    dYY = sum(dY.^2, 2);  	% N x 1

	    SY  = (sum(dYY) + sum( WW./A ))/(N*T);
	    % Prevent zero variance
	    SY  = max( SY, MINVAL);
%	    fprintf('*')
%	end

	% Log variance
    log_sy  = N * log(SY) ;
    if Ta0 > 0,
	    log_a   = Ta0 * sum( - log(A./a0) - a0./A + 1);
	end

    % Free energy
    H(k)   =   0.5*N * (log_sw - M);
    LP(k)  = - 0.5*(T * log_sy) ;
    FE(k)  = LP(k) + H(k);
    Err(k) = (SY)./(SY0);
    MM(k)  = M;

    % Hyper parameter for weight variance (ARD)
    G_A = max((G_A), MINVAL);

	if k <= Npre_train,
		% VB update rule
		%  	N * A  = (W.^2)./SY + N * (A - A.*G_A)  ;
   		%A  = WW./SY + N * (A - A.*G_A)  ;
		if T < M
			A  = (WW./SY + N * A.*(1 - G_A) + 2*Ta0*a0)./( N + 2*Ta0 );
		else
			A  = (WW./SY + N * diag(SW)' + 2*Ta0*a0)./( N + 2*Ta0 );
		end
		%	A^2  = A .* (1./SY)' * (W.^2) ./ (G_A * N);
	else
	    % Accelerated update rule
		%	A  = (1./SY)' * (W.^2) ./ (G_A * N);
	    A  = sqrt(A.*(WW./SY)./(G_A * N));
	    %A  = ((WW./SY) + 2*Ta0*a0)./(G_A * N + 2*Ta0);
	end

    % Prune small variance
    if Prune == 1
	    ix_act_old = ix_act;

	    % Recover all component
	    switch	Prune
	    case	1
		    A_all(ix_act) = WW/max(WW);    % Prune by Weight
	    case	2
		    A_all(ix_act) = A /max(A);	   % Prune by Alpha
	    case	3
		    A_all(ix_act) = A * (1/SX);    % Prune by Alpha
	    end

	    % Find active input dimension (absolute index)
	    ix_act = find( A_all > a_min ); % effective indices
	    Mnew   = length(ix_act);  		% # of effective input

	    if Mnew < M,
		    % convert to relative index
		    jx_act = trans_index(ix_act,ix_act_old,M_all);

		    M   = Mnew;
		    A   = A(jx_act) ;  			% 1 x M
		    W   = W(:,jx_act) ;  		% N x M
			X 	= X(jx_act,:);			% M x T
			YX  = YX(:,jx_act);  	 	% N x M
			if ~isempty(XX)
				XX	= XX(jx_act,jx_act);	% M x M
			end
		end
    end

	A = max(A,MINVAL);

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, H = %g\n', ...
               k, M, Err(k), FE(k), - H(k));
    end

	if k > Ncheck && M == MM(k-1)
		Adif = max(abs(A - A_old));
	else
		Adif = 1;
	end
	if Adif < Fdiff,
		fprintf('Converged : Alpha change = %g\n',Adif)
		break;
	end;

	A_old = A;

%		Fdif = (FE(k) - FE(k-Fstep))/abs(FE(k));
%	else
%		Fdif = Fdiff + 1;
%	end
end

% convert to relative index
ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save trained variable
%  W & A is sufficient for cov-method initialization
Model.A  = A ;
Model.W  = W ;
Model.SY = SY;

Model.method = 'linear_map_sparse_cov';
Model.mode   = 'cov';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = MM(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);

author: KamitaniLab

repo: GenericObjectDecoding


text:
function D = cubic_disp_2d(Knots, volsz, spacing)
% deform image
    if isa(Knots, 'single')
        D = mex_2d_cubic_disp_float(Knots, single(volsz), single(spacing));
    elseif isa(Knots, 'double')
        D = mex_2d_cubic_disp_double(Knots, volsz, spacing);
    elseif isa(Knots, 'gpuArray')
        error('Not implemented')
    else
        error('Variables class mismatch')
    end
end

repo: pTVreg


text:
function Hilbert_EWT(ewt,t,f,sub,color)

%==================================================================
% function Hilbert_EWT(ewt,t,f,sub,color)
%
% This function display the time-frequency plane by applying
% the Hilbert transform on each EWT component.
%
% TO RUN THIS FUNCTION, YOU MUST HAVE FLANDRIN'S EMD TOOLBOX
%
% Inputs:
%   - ewt: output of the EWT transform
%   - t: the time vector corresponding to your input signal
%   - f: the input signal
%   - sub: permits to restrict the time-frequency plane to
%          frequencies from [0;pi/sub]. This value must be
%          >=1 (1 meaning the function display the entire TF-plane
%   - color: display the TF plane in grayscale if this parameter is
%            set to 0, otherwise the display is in color (useful to
%            generate figures for your papers!)
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2013
% Version: 1.0
%==================================================================

M=length(ewt);
ewtM=zeros(M,length(ewt{1}));
for i=1:M
   b=ewt{i};
   ewtM(i,:)=b';
end

if sub<1
    sub=1;
end

[Ae,fe,tte]=hhspectrum(ewtM);
[ime,tte,ffe]=toimage(Ae,fe);

disp_hhs2(ime,t,[],0,sub,color);
if color == 0
    subplot(6,1,1);plot(t,f,'black');
else
    subplot(6,1,1);plot(t,f);
end
axis tight

author: Sable

repo: mcbench-benchmarks


text:
function res = spm_eeg_specest_mtmfft(S, data, time)
% Plugin for spm_eeg_tf implementing spectral estimation using Fieldtrip's freqanalysis_mtmconvol
% FORMAT res = spm_eeg_specest_mtmfft(S, data, time)
%
% S                     - input structure
% fields of S:
%    S.taper       - taper to use ('hanning', 'rectwin', 'dpss', 'sine' or
%                    other possible inputs of 'window'
%    S.freqres     - frequency resolutions (plus-minus for each frequency, can
%                    be a vector with a value per frequency)
%    S.frequencies - vector of frequencies%
% Output:
%  res -
%   If no input is provided the plugin returns a cfg branch for itself
%
%   If input is provided:
%      res.fourier - the complex output of wavelet transform (in the case
%                    of single taper)
%      res.pow     - power (in case of multiple tapers, phase is not computed)
%      res.time    - time axis
%      res.freq    - frequency axis
%______________________________________________________________________________________
% Copyright (C) 2011-2014 Wellcome Trust Centre for Neuroimaging

% $Id: spm_eeg_specest_mtmfft.m 6021 2014-05-27 16:41:37Z vladimir $

%-This part if for creating a config branch that plugs into spm_cfg_eeg_tf
% Any parameters can be specified and they are then passed to the plugin
% when it's called.
%--------------------------------------------------------------------------
if nargin == 0
    freqres = cfg_entry;
    freqres.tag = 'freqres';
    freqres.name = 'Frequency resolution';
    freqres.strtype = 'r';
    freqres.num = [1 Inf];
    freqres.val = {2};
    freqres.help = {'Frequency resolution.',...
        'Note: 1 Hz resolution means plus-minus 1 Hz, i.e. 2 Hz badwidth',...
        'Either a single value or a vector of the same length as frequencies can be input'};

    taper = cfg_menu;
    taper.tag = 'taper';
    taper.name = 'Taper';
    taper.help = {'Save taper as well as power'};
    taper.labels = {'Hanning', 'Rectangular', 'DPSS', 'Sine'};
    taper.values = {'hanning', 'rectwin', 'dpss', 'sine'};
    taper.val = {'sine'};

    mtmfft = cfg_branch;
    mtmfft.tag = 'mtmfft';
    mtmfft.name = 'Spectrum';
    mtmfft.val = {taper, freqres};

    res = mtmfft;

    return
elseif nargin < 3
    error('Three input arguments are required');
end

%-Defaults
%--------------------------------------------------------------------------
if ~isfield(S, 'taper')
    S.taper = 'sine';
end

dt = time(end) - time(1) + diff(time(1:2));

if ~isfield(S, 'frequencies') || isempty(S.frequencies)
    S.frequencies = (1/dt):max(1/dt, floor(dt)/dt):48;
end

if ~isfield(S, 'freqres')
    S.freqres = max(1/dt, floor(dt)/dt);
end

if length(S.freqres) == 1
    freqres = S.freqres*ones(1, length(S.frequencies));
elseif length(S.freqres) == length(S.frequencies)
    freqres = S.freqres;
else
    error('Frequency resolution should be either a scalar or a vector the same length as the number of frequencies.')
end

%-Data dimensions
%--------------------------------------------------------------------------
fsample = 1./diff(time(1:2));

df = unique(diff(S.frequencies));
if length(df) == 1
    pad = ceil(dt*df)/df;
else
    pad = [];
end

[spectrum,ntaper,freqoi] = ft_specest_mtmfft(data, time, 'taper', S.taper,'freqoi', S.frequencies,...
    'tapsmofrq', freqres, 'pad', pad, 'padtype', 'mirror', 'verbose', 0);

% This in principle should not happen
if length(unique(diff(freqoi)))>1 && max(abs(diff(unique(diff(freqoi)))))>1e-3
    error('Non-uniform output frequency axis')
end

res = [];
res.freq = linspace(freqoi(1), freqoi(end), length(freqoi));
res.time = [1 1]*mean(time);

if ndims(spectrum) == 3 && size(spectrum, 1)>1
    res.pow = spm_squeeze(nanmean(spectrum.*conj(spectrum), 1), 1);
elseif ndims(spectrum) == 3
    res.fourier = spm_squeeze(spectrum, 1);
else
    res.fourier = spectrum;
end
end

function y = nansum(x, dim)

x(isnan(x)) = 0;
if nargin==1
    y = sum(x);
else
    y = sum(x,dim);
end
end

function y = nanmean(x, dim)
N = sum(~isnan(x), dim);
y = nansum(x, dim) ./ N;
end

author: spm


text:
function groupEdgeList=findEdgesInGroup(mesh,nodes)
% Create a list of edges in a given group of nodes
% See also findFacesInGroup

nVerts=length(mesh.uniqueVertices);

% This is wasteful - don't make two new sparse matrices! Just fiddle the one you've got....
% Want to eliminate entries that are not in (nodes,nodes);
diagMat=sparse(nodes,nodes,ones(length(nodes),1),nVerts,nVerts);

% make a connection matrix of just edges in this group
mesh.connectionMatrix=mesh.connectionMatrix*diagMat;
mesh.connectionMatrix=((mesh.connectionMatrix')*diagMat)';

[groupEdgeList1,groupEdgeList2]=find(triu(mesh.connectionMatrix));
groupEdgeList=[groupEdgeList1,groupEdgeList2];

groupEdgeList=sort(groupEdgeList,2); % Sort them so that the lowest numbered nodes appear first

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function rate = getFrameRate(view,scan)
%
% rate = getFrameRate(view,scan)
%
% djh, 2/21/2001
% ARW Renamed to getFrameRate 102402 to avoid conflict with BP psychToolbox
% function
% $Author: sayres $
% $Date: 2004/12/16 22:38:10 $

global dataTYPES;

rate = dataTYPES(view.curDataType).scanParams(scan).framePeriod;


text:
function trgPatch = sr_prep_target_patch(img, patchSize)

% SR_PREP_TARGET_PATCH
%
% Prepare target patches. Target patches are axis-aligned with sizes
% patchSize x patchSize.
%
% Input:
%   - img:       input image
%   - patchSize: typically 5 or 7
% Output:
%   - trgPatch:  target patches [patchSize*patchSize] x [3] x [numUvPix]

% =========================================================================

[imgH, imgW, nCh] = size(img);
numUvPix = (imgH - patchSize + 1)*(imgW - patchSize + 1);

% Initialization
trgPatch = zeros(patchSize*patchSize, 3, numUvPix, 'single');

% Get target patches using im2col
for i = 1 : nCh
    trgPatch(:,i,:) = im2col(img(:, :, i), [patchSize, patchSize], 'sliding');
end

end

repo: SelfExSR


text:
function AlignTsp2Whl(fbasename,colorvec,varargin)

% USAGE:
%     AlignTsp2Whl(fbasename,colorvec)
%
% convert .tsp file to .whl file. Requires the .meta file
%
% INPUT:
% 'fbasename': the file base names ('fbasename.tsp', etc.)
% 'colorvec': a 2 value vector [colFront colRear] which defines which
% color from [R G B] is the front and rear LEDs. Default: [1 3]
% optionnal:
%    AlignTsp2Whl(fbasename,colorvec,frommpg)
%    if frommpg is set to 1 (default 0), the tracking is done from the video
%
% Adrien Peyrache and Antal Berényi, 2012
% This particular version is corrected by Adrien Peyrache, 2012.
% It also includes 2 new things: NaN instead of -1 for interpolation and
% video tracking for bad tsp files.
% Oct 2013: Brendon Watson made somewhat robust to instances of recording system
% crashes with blank .meta files

frommpg = 0;
if ~isempty(varargin)
    frommpg = varargin{1};
end

if strcmp('.tsp',fbasename(end-3:end));
    tspfile = fbasename;
    fbasename = fbasename(1:end-4);
else
   tspfile = [fbasename '.tsp'];
end

% get timestamps and positions from tsp file
if exist(tspfile,'file')
    tspdata=load(tspfile);
else
    error('Couldn''t do anything without the tsp file')
end

if size(tspdata,2)==1 || frommpg==1
    suffix = [];
    if exist([tspfile '.old'])
        suffix = 1;
        while exist([tspfile '.old. ' num2str(suffix)])
            suffix = suffix+1;
        end
        suffix = ['.' num2str(suffix)];
    end
    eval(['!cp ' tspfile ' ' tspfile '.old' suffix]);
    RecreateTspFile(fbasename);
    tspdata=load(tspfile);
end

%get length of dat file
%infoFile = dir([fbasename '.dat']);
%filelength=infoFile.bytes/(512*2);
%clear infoFile
%get start and end timestamp of dat file
EndTimestamp = [];%set these up so can detect if problem w10389984000ith getting them from .meta
StartTimestamp = [];
ChanNum = [];
DatSize = [];

fid=fopen([fbasename '.meta']);
tline= fgetl(fid);
while ischar(tline)
    try
    if strcmp(tline(1:20),'TimeStamp of the end')
        tline=tline(59:end);
        EndTimestamp=sscanf(tline,'%d',1);
    end
    end
    try
    if strcmp(tline(1:22),'TimeStamp of the start')
        tline=tline(61:end);
        StartTimestamp=sscanf(tline,'%d',1);
    end
    end
    try
    if strcmp(tline(1:9),'Number of')
        tline=tline(31:end);
        ChanNum=sscanf(tline,'%d',1);
    end
    end
    try
    if strcmp(tline(1:9),'File size')
        tline=tline(21:end);
        DatSize=sscanf(tline,'%lu',1);
    end
    end

    tline= fgetl(fid);
end
fclose(fid);

%making up for problems reading parameters (basically if .meta isn't good,
%but will address one at a time
if isempty(EndTimestamp)
    EndTimestamp = tspdata(end,1);
end

if isempty(StartTimestamp)
    StartTimestamp = tspdata(1,1);
end

if isempty(ChanNum);
    fid=fopen([fbasename '.ini']);
    tline= fgetl(fid);
    while ischar(tline)
        try
        if strcmp(tline(1:14),'ChannelsToSave')
            tline=tline(16:end);
            ChanNum = sum(str2num(tline));
        end
        end
        tline= fgetl(fid);
    end
    fclose(fid);
end

if isempty(DatSize)
    DatSize = dir([fbasename,'.dat']);
    DatSize = DatSize.bytes;
else
    DatSize = double(DatSize);
end

%Calculate file length from dat file sample rat
DatLength=DatSize/(ChanNum*2*20); %Dat file size in ms

% TspLength=EndTimestamp-StartTimestamp;  %Unused > problematic if crashes
% yielding blank .meta files

colorvec = sort([2*colorvec-1 2*colorvec])+1;
%remove lines from tspdata which has the same timestamp as the previous
%does
repeatingts=find(tspdata(1:end-1,1)==tspdata(2:end,1))+1;
for r=size(repeatingts):-1:1
    tspdata(repeatingts(r),:)=[];
end

%Putting all bad tracked points at NaN
tspdata(tspdata==-1)=NaN;

%interpolate to 1 kHz - computer clock
t=tspdata(1,1):tspdata(end,1);
interpTsp1kHz = zeros(length(t),7);
interpTsp1kHz(:,1)=t;
warning off %Doesn't like NaN but does well
interpTsp1kHz(:,2:end)=interp1(tspdata(:,1),tspdata(:,2:7),interpTsp1kHz(:,1));
warning on

for i=1:3%length(colorvec)
    minusonesegments(:,1)=tspdata(isnan(tspdata((2:end-1),i*2)),1);
    minusonesegments(:,2)=tspdata(find(isnan(tspdata((2:end-1),i*2)))+2,1);
    for j=1:size(minusonesegments,1)
        %Replace bad values by NaN so that interpolation ignores those
        %points.
        interpTsp1kHz(minusonesegments(j,1)-interpTsp1kHz(1,1)+1:minusonesegments(j,2)-interpTsp1kHz(1,1),i*2:i*2+1)=NaN;
    end
    clear minusonesegments
end

%align the beginning
if (StartTimestamp<tspdata(1,1))
    i=1:tspdata(1,1)-StartTimestamp;
    tspnew(i,1)=StartTimestamp+i-1;
    tspnew(i,2:7)=NaN;
    tspnew=[tspnew; interpTsp1kHz];
else
    tspnew=interpTsp1kHz(StartTimestamp-tspdata(1,1)+1:end,:);
end
clear i

% Tony's version
if (EndTimestamp<tspnew(end,1))
    tspnew=tspnew(1:find(tspnew(:,1)==EndTimestamp),:);
else
    t = tspnew(end,1)+1:EndTimestamp;
    %tspnew=[tspnew; [t' repmat(NaN,[DatLength-size(tspnew,1) length(colorvec)])]];
    tspnew=[tspnew; [t' NaN(size(t,2),6)]];
end

clear t interpTsp1kHz

t = tspnew(1,1):size(tspnew,1)/DatLength:tspnew(end,1);
interpTsp = zeros(length(t),7);
interpTsp(:,1)=tspnew(1,1):size(tspnew,1)/DatLength:tspnew(end,1);
warning off
interpTsp(interpTsp==-1)=NaN;
interpTsp(:,2:end)=interp1(tspnew(:,1),tspnew(:,2:end),interpTsp(:,1));
warning on
interpTsp(isnan(interpTsp))=-1;

if 0 %Do we want a whl1k file? Personnally, no...
    fid=fopen([file '.whl1k'],'w');
    for i=1:size(interpTsp,1)
        fprintf (fid,'%i %i\n',interpTsp(i,2:3));
    end
    disp(['Samples in .dat file per channel: ' int2str(DatLength)]);
    disp(['Lines in .whl1k file:' int2str(size(interpTsp,1))]);
    fclose(fid);
end
%resample to 39.0625 Hz
whldata=interpTsp(floor(1:25.6:size(interpTsp,1)),2:end);

if 0
    figure(1),clf
    hold on
    if    sum(colorvec == 1)
        scatter(whldata(:,1),whldata(:,2),'r.')
    end
    if    sum(colorvec == 2)
        scatter(whldata(:,3),whldata(:,4),'b.')
    end
    if    sum(colorvec == 3)
        scatter(whldata(:,5),whldata(:,6),'g.')
    end
    %pause
    %keyboard
end

%dlmwrite([fbasename '.whlall'],whldata,'delimiter','\t');
dlmwrite([fbasename '.whl'],whldata(:,colorvec-1),'delimiter','\t');

end

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


text:
close all
tstart=tic;
addpath '~/project/MATLAB/PatternAnalysis/'
addpath '~/project/MATLAB/qdots/'
addpath '~/project/MATLAB/qdots/FastICA_25/'
% Nt = [100, 500, 1000, 5000];
Nt = 1000;
rs = 0.25; %resizing faction
sizevec = [0 32, 0 32];
nx = sizevec(2)-sizevec(1);
ny = sizevec(4)-sizevec(3);

center = ceil([nx, ny]/2);
d = [center; center + [1 0]];

lambda = 400; %nm
pixelsize = 106*rs; %in nm after resizing...
lambdapix = lambda/pixelsize; %pixels
NA = 1.0;
s = 1.4/(2*pi)*(lambdapix/NA); %gaussian approx of airy
% s = 1/rs*s; %for before resizing...

[X, Y] = meshgrid(-20:20);
psf = exp( -(X.^2+Y.^2)/(2*s^2) );
psf = psf / sum(psf(:));

maxphot = 100; % maximal expexted number of photons in one pixel
%  photcount = 100; %mabe made it such that sum(psf(:))=photcount...
offset = 0.01; % general offset as a fraction of maximum
meth = {'ica', 'nmf'};
path = [];
for ii = 1:4
    [dpixc, dveccr, N] = generatedata(d, sizevec, psf, maxphot, offset, Nt(ii), rs);
    n = num2str(ii);
    imstiled(dpixc(:,:,1:12));
    SaveImageFULL(['dpixc_' n], 'p')
    numOfIC = N;
    for jj = 1:2
        if jj==1
            [icasig, A, W] = fastica (dveccr, 'numOfIC', numOfIC, 'g', 'tanh');
            sica = size(A,2);
            icapix = reshape(A,nx*rs, ny*rs, sica);
        else
            [w,h]=nmf(dveccr',numOfIC,1);
            icapix=shiftdim(reshape(h,numOfIC,nx*rs,ny*rs), 1);
        end
        name = [n '_' meth{jj}];
        comp_images(icapix, dpixc, d, rs, saveon, name, path)
    end

end

author: aludnam

repo: MATLAB

save_path: github-repos/MATLAB/aludnam-MATLAB


text:
function rot = rotation(sym)

rot = sym.rot;

end

author: mtex-toolbox

repo: mtex

save_path: github-repos/MATLAB/mtex-toolbox-mtex


text:
function [p,m,n,s] = nanstats( x, d, av1, av2)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
% Averaging?
if ~exist( 'av1', 'var')
	av1 = 1;
end
if ~exist( 'av2', 'var')
	av2 = 1;
end

% Start from higher dimensions
d = sort( d, 'descend');

% Did we get the whole thing?
if isstruct( x)
	% Get each case's stats
	for i = 1:length( x.sdr)
		[p1(i,:),m1(i,:),n1(i,:),s1(i,:)] = nanstats( x.sdr{i}, d, av1, av2);
		[p2(i,:),m2(i,:),n2(i,:),s2(i,:)] = nanstats( x.sir{i}, d, av1, av2);
		[p3(i,:),m3(i,:),n3(i,:),s3(i,:)] = nanstats( x.sar{i}, d, av1, av2);
		[p4(i,:),m4(i,:),n4(i,:),s4(i,:)] = nanstats( x.sto{i}, d, av1, av2);
	end

	% Put them together
	p = [p1 p2 p3 p4];
	m = [m1 m2 m3 m4];
	n = [n1 n2 n3 n4];
	s = [s1 s2 s3 s4];

	% Reshape
	if isvector( p) && size( av1, 2) > 1 && ~isscalar( av1)
		p = reshape( p, size( av1, 2), [])';
		m = reshape( m, size( av1, 2), [])';
		n = reshape( n, size( av1, 2), [])';
		s = reshape( s, size( av1, 2), [])';
	end
	if isvector( p) && size( av2, 1) > 1
		size( p)
		size( av2)
		p = reshape( p, [], size( av2, 1))';
		m = reshape( m, [], size( av2, 1))';
		n = reshape( n, [], size( av2, 1))';
		s = reshape( s, [], size( av2, 1))';
	end
	return
end

% Init
p = x;
m = x;
n = x;
s = x;

% Get each dimension stats
for i = 1:length( d)
	p = nanmean( p, d(i));
	m = nanmax( m, [], d(i));
	n = nanmin( n, [], d(i));
	s = nanstd( s, [], d(i));
end

% Thin out
if length( size( p)) > 2
	p = (av2' * squeeze( p)' * av1)';
	m = (av2' * squeeze( m)' * av1)';
	n = (av2' * squeeze( n)' * av1)';
	s = (av2' * squeeze( s)' * av1)';
else
	p = (av1' * p * av2);
	m = (av1' * m * av2);
	n = (av1' * n * av2);
	s = (av1' * s * av2);
end
return
p = vec( p);
m = vec( m);
n = vec( n);
s = vec( s);

author: posenhuang

repo: deeplearningsourceseparation


text:
function [ region_image ] = average_feature_region(im, region_size)
% compute cell wise averages, where a cell is a region_size*region_sized
% region in the image. Input can be uint8t, single or double matrices
% of arbitrary dimension

region_area = region_size.^2;

if isa(im,'double') || isa(im,'single') || isa(im,'gpuArray')
    maxval = 1.0;
elseif isa(im,'unit8')
    maxval = 255;
end

% compute the integral image
iImage = integralVecImage(im);

% region indices
i1 = (region_size:region_size:size(im,1)) + 1;
i2 = (region_size:region_size:size(im,2)) + 1;

% sum over region, divided by number of elements, and normalize to [0,1]
% range if integer image
region_image = (iImage(i1,i2,:,:) - iImage(i1,i2-region_size,:,:) - iImage(i1-region_size,i2,:,:) + iImage(i1-region_size,i2-region_size,:,:)) ./ (region_area * maxval);

end

author: martin-danelljan

repo: ECO


text:
function t = transmission_postprocessing_guided_filter(t_in, I, varargin)
%TRANSMISSION_POSTPROCESSING_GUIDED_FILTER  Process input transmission map with
%guided filter. Parameters for the filter are set to their recommended values.

window_size = 41;
mu = 1e-3;
t = clip_to_unit_range(transmission_guided_filtering(t_in, I, window_size, mu));

end

author: sakaridis


text:
function h = label(A, B, C)
% TERNARY.LABEL label ternary phase diagram
%   TERNARY.LABEL('ALABEL', 'BLABEL', 'CLABEL') labels a ternary phase diagram created using TERNARY.PLOT
%
%   H = TERNARY.LABEL('ALABEL', 'BLABEL', 'CLABEL') returns handles to the
%   text objects created with the labels provided.  LaTeX escape codes are accepted.
%
%   See also TERNARY.PLOT

% Author: Carl Sandrock 20020827
% Modifications: FVA

% To Do

% Modifications

% Modifiers

% r(1) = text(0.5, -0.05, A, 'horizontalalignment', 'center');
% r(2) = text(1-0.45*sin(deg2rad(30)), 0.5, B, 'rotation', -60, 'horizontalalignment', 'center');
% r(3) = text(0.45*sin(deg2rad(30)), 0.5, C, 'rotation', 60, 'horizontalalignment', 'center');
r(1) = text(0.5, -0.07, A, 'horizontalalignment', 'center','Interpreter','latex');
r(2) = text(1-0.42*sin(deg2rad(30)), 0.5, B, 'rotation', -60, 'horizontalalignment', 'center','Interpreter','latex');
r(3) = text(0.40*sin(deg2rad(30)), 0.5, C, 'rotation', 60, 'horizontalalignment', 'center','Interpreter','latex');
if nargout > 0
    h = r;
end;


text:
function Speciation
% Speciation
%    using MATLAB
%
%   $Ekkehard Holzbecher  $Date: 2006/05/03 $
%--------------------------------------------------------------------------
toll = 1.e-15;                    % tolerance
nmax = 50;                        % max. no. of iterations
Se = [-1 -1 1];                   % reaction matrix
logc = [1.e-10; 1.e-10; 0];       % initial guess (log)
logK = [-0.93];                   % equilibrium constants (log)
logu = [-0.301; 0];               % total concentrations (log)

ln10 = 2.3026;
n=size(Se,1); m=size(Se,2);
S1 = Se(:,1:m-n);
S2 = Se(:,m-n+1:m);
S1star = -S2\S1;
U = [eye(m-n),S1star'];

c=exp(ln10*logc);
u(1:m-n,1) = exp(ln10*logu);
err = toll+1; nit = 0;
while (nit < nmax & err > toll),
    nit = nit+1;
    F = [U*c-u; Se*log10(c)-logK];
    DF = [U; Se*diag((1/ln10)./c)];
    dc = -DF\F;
    cn = max(c+dc,0.005*abs(c));
    err = max(abs(cn-c));
    c = cn;
    logc = log10(c);
end
display (['Species concentrations obtained after ' num2str(nit) ' iterations:']);
c
exp(ln10*logK)-c(3)/c(1)/c(2)


text:
function hoxlist = hoxsetup(numhox,hoxrange,sizeGenerations)
%
% Tor Wager, 12/29/01

if numhox ~= size(hoxrange,1), error('num hox genes does not equal number of rows of hoxrange.'),end

hoxlist = rand(numhox, sizeGenerations);

for i = 1:size(hoxlist,1)
    hoxlist(i,:) = hoxlist(i,:) * (hoxrange(i,2) - hoxrange(i,1)) + hoxrange(i,1);
end

return

author: canlab

repo: CanlabCore

save_path: github-repos/MATLAB/canlab-CanlabCore


text:
% LFCalRefine - refine calibration by minimizing point/ray reprojection error, called by LFUtilCalLensletCam
%
% Usage:
%     CalOptions = LFCalRefine( InputPath, CalOptions )
%
% This function is called by LFUtilCalLensletCam to refine an initial camera model and pose
% estimates through optimization. This follows the calibration procedure described in:
%
% D. G. Dansereau, O. Pizarro, and S. B. Williams, "Decoding, calibration and rectification for
% lenslet-based plenoptic cameras," in Computer Vision and Pattern Recognition (CVPR), IEEE
% Conference on. IEEE, Jun 2013.
%
% Minor differences from the paper: camera parameters are automatically initialized, so no prior
% knowledge of the camera's parameters are required; the free intrinsics parameters have been
% reduced by two: H(3:4,5) were previously redundant with the camera's extrinsics, and are now
% automatically centered; and the light field indices [i,j,k,l] are 1-based in this implementation,
% and not 0-based as described in the paper.
%
% Inputs:
%
%     InputPath : Path to folder containing decoded checkerboard images. Checkerboard corners must
%                 be identified prior to calling this function, by running LFCalFindCheckerCorners
%                 for example. An initial estiamte must be provided in a CalInfo file, as generated
%                 by LFCalInit. LFUtilCalLensletCam demonstrates the complete procedure.
%
%     CalOptions struct controls calibration parameters :
%                        .Phase : 'NoDistort' excludes distortion parameters from the optimization
%                                 process; for any other value, distortion parameters are included
%             .CheckerInfoFname : Name of the file containing the summarized checkerboard
%                                 information, as generated by LFCalFindCheckerCorners. Note that
%                                 this parameter is automatically set in the CalOptions struct
%                                 returned by LFCalFindCheckerCorners.
%                 .CalInfoFname : Name of the file containing an initial estimate, to be refined.
%                                 Note that this parameter is automatically set in the CalOptions
%                                 struct returned by LFCalInit.
%          .ExpectedCheckerSize : Number of checkerboard corners, as recognized by the automatic
%                                 corner detector; edge corners are not recognized, so a standard
%                                 8x8-square chess board yields 7x7 corners
%            .LensletBorderSize : Number of pixels to skip around the edges of lenslets, a low
%                                 value of 1 or 0 is generally appropriate
%                   .SaveResult : Set to false to perform a "dry run"
%        [optional]    .OptTolX : Determines when the optimization process terminates. When the
%                                 estimted parameter values change by less than this amount, the
%                                 optimization terminates. See the Matlab documentation on lsqnonlin,
%                                 option `TolX' for more information. The default value of 5e-5 is set
%                                 within the LFCalRefine function; a value of 0 means the optimization
%                                 never terminates based on this criterion.
%      [optional]    .OptTolFun : Similar to OptTolX, except this tolerance deals with the error value.
%                                 This corresponds to Matlab's lsqnonlin option `TolFun'. The default
%                                 value of 0 is set within the LFCalRefine function, and means the
%                                 optimization never terminates based on this criterion.
%
% Outputs :
%
%     CalOptions struct maintains the fields of the input CalOptions, and adds the fields:
%
%                    .LFSize : Size of the light field, in samples
%            .IJVecToOptOver : Which samples in i and j were included in the optimization
%           .IntrinsicsToOpt : Which intrinsics were optimized, these are indices into the 5x5
%                              lenslet camera intrinsic matrix
%     .DistortionParamsToOpt : Which distortion params were optimized
%     .PreviousCamIntrinsics : Previous estimate of the camera's intrinsics
%     .PreviousCamDistortion : Previous estimate of the camera's distortion parameters
%                    .NPoses : Number of poses in the dataset
%
%
% User guide: LFToolbox.pdf
% See also:  LFUtilCalLensletCam, LFCalFindCheckerCorners, LFCalInit, LFUtilDecodeLytroFolder

% Copyright (c) 2013-2020 Donald G. Dansereau

function CalOptions = LFCalRefine( InputPath, CalOptions )

%---Defaults---
CalOptions = LFDefaultField( 'CalOptions', 'OptTolX', 5e-5 );
CalOptions = LFDefaultField( 'CalOptions', 'OptTolFun', 0 );

%---Load checkerboard corners and previous cal state---
CheckerInfoFname = fullfile(InputPath, CalOptions.CheckerInfoFname);
CalInfoFname = fullfile(InputPath, CalOptions.CalInfoFname);

load(CheckerInfoFname, 'CheckerObs', 'IdealChecker', 'LFSize');
[EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CamInfo, LensletGridModel, DecodeOptions] = ...
    LFStruct2Var( LFReadMetadata(CalInfoFname), 'EstCamPosesV', 'EstCamIntrinsicsH', 'EstCamDistortionV', 'CamInfo', 'LensletGridModel', 'DecodeOptions' );
CalOptions.LFSize = LFSize;

%---Set up optimization variables---
CalOptions.IJVecToOptOver = CalOptions.LensletBorderSize+1:LFSize(1)-CalOptions.LensletBorderSize;
CalOptions.IntrinsicsToOpt = sub2ind([5,5], [1,3, 2,4, 1,3, 2,4], [1,1, 2,2, 3,3, 4,4]);

switch( lower(CalOptions.Phase) )
    case 'nodistort'
        CalOptions.DistortionParamsToOpt = [];
    otherwise
        CalOptions.DistortionParamsToOpt = 1:5;
end
if( isempty(EstCamDistortionV) && ~isempty(CalOptions.DistortionParamsToOpt) )
    EstCamDistortionV(CalOptions.DistortionParamsToOpt) = 0;
end
CalOptions.PreviousCamIntrinsics = EstCamIntrinsicsH;
CalOptions.PreviousCamDistortion = EstCamDistortionV;

fprintf('\n===Calibration refinement step, optimizing:===\n');
fprintf('    Intrinsics: ');
disp(CalOptions.IntrinsicsToOpt);
if( ~isempty(CalOptions.DistortionParamsToOpt) )
    fprintf('    Distortion: ');
    disp(CalOptions.DistortionParamsToOpt);
end

%---Compute initial error between projected and measured corner positions---
IdealChecker = [IdealChecker; ones(1,size(IdealChecker,2))]; % homogeneous coord

%---Encode params and grab info required to build Jacobian sparsity matrix---
CalOptions.NPoses = size(EstCamPosesV,1);
[Params0, ParamsInfo, JacobSensitivity] = EncodeParams( EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CalOptions );

[PtPlaneDist0,JacobPattern] = FindError( Params0, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity );
if( numel(PtPlaneDist0) == 0 )
    error('No valid grid points found -- possible grid parameter mismatch');
end

fprintf('\n    Start SSE: %g m^2, RMSE: %g m\n', sum((PtPlaneDist0).^2), sqrt(mean((PtPlaneDist0).^2)));

%---Start the optimization---
ObjectiveFunc = @(Params) FindError(Params, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity );
OptimOptions = optimset('Display','iter', ...
    'TolX', CalOptions.OptTolX, ...
    'TolFun',CalOptions.OptTolFun, ...
    'JacobPattern', JacobPattern, ...
    'PlotFcns', @optimplotfirstorderopt, ...
    'UseParallel', 'Always' );
[OptParams, ~, FinalDist] = lsqnonlin(ObjectiveFunc, Params0, [],[], OptimOptions);

%---Decode the resulting parameters and check the final error---
[EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams(OptParams, CalOptions, ParamsInfo);
fprintf(' ---Finished calibration refinement---\n');

fprintf('Estimate of camera intrinsics: \n');
disp(EstCamIntrinsicsH);
if( ~isempty( EstCamDistortionV ) )
    fprintf('Estimate of camera distortion: \n');
    disp(EstCamDistortionV);
end

ReprojectionError = struct( 'SSE', sum(FinalDist.^2), 'RMSE', sqrt(mean(FinalDist.^2)) );
fprintf('\n    Start SSE: %g m^2, RMSE: %g m\n    Finish SSE: %g m^2, RMSE: %g m\n', ...
    sum((PtPlaneDist0).^2), sqrt(mean((PtPlaneDist0).^2)), ...
    ReprojectionError.SSE, ReprojectionError.RMSE );

if( CalOptions.SaveResult )
    TimeStamp = datestr(now,'ddmmmyyyy_HHMMSS');
    GeneratedByInfo = struct('mfilename', mfilename, 'time', TimeStamp, 'VersionStr', LFToolboxVersion);

    SaveFname = fullfile(InputPath, CalOptions.CalInfoFname);
    fprintf('\nSaving to %s\n', SaveFname);

    LFWriteMetadata(SaveFname, LFVar2Struct(GeneratedByInfo, LensletGridModel, EstCamIntrinsicsH, EstCamDistortionV, EstCamPosesV, CamInfo, CalOptions, DecodeOptions, ReprojectionError));
end

end

%---------------------------------------------------------------------------------------------------
function [Params0, ParamsInfo, JacobSensitivity] = EncodeParams( EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CalOptions )
% This makes use of FlattenStruct to reversibly flatten all params into a single array.
% It also applies the same process to a sensitivity list, to facilitate building a Jacobian
% Sparisty matrix.

% The 'P' structure contains all the parameters to encode, and the 'J' structure mirrors it exactly
% with a sensitivity list. Each entry in 'J' lists those poses that are senstitive to the
% corresponding parameter. e.g. The first estimated camera pose affects only observations made
% within the first pose, and so the sensitivity list for that parameter lists only the first pose. A
% `J' value of 0 means all poses are sensitive to that variable -- as in the case of the intrinsics,
% which affect all observations.
P.EstCamPosesV = EstCamPosesV;
J.EstCamPosesV = zeros(size(EstCamPosesV));
for( i=1:CalOptions.NPoses )
    J.EstCamPosesV(i,:) = i;
end

P.IntrinParams = EstCamIntrinsicsH(CalOptions.IntrinsicsToOpt);
J.IntrinParams = zeros(size(CalOptions.IntrinsicsToOpt));

P.DistortParams = EstCamDistortionV(CalOptions.DistortionParamsToOpt);
J.DistortParams = zeros(size(CalOptions.DistortionParamsToOpt));

[Params0, ParamsInfo] = FlattenStruct(P);
JacobSensitivity = FlattenStruct(J);
end
%---------------------------------------------------------------------------------------------------
function [EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams( Params, CalOptions, ParamsInfo )
P = UnflattenStruct(Params, ParamsInfo);
EstCamPosesV = P.EstCamPosesV;

EstCamIntrinsicsH = CalOptions.PreviousCamIntrinsics;
EstCamIntrinsicsH(CalOptions.IntrinsicsToOpt) = P.IntrinParams;

EstCamDistortionV = CalOptions.PreviousCamDistortion;
EstCamDistortionV(CalOptions.DistortionParamsToOpt) = P.DistortParams;

EstCamIntrinsicsH = LFRecenterIntrinsics(EstCamIntrinsicsH, CalOptions.LFSize);
end

%---------------------------------------------------------------------------------------------------
function [Params, ParamInfo] = FlattenStruct(P)
Params = [];
ParamInfo.FieldNames = fieldnames(P);
for( i=1:length( ParamInfo.FieldNames ) )
    CurFieldName = ParamInfo.FieldNames{i};
    CurField = P.(CurFieldName);
    ParamInfo.SizeInfo{i} = size(CurField);
    Params = [Params; CurField(:)];
end
end
%---------------------------------------------------------------------------------------------------
function [P] = UnflattenStruct(Params, ParamInfo)
CurIdx = 1;
for( i=1:length( ParamInfo.FieldNames ) )
    CurFieldName = ParamInfo.FieldNames{i};
    CurSize = ParamInfo.SizeInfo{i};
    CurField = Params(CurIdx + (0:prod(CurSize)-1));
    CurIdx = CurIdx + prod(CurSize);
    CurField = reshape(CurField, CurSize);
    P.(CurFieldName) = CurField;
end
end

%---------------------------------------------------------------------------------------------------
function [PtPlaneDists, JacobPattern] = FindError(Params, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity )
    %---Decode optim params---
    [EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams(Params, CalOptions, ParamsInfo);

    %---Tally up the total number of observations---
    TotCornerObs = size( [CheckerObs{:,CalOptions.IJVecToOptOver,CalOptions.IJVecToOptOver}], 2 );
    CheckCornerObs = 0;

    %---Preallocate JacobPattern if it's requested---
    if( nargout >= 2 )
        JacobPattern = zeros(TotCornerObs, length(Params));
    end

    %---Preallocate point-plane distances---
    PtPlaneDists = zeros(1, TotCornerObs);

    %---Compute point-plane distances---
    OutputIdx = 0;
    for( PoseIdx = 1:CalOptions.NPoses )
        %---Convert the pertinent camera pose to a homogeneous transform---
        CurEstCamPoseV = squeeze(EstCamPosesV(PoseIdx, :));
        CurEstCamPoseH = eye(4);
        CurEstCamPoseH(1:3,1:3) = rodrigues(CurEstCamPoseV(4:6));
        CurEstCamPoseH(1:3,4) = CurEstCamPoseV(1:3);

        %---Iterate through the corners---
        for( TIdx = CalOptions.IJVecToOptOver )
            for( SIdx = CalOptions.IJVecToOptOver )
                CurCheckerObs = CheckerObs{PoseIdx, TIdx,SIdx};
                NCornerObs = size(CurCheckerObs,2);
                if( NCornerObs ~= prod(CalOptions.ExpectedCheckerSize) )
                    continue; % this implementation skips incomplete observations
                end
                CheckCornerObs = CheckCornerObs + NCornerObs;

                %---Assemble observed corner positions into complete 4D [i,j,k,l] indices---
                CurCheckerObs_Idx = [repmat([SIdx;TIdx], 1, NCornerObs); CurCheckerObs; ones(1, NCornerObs)];

                %---Transform ideal 3D corner coords into camera's reference frame---
                IdealChecker_CamFrame = CurEstCamPoseH * IdealChecker;
                IdealChecker_CamFrame = IdealChecker_CamFrame(1:3,:); % won't be needing homogeneous points

                %---Project observed corner indices to [s,t,u,v] rays---
                CurCheckerObs_Ray = EstCamIntrinsicsH * CurCheckerObs_Idx;

                %---Apply direction-dependent distortion model---
                if( ~isempty(EstCamDistortionV) && any(EstCamDistortionV(:)~=0))
                    k1 = EstCamDistortionV(1);
                    k2 = EstCamDistortionV(2);
                    k3 = EstCamDistortionV(3);
                    b1dir = EstCamDistortionV(4);
                    b2dir = EstCamDistortionV(5);
                    Direction = CurCheckerObs_Ray(3:4,:);
                    Direction = bsxfun(@minus, Direction, [b1dir;b2dir]);
                    DirectionR2 = sum(Direction.^2);
                    Direction = Direction .* repmat((1 + k1.*DirectionR2 + k2.*DirectionR2.^2 + k3.*DirectionR2.^3),2,1);
                    Direction = bsxfun(@plus, Direction, [b1dir;b2dir]);
                    CurCheckerObs_Ray(3:4,:) = Direction;
                end

                %---Find 3D point-ray distance---
                STPlaneIntersect = [CurCheckerObs_Ray(1:2,:); zeros(1,NCornerObs)];
				% Here interpret u,v as relative, at a distance of 1 m
				% Thus we use a relative 2pp, with D = 1m.
                RayDir = [CurCheckerObs_Ray(3:4,:); ones(1,NCornerObs)];
                CurDist3D = LFFind3DPtRayDist( STPlaneIntersect, RayDir, IdealChecker_CamFrame );

                PtPlaneDists(OutputIdx + (1:NCornerObs)) = CurDist3D;

                if( nargout >=2 )
                    % Build the Jacobian pattern. First we enumerate those observations related to
                    % the current pose, then find all parameters to which those observations are
                    % sensitive. This relies on the JacobSensitivity list constructed by the
                    % FlattenStruct function.
                    CurObservationList = OutputIdx + (1:NCornerObs);
                    CurSensitivityList = (JacobSensitivity==PoseIdx | JacobSensitivity==0);
                    JacobPattern(CurObservationList, CurSensitivityList) = 1;
                end
                OutputIdx = OutputIdx + NCornerObs;
            end
        end
    end

    %---Check that the expected number of observations have gone by---
    if( CheckCornerObs ~= TotCornerObs )
        error(['Mismatch between expected (%d) and observed (%d) number of corners' ...
            ' -- possibly caused by a grid parameter mismatch'], TotCornerObs, CheckCornerObs);
    end
end

%---Compute distances from 3D rays to a 3D points---
function [Dist] = LFFind3DPtRayDist( PtOnRay, RayDir, Pt3D )

RayDir = RayDir ./ repmat(sqrt(sum(RayDir.^2)), 3,1); % normalize ray
Pt3D = Pt3D - PtOnRay;    % Vector to point

PD1 = dot(Pt3D, RayDir);
PD1 = repmat(PD1,3,1).*RayDir; % Project point vec onto ray vec

Pt3D = Pt3D - PD1;
Dist = sqrt(sum(Pt3D.^2, 1)); % Distance from point to projected point

end

repo: LFToolbox


text:
function [Xmat,pobjround,info] = local_search_ape_v1(Zmat,C,rrPar,opt,roundonly)
%% local search for absolute pose estimation
%% used as a subroutine for STRIDE
%% Heng Yang, June 28, 2021

if nargin < 5
    roundonly = false;
end
if nargin < 4
    % default round the first two eigenvectors
    opt = [1,2];
end

tBound        = rrPar.translationBound;
dBound        = rrPar.depthBound;
blk           = rrPar.blk;
FOV           = rrPar.FOV;

if roundonly
    [R,t,theta] = round_ape_v1(Zmat,tBound,dBound,[1]);
    xtld        = lift_ape_v1(R(:),t,theta,tBound,dBound,FOV);
    Xmat        = {xtld{1}*xtld{1}';xtld{2}*xtld{2}';xtld{3}*xtld{3}';xtld{4}*xtld{4}'};
    pobjround   = blktrace(blk,Xmat,C);
else
    [R,t,theta] = round_ape_v1(Zmat,tBound,dBound,opt);
    Zmatround   = {};
    pobjround   = zeros(length(opt),1);
    for i = 1:length(opt)
        Ri              = squeeze(R(:,:,i));
        ti              = squeeze(t(:,i));
        thetai          = squeeze(theta(:,i));
        [out,pobjtmp]   = nlp_ape(C,rrPar,Ri,ti,thetai);
        Zmatround{end+1}   = out.X;
        pobjround(i)       = pobjtmp;
    end
    pobjs            = pobjround;
    [pobjround,idx]  = min(pobjround);
    if pobjround == inf
        fprintf('        NLP fails to find a good solution, return a rounded solution only.\n');
        Ropt         = squeeze(R(:,:,1));
        ropt         = Ropt(:);
        topt         = t(:,1);
        thetaopt     = theta(:,1);
        xopt         = lift_ape_v1(ropt,topt,thetaopt,tBound,dBound,FOV);
        Xmat         = {xopt{1} * xopt{1}';xopt{2} * xopt{2}';xopt{3}*xopt{3}';xopt{4}*xopt{4}'};
        pobjround    = blktrace(blk,Xmat,C);
        nlpsuccess   = false;
    else
        Xmat = Zmatround{idx};
        nlpsuccess   = true;
    end
end

if nargout > 2
    info.minidx     = idx;
    info.nlpsuccess = nlpsuccess;
    info.pobjs      = pobjs;
    info.diffpobj   = pobjs(1) - pobjround;
end

end

author: MIT-SPARK

repo: CertifiablyRobustPerception


text:
function [Results, OPTIONS] = bst_inverse_linear_2016(HeadModel,OPTIONS)
% BST_INVERSE_LINEAR_2016: Compute an inverse solution (minimum norm, dipole fitting or beamformer)
% USAGE:  [Results,OPTIONS] = bst_inverse_linear_2016(HeadModel, OPTIONS) : Compute mininum operator
%                   OPTIONS = bst_inverse_linear_2016()                   : Return default options
%
% DESCRIPTION:
%     This program computes the whitened and weighted minimum-norm operator,
%     (the wMNE imaging kernel), which is used to compute whitened
%     and weighted minimum-norm estimates (MNE).
%         (e.g., J=wMNEoperator*B; where B is the unwhitened data).
%     It can also compute the whitened and noise-normalized dynamic
%     statistical parametric mapping (dSPM) inverse operator, and/or the
%     whitened standardized low resolution brain electromagnetic tomography
%     (sLORETA) inverse operator, which are used to compute whitened source
%     activity dSPM and sLORETA maps.
%         (e.g., S_dSPM=dSPMoperator*B; where B is the unwhitened data).
%         (e.g., S_sLORETA=sLORETAoperator*B; where B is the unwhitened data).
%
%     The function was originally written with the goal of providing some of the same
%     functionality of the MNE software written by Matti Hamalainen, but no
%     guarantees are made that it performs all computations in the same
%     exact way. It also provides some functionalities not available in the
%     MNE software.
%
%     In March 2015, this function was completely overhauled by John
%     Mosher to create dipole fitting, beamforming, and min norm images all
%     in the same imaging kernel framework. After the imaging kernel is
%     generated, subsequent Process Operations can be applied to further
%     interpret the results, such as finding the optimal dipole in each
%     time slice.
%
% INPUTS:
%    - HeadModel: Array of Brainstorm head model structures
%         |- Gain       : Forward field matrix for all the channels (unconstrained source orientations)
%         |- GridOrient : Dipole orientation matrix
%    - OPTIONS: structure
%         |- NoiseCovMat        : Noise covariance structure
%         |   |- NoiseCov       : Noise covariance matrix
%         |   |- FourthMoment   : Fourth moment (F^2 * F^2'/n)
%         |   |- nSamples       : Number of times samples used to compute those measures
%         |- DataCovMat         : Data covariance structure
%         |   |- NoiseCov       : Data covariance matrix  (F*F'/n)
%         |   |- FourthMoment   : Fourth moment (F^2 * F^2'/n)
%         |   |- nSamples       : Number of times samples used to compute those measures
%         |- ChannelTypes   : Type of each channel (for each row of the Leadfield and the NoiseCov matrix)
%         |- InverseMethod  : {'minnorm', 'gls', 'lcmv'}
%         |- InverseMeasure :    | minnorm: {'amplitude',  'dspm', 'sloreta'}
%         |                      |     gls: {'performance'}
%         |                      |    lcmv: {'performance'}
%         |- SourceOrient   : String or a cell array of strings specifying the type of orientation constraints for each HeadModel (default: 'fixed')
%         |- Loose          :    | Value that weights the source variances of the dipole components defining the tangent space of the cortical surfaces (default: 0.2).
%         |- UseDepth       : Flag to do depth weighting (default: 1).
%         |- WeightExp      :    | Order of the depth weighting. {0=no, 1=full normalization, default=0.8}
%         |- WeightLimit    :    | Maximal amount depth weighting (default: 10).
%         |- NoiseMethod    : {'shrink', 'median', 'reg', 'diag', 'none'}
%         |- NoiseReg       :    | NoiseMethod='reg' : Amount of regularization
%         |- SnrMethod      : {'rms', 'fixed'}
%         |- SnrRms         :    | SnrMethod='rms'   : RMS source amplitude, in nAm  (Default=1000)
%         |- SnrFixed       :    | SnrMethod='fixed' : Fixed SNR value (Default=3)
%
% OUTPUTS:
%    - Results : Source structure
%       NEW in 2015: if GridOrient is returned empty, the orientation is assumed to be
%       the original grid orientation (usually normal to the surface, or unconstrained in a volume). If
%       it is returned here, then we are returning an optimized orientation
%       that overrides the original orientation.
%    - OPTIONS : Return the modified options

% NOTES (mostly not updated for 2015, see notes below):
%     - More leadfield matrices can be used: the solution will combine all
%       leadfield matrices appropriately.
%
%     - This leadfield structure allows to combine surface and volume
%       source spaces with and without dipole orientation constraints,and
%       with and without area or volumetric current density computations.
%       If using a single sphere headmodel, the silent radial
%       component could be eliminated using the SVD (e.g., use
%       bst_remove_silent.m).
%       NOTE by JCM 2015: Under what considerations would we mix different
%       source spaces? I don't understand combining, for example,
%       cortically constrained surface dipoles with unconstrained volume
%       dipoles. In my 2015 version, I anticipate the user has picked a
%       consistent source modeling space, e.g. unconstrained volume or
%       fixed orientation surface, as dictated by the head model.
%
%
%     - NoisCov: This should be computed from the pre-stimulus period for
%       averaged ERF data (e.g., using MNE), or from an empty room recording
%       for unaveraged spontaneous or induced data.
%
%     - Orientation constrains for dipoles (.SourceOrient field)
%         - "fixed"     : Dipoles constrained to point normal to cortical surfaces
%         - "free"      : No constraints, dipoles point in x,y,z directions
%         - "loose"     : Source variances of dipole components pointing
%                         tangentially to the cortical surfaces are
%                         multipled by OPTIONS.Loose
%         - "optimal"   : NEW IN 2015: Optimal orientation is returned
%                         in GridOrients for each dipole
%       => OBSOLETE IN 2015: For dealing with multiple source spaces with different types of orientation constraints use for example,
%          OPTION.SourceOrient{1}='fixed';
%          OPTION.SourceOrient{2}='free';
%          This has to correspond with the HeadModel Structure
%
%     - sLORETA: TODO: August 2016, Output values are multiplied by 1e12 for display in
%                Brainstorm (time series and cortical maps). There is a
%                discrepancy in the Pascual-Marqui publications regarding
%                resolution kernels vs data covariance that needs
%                addressing by a sLORETA expert. For now, we leave it as
%                its legacy version since 2011.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors:  Rey Rene Ramirez, Ph.D, 2010-2012
%           Francois Tadel, 2010-2016
%           John Mosher, 2014-2016

%% ===== DEFINE DEFAULT OPTIONS =====
Def_OPTIONS.NoiseCovMat    = [];
Def_OPTIONS.DataCovMat     = [];
Def_OPTIONS.ChannelTypes   = {};
Def_OPTIONS.InverseMethod  = 'minnorm';
Def_OPTIONS.InverseMeasure = 'amplitude';
Def_OPTIONS.SourceOrient   = {'fixed'};
Def_OPTIONS.Loose          = 0.2;
Def_OPTIONS.UseDepth       = 1;
Def_OPTIONS.WeightExp      = 0.5;
Def_OPTIONS.WeightLimit    = 10;
Def_OPTIONS.NoiseMethod    = 'reg';
Def_OPTIONS.NoiseReg       = .1;
Def_OPTIONS.SnrMethod      = 'fixed';
Def_OPTIONS.SnrRms         = 1000;
Def_OPTIONS.SnrFixed       = 3;
Def_OPTIONS.FunctionName   = [];
% Return the default options
if (nargin == 0)
    Results = Def_OPTIONS;
    return
end

% Make the default for all the leadfields
% JCM: Why are there multiple headmodels being passed to this function?
numL = size(HeadModel,2);

%TODO: Understand this:
if numL > 1,
    error('BST_INVERSE > Mosher does not understand why we are passing multiple headmodels')
    % This is for supporting mixed head models:
    % https://neuroimage.usc.edu/brainstorm/News#Mixed_head_models
    % @JCM: You need to add this support back in your function (see bst_wmne.m for an example).
end

Def_OPTIONS.SourceOrient = repmat(Def_OPTIONS.SourceOrient, [1 numL]);

% Copy default options to OPTIONS structure (do not replace defined values)
OPTIONS = struct_copy_fields(OPTIONS, Def_OPTIONS, 0);

%% ===== CHECK FOR INCONSISTENT VALUES =====
if (OPTIONS.UseDepth && strcmpi(OPTIONS.InverseMeasure, 'sloreta'))
    disp('BST_INVERSE > Depth weighting is not necessary when using sLORETA normalization, ignoring option UseDepth=1');
    OPTIONS.UseDepth = 0;
elseif (OPTIONS.UseDepth && ~strcmpi(OPTIONS.InverseMethod, 'minnorm'))
    disp('BST_INVERSE > Depth weighting is only available for minnorm, ignoring option UseDepth=1');
    OPTIONS.UseDepth = 0;
end
if (numL ~= length(OPTIONS.SourceOrient))
    error('BST_INVERSE > The number of elements in the HeadModel structure should equal the length of the cell array OPTIONS.SourceOrient.')
end
if ~isempty(OPTIONS.Loose) && (OPTIONS.Loose>=1 || OPTIONS.Loose<=0)
    error('BST_INVERSE > Loose value should be smaller than 1 and bigger than 0, or empty for no loose orientations.')
end
if (OPTIONS.WeightExp > 1) || (OPTIONS.WeightExp < 0)
    error('BST_INVERSE > WeightExp should be a scalar between 0 and 1')
end

if (OPTIONS.NoiseReg > 1) || (OPTIONS.NoiseReg < 0)
    error('BST_INVERSE > NoiseReg should be a scalar between 0 and 1')
end

%% ===== NOISE COVARIANCE REGULARIZATION =====

% Convenient notation
C_noise = OPTIONS.NoiseCovMat.NoiseCov; % all of the channels requested
Var_noise = diag(C_noise); % Diagonal vector of the noise variances per channel
nChannels = length(Var_noise);

% JCM: March 2015, probably don't need this, but will retain
% Detect if the input noise covariance matrix is or should be diagonal
if (norm(C_noise,'fro') - norm(Var_noise,'fro')) < eps(single(norm(Var_noise,'fro'))),
    % no difference between the full matrix and the diagonal matrix
    disp(['BST_INVERSE > Detected diagonal noise covariance, ignoring option NoiseMethod="' OPTIONS.NoiseMethod '".']);
    OPTIONS.NoiseMethod = 'diag';
end

% Commentary April 2015: There is a tendency to apply generic scale values to the
% channels to balance them with regards to units, such as Volts and Tesla.
% But we also have a problem with gradiometer channels vs magnetometers. So
% the "natural" way is to use the channel variances themselves to balance
% out the differences between modalities. But we don't want to do each
% channel, since a dead channel has (near) zero variance. So instead we
% calculate a common variance for each modality, to get us in the ball
% park. So we initially treat each modality as Independent and Identically
% Distributed (IID) and pre-whiten by this to bring the modalities into
% closer alignment.

% Because the units can be different, we need to first balance the
% different types of arrays. How many unique array types do we have?

% Updated Commentary August 2016, by John Mosher: The issue of balancing is
% primarily a problem of multi-modal statistics, i.e., across multiple
% channel types. Ideally, we should allow for possible cross-covariances
% between the modalities to assist in the overal spatial correlation of the
% array. The problem is that we simultaneously need to "regularize" the
% covariance matrices to catch bad channels and experimental dependencies
% that creep into measurements, while also calculating cross-dependencies
% between modalities of disparate units.
%
% We have tried numerous "black-box" methods to get, for example, Neuromag
% magnetometers and gradiometers combined in the same covariance matrix.
% Unfortunately, each situation can be unique, such as in a clinical
% setting, where the magnetometers can span an enormous dynamic range,
% while gradiometers span a much tighter range, and these ranges overlap.
%
% In consultation with co-investigator Matti Hamalainen and MNE, we opted
% for now to keep the use of the multimodal calculation simpler by
% eliminating the cross modality terms, then regularizing within each
% modality. This has been the default approach to multimodal noise
% covariance calculations in Brainstorm since 2011, and we retain that
% here.

% So how many channel types do we have:
Unique_ChannelTypes = unique(OPTIONS.ChannelTypes);

% Initialize the modality whitening matrices:
% iPrior_Matrix = eye(nChannels);

% Retain the below code in comments if only to discuss what does and does
% not work in trying to capture cross-modalities. August 2016

%     % calculate the average variance per channel as a prior
%     Prior_IID = zeros(length(Unique_ChannelTypes),1);
%     Prior_Vector = zeros(nChannels,1); % initialize as a vector
%
%     for i = 1:length(Unique_ChannelTypes),
%         ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
%         Prior_IID(i) = mean(Var_noise(ndx)); % mean of this type
%         % let's get a bit more sophisticated on calculating this IID value
%         % We wouldn't want a few extreme values distorting our mean
%         % How many channels are there:
%         len_ndx = length(ndx);
%         % let's toss out the upper and lower values
%         ndx_clip = round(len_ndx/10); % 10%
%         Variances_This_Modality = sort(Var_noise(ndx));
%         % trim to central values
%         Variances_This_Modality = Variances_This_Modality((ndx_clip+1):(end-ndx_clip));
%         Prior_IID(i) = median(Variances_This_Modality); % mean of the middle values
%         Prior_Vector(ndx) = sqrt(Prior_IID(i)); % map to this part of the array
%     end
%
%     %TODO: Test for bizarre case of Prior_IID too small
%
%     % build whitener to balance out the different types of channels
%     iPrior_Matrix = spdiags(1./Prior_Vector,0,nChannels,nChannels);
%
%     % Now we can use this iPrior_Matrix to "pre-whiten" the noise covariance
%
%     % Block Whitened noise covariance
%     Cw_noise = iPrior_Matrix * C_noise * iPrior_Matrix;
%
%     % Now the units imbalance between different subarrays is theoretically
%     % balanced.

% Now remove the off_diagonal components
Cw_noise = zeros(size(C_noise)); % initialize

for i = 1:length(Unique_ChannelTypes),
    % for each unique modality
    ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
    Cw_noise(ndx,ndx) = C_noise(ndx,ndx);
end
% the off diagonal components corresponding to cross-modality
% covariances have been removed

% By continuing to call it "Cw_noise", the below codes can continue as
% before.

% Before any additional regularizations that may interfere

% Make sure the noise covariance matrix is strictly symmetric
% (the previous operations may cause rounding errors that make the matrix not exactly symmetrical)
Cw_noise = (Cw_noise + Cw_noise')/2;

% So the block whitened noise covariance matrix is Cw_noise.

% If the modalities were truly balanced, we could proceed with
% regularization in one fell swoop; however, the reality is that
% regularization should be applied modality by modality, with the cross
% covariance of the modalities zeroed out.

% initialize inverse whitener for noise covariance matrix
iWw_noise = zeros(size(Cw_noise));

FourthMoment = []; % initialize
nSamples = [];
if strcmpi(OPTIONS.NoiseMethod, 'shrink')
   % Has the user calculated the Fourth Order moments?
   if ~isfield(OPTIONS.NoiseCovMat,'FourthMoment'),
      error('BST_INVERSE > For Method ''shrink'' please recalculate Noise Covariance, to include Fourth Order Moments');
   else
      FourthMoment = OPTIONS.NoiseCovMat.FourthMoment;
   end

   % How many samples used to calculate covariance
   if isempty(OPTIONS.NoiseCovMat.nSamples),
      error('BST_INVERSE > No noise samples found. For Method ''shrink'' please recalculate Noise Covariance from actual data samples.');
   else
      nSamples = OPTIONS.NoiseCovMat.nSamples(1);
      % FIX: What if different lengths are used, but only an issue at this
      % point for "shrink" method
   end
end

for i = 1:length(Unique_ChannelTypes),
   % each modality
    ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones

    % regularize and form whitener for each modality, using local
    % subfunction
    [Cw_noise(ndx,ndx),iWw_noise(ndx,ndx)] = ...
        truncate_and_regularize_covariance(Cw_noise(ndx,ndx),...
        OPTIONS.NoiseMethod,Unique_ChannelTypes{i},...
        OPTIONS.NoiseReg,FourthMoment,nSamples);
end

% So now we have calculated the noise covariance matrix across all
% modalities, but have eliminated the cross covariances between modalities.
% Within each modality, we have checked for rank deficiencies and have
% added possible regularization within the modality.

% so Ww * Ww' = Cw, and iWw*iWw' = pinv(Cw)

% so the entire "whitening" process is to first apply the modality
% whitener, then apply the unitless whitener.
% iWw_noise * iPrior_Matrix * d;

% For convenience in the later modeling, let's combine into one whitener

iW_noise = iWw_noise; % * iPrior_Matrix; % August 2016, iPrior is simpy I

%% ======== Data Covariance Manipulation, if there is one =============

% August 2016, we treat Data Covariances in exactly the same manner as
% Noise Covariance matrices. To be done is to explore whitening methods of
% data covariance matrices, such as would be needed in MUSIC, but not there
% yet, part of future work.

% We only calculate the data covariance matrix for LCMV

if isempty(OPTIONS.DataCovMat) || ~strcmpi(OPTIONS.InverseMethod, 'lcmv')  % user may not have calculated

    iW_data = [];

else
    % Regularize and invert the data covariance matrix, same as the noise
    % covariance matrix.

    % Convenient notation
    C_data = OPTIONS.DataCovMat.NoiseCov; % all of the channels requested
    Var_data = diag(C_data); % Diagonal vector of the noise variances per channel

    % quick error check
    if nChannels ~= length(Var_data),
        error('BST_INVERSE > Data covariance is not the same size as noise covariance, something is wrong')
    end

    % Zero out any cross covariance between modalities.
    Cw_data =zeros(size(C_data));

    for i = 1:length(Unique_ChannelTypes),
        % for each unique modality
        ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones
        Cw_data(ndx,ndx) = C_data(ndx,ndx);
    end

    % ensure symmetry

    Cw_data = (Cw_data + Cw_data')/2;

    % So the block whitened noise covariance matrix is Cw_noise.

    % If the modalities were truly balanced, we could proceed with
    % regularization in one fell swoop; however, the reality is that
    % regularization should be applied modality by modality, with the cross
    % covariance of the modalities zeroed out.

    % initialize inverse whitener for noise covariance matrix
    iWw_data = zeros(size(Cw_data));

    FourthMoment = []; % initialize
    nSamples = [];
    if strcmpi(OPTIONS.NoiseMethod, 'shrink')
       % Has the user calculated the Fourth Order moments?
       if ~isfield(OPTIONS.DataCovMat,'FourthMoment'),
          error('BST_INVERSE > For Method ''shrink'' please recalculate Data Covariance, to include Fourth Order Moments');
       else
          FourthMoment = OPTIONS.DataCovMat.FourthMoment;
       end

       % How many samples used to calculate covariance
       if isempty(OPTIONS.DataCovMat.nSamples),
          error('BST_INVERSE > No data samples found. For Method ''shrink'' please recalculate Data Covariance from actual data samples.');
       else
          nSamples = OPTIONS.DataCovMat.nSamples(1);
          % FIX: What if different lengths are used, but only an issue at this
          % point for "shrink" method
       end
    end % for method "shrink"

    for i = 1:length(Unique_ChannelTypes),
       % each modality
       ndx = find(strcmpi(Unique_ChannelTypes(i),OPTIONS.ChannelTypes)); % which ones

        % regularize and form whitener for each modality, using local
        % subfunction. Note DataCov still used NoiseMethod and NoiseReg to
        % pass parameters.

        [Cw_data(ndx,ndx),iWw_data(ndx,ndx)] = ...
            truncate_and_regularize_covariance(Cw_data(ndx,ndx),...
            OPTIONS.NoiseMethod,Unique_ChannelTypes{i},...
            OPTIONS.NoiseReg,FourthMoment,nSamples);
    end

    iW_data = iWw_data; % * iPrior_Matrix; % August 2016, iPrior is simply I

    % =============  MAJOR PROGRAMMING SIMPLICITY HERE ==============
    iW_noise_true = iW_noise; % save the true noise whitener
    iW_noise = iW_data; % we replace the noise whitener with the data whitener in the below equations.

end % if there is a data covariance matrix

%% ===== Source Model Assumptions ===============
%
% Calculated separately from the data and noise covariance considerations
% number of sources:
NumDipoles = size(HeadModel.GridLoc,1); % number of dipoles (source points)
% (insensitive to the number of components per dipole)

% orientation of each source:
% Each current dipole has an explicit or implied covariance matrix for its
% moment, C_q. Let W_q be the matrix square root, C_q = W_q * W_q'.

% If the HeadModelType is "surface" then there is an orientation available,
% if it is "volume" then an orientation is not pre-defined in
% HeadModel.GridOrient.

Wq = cell(1,NumDipoles); % source unit orientations

% Optional Depth Weighting Scalar (used particularly in Min Norms)
% Calculate Depth Weighting Scalar
Alpha = ones(1,NumDipoles); % initialize to unity

if OPTIONS.UseDepth
    % See eq. 6.2.10 of MNE Manual version 2.7 (Mar 2010).
    % Original code had some backflips to check for instabilities.
    % Here we take a simpler direct approach.

    % We are assuming unconstrained (three source directions) per source
    % point. We form the unconstrained norm of each point
    ColNorms2 = sum(HeadModel.Gain .* HeadModel.Gain); % square of each column
    SourceNorms2 = sum(reshape(ColNorms2,3,[]),1); % Frobenius norm squared of each source

    % Now Calculate the *non-inverted* value
    Alpha2 = SourceNorms2 .^ OPTIONS.WeightExp; % note not negative weight (yet)
    AlphaMax2 = max(Alpha2); % largest squared source norm
    % The limit is meant to keep the smallest from getting to small
    Alpha2 = max(Alpha2, AlphaMax2 ./ (OPTIONS.WeightLimit^2)); % sets lower bound on source strengths

    % Now invert it
    Alpha2 = AlphaMax2 ./ Alpha2; % goes from WeightLimit^2 to 1, depending on the inverse strength of the source

    Alpha = sqrt(Alpha2);
    % Thus deep weak sources can be amplified up to WeightLimit times greater
    % strength, relative to the stronger shallower sources.
end

% So, at this point, we have an orientation matrix defined for every source
% point, and we have a possible weighting scalar to apply to it. Each
% source has (as yet unknown) source variance of sigma2. The total source
% covariance we are modeling is
% Cq = Alpha2 * sigma2 * Wq * Wq'; % where sigma2 is unknown and to be
% estimated from the data.

% So we now define Wq to include the optional alpha weighting, as
% calculated above

% So if the headmodel is "volume", user should not have been able to select
% "fixed" (normal to cortex), nor "loose". Francois has controlled this
% through the GUI interface. Check anyway

if strcmpi('volume', HeadModel.HeadModelType)
    switch OPTIONS.SourceOrient{1}
        case 'free'
            % do nothing, okay
        otherwise
            error('BST SOURCE ERROR: HeadModel is a volume with no defined orientation. Change Orientation from %s to ''unconstrained''.\n',OPTIONS.SourceOrient{1})
    end
end

% Initialize each source orientation

fprintf('BST_INVERSE > Using ''%s'' surface orientations\n',OPTIONS.SourceOrient{1})

for i = 1:NumDipoles

    switch OPTIONS.SourceOrient{1}
        case 'fixed'
            % fprintf('BST_INVERSE > Using constrained surface orientations\n');
            NumDipoleComponents = 1;
            tmp = HeadModel.GridOrient(i,:)'; % 3 x 1
            Wq{i} = tmp/norm(tmp); % ensure unity

        case 'loose'
            % fprintf('BST_INVERSE > Using loose surface orientations\n');
            NumDipoleComponents = 3;
            tmp = HeadModel.GridOrient(i,:)'; % preferred direction
            tmp = tmp/norm(tmp); % ensure unity
            tmp_perp = null(tmp'); % orientations perpedicular to preferred
            Wq{i} = [tmp tmp_perp*OPTIONS.Loose]; % weaken the other directions

        case 'free'
            % fprintf('BST_INVERSE > Using unconstrained orientations\n');
            NumDipoleComponents = 3;
            Wq{i} = eye(NumDipoleComponents);

        otherwise
            error('BST_INVERSE > Unknown Source Orientation')
    end

    % L2norm of Wq in everycase above is 1, (even for loose, since L2 norm
    % of matrix is largest singular value of the matrix).

    Wq{i} = Alpha(i)*Wq{i}; % now weight by desired amplifier

    % So L2 norm of Wq is equal to the desired amplifier (if any).
end

%% ===== PROCESSING LEAD FIELD MATRICES, WEIGHTS & ORIENTATIONS =====

% put all covariance priors into one big sparese matrix
WQ = blkdiag(sparse(Wq{1}),Wq{2:end});
% (by making first element sparse, we force Matlab to use efficient sparse
% mex function)

% With the above defined, then the whitened lead field matrix is simply

L = iW_noise * (HeadModel.Gain * WQ);  % if LCMV, this is data whitened.

% we note that the number of columns of the ith source is found as the number of
% corresponding columns in WQ{i}. In this version (March 2016), we still
% assume equal numbers of columns (1 or 3). The Mixed Head Model may
% contain mixed quantities.
%

% The model at this point is d = {A}{x} + n, where d and n are whitened,
% and x has a covariance prior of unity times unknown lambda (= sigma q 2)

% Every NumDipoleComponents columns of L is a source, which we call A

% (We could optimize this to only do when needed, but it's not too
% expensive in any event, taking less than two seconds for 15000 dipoles)

% Decompose for each source
tic
clear A
A(1:NumDipoles) = deal(struct('Ua',[],'Sa',[],'Va',[]));

for i = 1:NumDipoles,
    % index to next source in the lead field matrix
    ndx = ((1-NumDipoleComponents):0) + i*NumDipoleComponents;
    [Ua,Sa,Va] = svd(L(:,ndx),'econ');
    Sad = diag(Sa); % strip to vector

    tol = length(Sad) * eps(single(Sad(1))); % single precision tolerance
    Rank_Dipole = sum(Sad > tol);

    % Trim or not to the rank of the model.
    switch 'notrim'
        case 'trim' % trim each source model to just the non-zero
            A(i).Ua = Ua(:,1:Rank_Dipole);
            A(i).Sa = Sad(1:Rank_Dipole);
            A(i).Va = Va(:,1:Rank_Dipole);
        case 'notrim'
            % don't trim each source model, but do force small singular values to perfectly zero
            Sad((Rank_Dipole+1):end) = 0; % force to perfect zeros
            A(i).Ua = Ua;
            A(i).Sa = Sad;
            A(i).Va = Va;
    end % switch

end
fprintf('BST_INVERSE > Time to decompose is %.1f seconds\n',toc);
% So L = [A.Ua]*blkdiag(A.Sa)*blkdiag(A.Va)'; % good for any subset too.

% Now do a global decomposition for setting SNR and doing min norms
% Won't need the expensive VL of the SVD, and UL is relatively small.

[UL,SL2] = svd(L*L');
SL2 = diag(SL2);
SL = sqrt(SL2); % the singular values of the lead field matrix
tol = length(SL)*eps(single(SL(1))); % single precision tolerance
Rank_Leadfield = sum(SL > tol);
fprintf('BST_INVERSE > Rank of leadfield matrix is %.0f out of %.0f components\n',Rank_Leadfield,length(SL));
% but don't trim to rank, we use full matrix below

%% =========== SNR METHODS ================

% Recall our source covariance prior is Cx = Lamda * I, where we have already
% factored out the dipolar covariance, Cq = alpha2 * Lambda * Wq * Wq';
%
% We use the SNR settings to establish a prior on the source variance,
% which is in turn used as essentially a regularizer in the inverse
% methods.
%
% This may seem backwards, i.e. the signal variance with respect
% to the noise variance establishes the SNR, but in inverse processing, we
% may do this backwards, setting an SNR in order to regularize, depending
% on the selection below.

% Calculate Lambda multiplier to achieve desired SNR

switch (OPTIONS.SnrMethod)
    case 'rms'
        % user had directly specified the variance
        Lambda =  OPTIONS.SnrRms^2;
        SNR = Lambda * SL2(1); % the assumed SNR for the entire leadfield
    case 'fixed'
        % user had given a desired SNR, set lambda of the Grammian to achieve it
        SNR = OPTIONS.SnrFixed^2;

        % several options here. Hamalainen prefers the average eigenvalue
        % of the Grammian. Mosher would prefer the maximum (norm) for other
        % consistency measures, however user's have become accustomed to
        % the Hamalainen measure.

        % Maximum (matrix norm):
        % Lambda = SNR/(SL(1)^2); % thus SL2(1)*Lambda = desired SNR.

        % Hamalainen definition of SNR in the min norm:
        Lambda = SNR/mean(SL.^2); % should be equalivent to Matti's average eigenvalue definition

    otherwise
        error(['BST_INVERSE > Not supported yet: NoiseMethod="' OPTIONS.SnrMethod '"']);
end

fprintf('BST_INVERSE > Confirm units\n')
if exist('engunits', 'file')
    [LambdaY,tmp,LambdaU] = engunits(sqrt(Lambda));
    fprintf('BST_INVERSE > Assumed RMS of the sources is %.3g %sA-m\n',LambdaY,LambdaU);
else
    fprintf('BST_INVERSE > Assumed RMS of the sources is %g A-m\n', sqrt(Lambda));
end
fprintf('BST_INVERSE > Assumed SNR is %.1f (%.1f dB)\n',SNR,10*log10(SNR));

%% ===== INVERSE METHODS =====
% Generate first the inversions (current dipole time series)
switch lower(OPTIONS.InverseMethod) % {minnorm, lcmv, gls}
    case 'minnorm'

        % We set a single lambda to achieve desired source variance. In min
        % norm, all dipoles have the same lambda. We already added an
        % optional amplifier weighting into the covariance prior above.

        % So the data covariance model in the MNE is now
        % Cd = Lambda * L * L' + I
        % = Lambda *UL * SL * UL' + I = UL * (LAMBDA*SL + I) * UL'
        % so invert Cd and use for kernel
        % xhat = Lambda * L' * inv(Cd)
        % we reapply all of the covariances to put data back in original
        % space in the last step.

        % as distinct from GLS, all dipoles have a common data covariance,
        % but each has a unique noise covariance.

        switch OPTIONS.InverseMeasure % {'amplitude',  'dspm', 'sloreta'}
            case 'amplitude'
                OPTIONS.FunctionName = 'mn';
                % xhat = Lambda * L' * inv(Cd)
                % Kernel = Lambda * L' * inv(Lambda * L * L' + eye(size(L,1))) * iW_noise;

                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL')*iW_noise;

            case 'dspm'
                OPTIONS.FunctionName = 'dspm';
                % ===== dSPM OPERATOR =====
                % =========== NEEDS REWRITING by JCM ======

                % xhat = Lambda * L' * inv(Cd)
                % Kernel = Lambda * L' * inv(Lambda * L * L' + eye(size(L,1))) * iW_noise;

                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL');

                dspmdiag = sum(Kernel .^2, 2);
                if (NumDipoleComponents == 1)
                    dspmdiag = sqrt(dspmdiag);
                elseif (NumDipoleComponents==3 || NumDipoleComponents==2)
                    dspmdiag = reshape(dspmdiag, NumDipoleComponents,[]);
                    dspmdiag = sqrt(sum(dspmdiag,1)); % Taking trace and sqrt.
                    dspmdiag = repmat(dspmdiag, [NumDipoleComponents, 1]);
                    dspmdiag = dspmdiag(:);
                end
                Kernel = bst_bsxfun(@rdivide, Kernel, dspmdiag);

                Kernel = Kernel * iW_noise; % overall whitener

            case 'sloreta'
                OPTIONS.FunctionName = 'sloreta';

                % calculate the standard min norm solution
                Kernel = Lambda * L' * (UL * diag(1./(Lambda * SL2 + 1)) * UL');

                % until I fix multiple head models

                %                 if (NumDipoleComponents(k) == 1)
                %                     sloretadiag = sqrt(sum(Kernel(start:endd,:) .* L(:,start:endd)', 2));
                %                     Kernel(start:endd,:) = bst_bsxfun(@rdivide, Kernel(start:endd,:), sloretadiag);
                %                 elseif (NumDipoleComponents(k)==3 || NumDipoleComponents(k)==2)
                %                     for spoint = start:NumDipoleComponents(k):endd
                %                         R = Kernel(spoint:spoint+NumDipoleComponents(k)-1,:) * L(:,spoint:spoint+NumDipoleComponents(k)-1);
                %                         SIR = sqrtm(pinv(R));
                %                         Kernel(spoint:spoint+NumDipoleComponents(k)-1,:) = SIR * Kernel(spoint:spoint+NumDipoleComponents(k)-1,:);
                %                     end
                %                 end

                if (NumDipoleComponents == 1)
                    sloretadiag = sqrt(sum(Kernel .* L', 2));
                    Kernel = bst_bsxfun(@rdivide, Kernel, sloretadiag);
                elseif (NumDipoleComponents==3 || NumDipoleComponents==2)
                    for spoint = 1:NumDipoleComponents:size(Kernel,1),
                        R = Kernel(spoint:spoint+NumDipoleComponents-1,:) * L(:,spoint:spoint+NumDipoleComponents-1);
                        % SIR = sqrtm(pinv(R)); % Aug 2016 can lead to errors if
                        % singular Use this more explicit form instead
                        [Ur,Sr,Vr] = svd(R); Sr = diag(Sr);
                        RNK = sum(Sr > (length(Sr) * eps(single(Sr(1))))); % single precision Rank
                        SIR = Vr(:,1:RNK) * diag(1./sqrt(Sr(1:RNK))) * Ur(:,1:RNK)'; % square root of inverse

                        Kernel(spoint:spoint+NumDipoleComponents-1,:) = SIR * Kernel(spoint:spoint+NumDipoleComponents-1,:);
                    end
                end

                Kernel = Kernel * iW_noise; % overall whitener

            otherwise
                error('Unknown Option Inverse Measure: %s',OPTIONS.InverseMeasure)

        end

    case {'gls','lcmv'}

        % In generalized least-squares, each dipolar source may have a
        % unique lambda, to achieve the desired SNR. So unlike min norm, we
        % may need to set lambda for each and every dipole.

        % However, as a prior, this seems counter-intuitive, since it would
        % mean adjusting the variance of deep sources to be much greater
        % (and therefore less reqularized) than shallower sources.

        % So in this version, JCM opted to set one global source variance, as in min norm.

        % Note that each Wq may already contain a desired amplifier for the
        % gain matrix of that source. That's okay.

        % As distinct from minimum norm, every dipole has the exact same
        % noise covariance assumption.

        % Neyman-Pearson Performance
        if strcmpi(OPTIONS.InverseMethod, 'gls')
            OPTIONS.FunctionName = 'glsp';
        else
            OPTIONS.FunctionName = 'lcmvp';
        end

        Kernel = zeros(size(L')); % preallocate

        for i = 1:NumDipoles,
            ndx = ((1-NumDipoleComponents):0) + i*NumDipoleComponents;
            %Kernel(ndx,:) =  A(i).Va*(Lambda*A(i).Sa)*inv(Lambda*A(i).Sa + I)*A(i).Ua';
            Kernel(ndx,:) =  A(i).Ua';
        end
        Kernel = Kernel * iW_noise; % final noise whitening

    otherwise
        error('BST> Unknown inverse method: %s',OPTIONS.InverseMethod)

end

%% ===== ASSIGN IMAGING KERNEL =====
% Multiply inverse operator and whitening matrix, so no need to whiten data.
% premultiply by covariance priors to put back into original domain

% Now the Kernel has been designed for the number of dipole components.
% Ordinarily, to convert back to three d, we would use
% Kernel = WQ * Kernel, which puts all one-d answers back in three-d. But
% that's not optimal for storage. We do need, however, to account for the
% possible Alpha increase in the storage.

% Key assumption is that the source prior is norm 1, as designed in the
% beginning of this program.
if strcmpi(OPTIONS.InverseMeasure, 'amplitude')
    % we need to put orientation and weighting back into solution
    if NumDipoleComponents == 3, % we are in three-d,
        Kernel = WQ * Kernel; % put the source prior back into the solution
    elseif NumDipoleComponents == 1,
        Kernel = diag(Alpha)*Kernel;  % put possible alpha weighting back in
    end
end

% now orient the dipoles, if requested
% Orientation Optimization is now done in the "dipole scanning" process
% applied to the performance measure.
GridOrient = [];

% Return results structure
Results.ImagingKernel = Kernel;
Results.GridOrient    = GridOrient; % either empty or optimized
Results.ImageGridAmp  = [];
Results.Whitener      = iW_noise; % full noise covariance whitener
if strcmpi(OPTIONS.InverseMethod, 'lcmv')
    % in the lcmv case, we used the data covariance as the whitener for
    % programming simplicity
    Results.Whitener = iW_noise_true; % the true noise whitener, unused in the case of LCMV, but may be useful in post processing
end
Results.DataWhitener  = iW_data;  % full data covariance whitener, null unless lcmv
Results.nComponents   = NumDipoleComponents;

% clear SourceDecomp % decomposition products of the sources
% % don't need Ua, it was already included in the ImagingKernel
% % Need the other components for reconstructing the source orientation and
% % amplitude
% [SourceDecomp(1:length(A))] = deal(struct('Sa',[],'Va',[]));
%
% for i =1:length(A),
%     SourceDecomp(i).Sa = diag(A(i).Sa); % let's be efficient and store as vector
%     SourceDecomp(i).Va = A(i).Va;
% end
%
% Results.SourceDecomp = SourceDecomp;

% More storage efficient:
Results.SourceDecompVa = [A.Va];
Results.SourceDecompSa = [A.Sa];

end

%% ==============================================================================
%  ===== HELPER FUNCTIONS =======================================================
%  ==============================================================================

%% =========== Covariance Truncation and Regularization
function [Cov,iW] = truncate_and_regularize_covariance(Cov,Method,Type,NoiseReg,FourthMoment,nSamples)
% Cov is the covariance matrix, to be regularized using Method
% Type is the sensor type for display purposes
% NoiseReg is the regularization fraction, if Method "reg" selected
% FourthMoment and nSamples are used if Method "shrinkage" selected

VERBOSE = true; % be talkative about what's happening

% Ensure symmetry
Cov = (Cov + Cov')/2;

% Note,impossible to be complex by above symmetry check
% Decompose just this covariance.
[Un,Sn2] = svd(Cov,'econ');
Sn = sqrt(diag(Sn2)); % singular values
tol = length(Sn) * eps(single(Sn(1))); % single precision tolerance
Rank_Noise = sum(Sn > tol);

if VERBOSE,
    fprintf('BST_INVERSE > Rank of the %s channels, keeping %.0f noise eigenvalues out of %.0f original set\n',...
        Type,Rank_Noise,length(Sn));
end

Un = Un(:,1:Rank_Noise);
Sn = Sn(1:Rank_Noise);

% now rebuild the noise covariance matrix with just the non-zero
% components
Cov = Un*diag(Sn.^2)*Un'; % possibly deficient matrix now

% With this modality truncated, see if we need any additional
% regularizations, and build the inverse whitener

if VERBOSE
    fprintf('BST_INVERSE > Using the ''%s'' method of covariance regularization.\n',Method);
end

switch(Method) % {'shrink', 'reg', 'diag', 'none', 'median'}

    case 'none'
        %  "none" in Regularization means no
        % regularization was applied to the computed Noise Covariance
        % Matrix
        % Do Nothing to Cw_noise
        iW = Un*diag(1./Sn)*Un'; % inverse whitener
        if VERBOSE,
            fprintf('BST_INVERSE > No regularization applied to covariance matrix.\n');
        end

    case 'median'
        if VERBOSE,
            fprintf('BST_INVERSE > Covariance regularized by flattening tail of eigenvalues spectrum to the median value of %.1e\n',median(Sn));
        end
        Sn = max(Sn,median(Sn)); % removes deficient small values
        Cov = Un*diag(Sn.^2)*Un'; % rebuild again.
        iW = Un*diag(1./Sn)*Un'; % inverse whitener

    case 'diag'
        Cov = diag(diag(Cov)); % strip to diagonal
        iW = diag(1./sqrt(diag(Cov))); % inverse of diagonal
        if VERBOSE,
            fprintf('BST_INVERSE > Covariance matrix reduced to diagonal.\n');
        end

    case 'reg'
        % The unit of "Regularize Noise Covariance" is as a percentage of
        % the mean variance of the modality.

        % Ridge Regression:
        RidgeFactor = Sn2(1) * NoiseReg ; % percentage of max

        Cov = Cov + RidgeFactor * eye(size(Cov,1));
        iW = Un*diag(1./(Sn + sqrt(RidgeFactor)))*Un'; % inverse whitener

        if VERBOSE,
            fprintf('BST_INVERSE > Diagonal of %.1f%% of largest eigenvalue added to covariance matrix.\n',NoiseReg * 100);
        end

    case 'shrink'
        % Method of Ledoit, recommended by Alexandre Gramfort

        % Need to scale the Fourth Moment for the modalities

        % use modified version of cov1para attached to this function
        % TODO, can we adjust this routine to handle different numbers of
        % samples in the generation of the fourth order moments
        % calculation? As of August 2016, still relying on a single scalar
        % number.
        [Cov,shrinkage]=cov1para_local(Cov,FourthMoment,nSamples);
        if VERBOSE,
            fprintf('\nShrinkage factor is %f\n\n',shrinkage)
        end
        % we now have the "shrunk" whitened noise covariance
        % Recalculate
        [Un,Sn2] = svd(Cov,'econ');
        Sn = sqrt(diag(Sn2)); % singular values
        tol = length(Sn) * eps(single(Sn(1))); % single precision tolerance
        Rank_Noise = sum(Sn > tol);

        if VERBOSE,
            fprintf('BST_INVERSE > Ledoit covariance regularization, after shrinkage, rank of the %s channels, keeping %.0f noise eigenvalues out of %.0f original set\n',...
                Type,Rank_Noise,length(Sn));
        end

        Un = Un(:,1:Rank_Noise);
        Sn = Sn(1:Rank_Noise);

        % now rebuild the noise covariance matrix with just the non-zero
        % components
        Cov = Un*diag(Sn.^2)*Un'; % possibly deficient matrix now

        iW = Un*diag(1./Sn)*Un'; % inverse whitener

    otherwise
        error(['BST_INVERSE > Unknown covariance regularization method: NoiseMethod="' Method '"']);

end % method of regularization

% Note the design of full rotating whiteners. We don't expect dramatic reductions in
% rank here, and it's convenient to rotate back to the original space.
% Note that these whitener matrices may not be of full rank.

end
%% ======== Ledoit Shrinkage
% Modified to use the precalculated stats

function [sNoiseCov,shrinkage]=cov1para_local(NoiseCov,FourthMoment,nSamples)
% Based on Ledoit's "cov1para" with some modifications
%   x is t x n, returns
%   sigma n x n
%
% shrinkage is the final computed shrinkage factor, used to weight the
%  i.i.d. prior vs the sample estimate. If shrinkage is specified, it is
%  used on input; else, it's computed.

% Original code from
% http://www.ledoit.net/cov1para.m
% Original Ledoit comments:
% function sigma=cov1para(x)
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards one-parameter matrix:
%    all variances are the same
%    all covariances are zero
% if shrink is specified, then this const. is used for shrinkage

% Based on
% http://www.ledoit.net/ole1_abstract.htm
% http://www.ledoit.net/ole1a.pdf (PDF of paper)
%
% A Well-Conditioned Estimator for Large-Dimensional Covariance Matrices
% Olivier Ledoit and Michael Wolf
% Journal of Multivariate Analysis, Volume 88, Issue 2, February 2004, pages 365-411
%
% Abstract
% Many economic problems require a covariance matrix estimator that is not
% only invertible, but also well-conditioned (that is, inverting it does
% not amplify estimation error). For large-dimensional covariance matrices,
% the usual estimator - the sample covariance matrix - is typically not
% well-conditioned and may not even be invertible. This paper introduces an
% estimator that is both well-conditioned and more accurate than the sample
% covariance matrix asymptotically. This estimator is distribution-free and
% has a simple explicit formula that is easy to compute and interpret. It
% is the asymptotically optimal convex combination of the sample covariance
% matrix with the identity matrix. Optimality is meant with respect to a
% quadratic loss function, asymptotically as the number of observations and
% the number of variables go to infinity together. Extensive Monte-Carlo
% confirm that the asymptotic results tend to hold well in finite sample.

% Original Code Header, updated to be now from (2014)
% http://www.econ.uzh.ch/faculty/wolf/publications/cov1para.m.zip
%
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards constant correlation matrix
% if shrink is specified, then this constant is used for shrinkage
%
% The notation follows Ledoit and Wolf (2003, 2004)
% This version 04/2014
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is released under the BSD 2-clause license.
%
% Copyright (c) 2014, Olivier Ledoit and Michael Wolf
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Wolf's site now,
% http://www.econ.uzh.ch/faculty/wolf/publications/cov1para.m.zip
% some differences from original Ledoit that confirm Mosher's
% original re-coding.

% % de-mean returns
% [t,n]=size(x);
% meanx=mean(x);
% x=x-meanx(ones(t,1),:);

% compute sample covariance matrix
% Provided
% NoiseCov=(1/t).*(x'*x);

% compute prior
n=size(NoiseCov,1); % number of channels
meanvar=mean(diag(NoiseCov));
prior=meanvar*eye(n); % Note, should be near identity by our pre-whitening

% what we call p
%y=x.^2;
%phiMat=y'*y/t - NoiseCov.^2;
phiMat = FourthMoment - NoiseCov.^2;
phi=sum(sum(phiMat));

% what we call r is not needed for this shrinkage target

% what we call c
gamma=norm(NoiseCov-prior,'fro')^2;

% compute shrinkage constant
kappa=phi/gamma;
% ensure bounded between zero and one
shrinkage=max(0,min(1,kappa/nSamples));

% compute shrinkage estimator
sNoiseCov=shrinkage*prior+(1-shrinkage)*NoiseCov;

end

author: brainstorm-tools