text:
function [g, model] = vargplvmPointLogLikeGradient(model, vardistx, y)
% VARGPLVMPOINTLOGLIKEGRADIENT Log-likelihood gradient for of some points of the GP-LVM.
% FORMAT
% DESC returns the gradient of the log likelihood with respect to
% the latent positions, where the log likelihood is conditioned on
% the training set. The function works even when we are interested only in
% one point. See vargplvmPointLogLikelihood for more details.
% ARG model : the model for which the gradient computation is being
% done.
% ARG x : the latent positions where the gradient is being computed.
% ARG y : the positions in data space for which the computation is
% being done.
% RETURN g : the gradient of the log likelihood, conditioned on the
% training data, with respect to the latent position.
%
% SEEALSO : vargplvmPointLogLikelihood, vargplvmOptimisePoint, vagplvmSequenceLogLikeGradient
%
% COPYRIGHT : Michalis K. Titsias and Neil D. Lawrence, 2009, 2011
% COPYRIGHT : Andreas C. Damianou, 2011
% VARGPLVM

if isfield(model, 'dynamics') && ~isempty(model.dynamics)
    dynUsed = 1;
else
    dynUsed = 0;
end

indexMissing = find(isnan(y(1,:)));
indexPresent = setdiff(1:model.d, indexMissing);
y = y(:,indexPresent);

if dynUsed
    % use a special function
    [g, model] = dynPointLogLikeGradient(model, vardistx, y, indexPresent, indexMissing);
    % g = g(:)'; %%% RE-OPT-CODE-REM
    return;
end

% normalize y exactly as model.m is normalized
my = y - repmat(model.bias(indexPresent),size(y,1),1);
my = my./repmat(model.scale(indexPresent),size(y,1),1);

[gPsi0, gPsi1, gPsi2] = vargpCovGrads(model, vardistx, my, indexPresent);

[gKern1, gVarmeans1, gVarcovs1] = kernVardistPsi1Gradient(model.kern, vardistx, model.X_u, gPsi1');
[gKern2, gVarmeans2, gVarcovs2] = kernVardistPsi2Gradient(model.kern, vardistx, model.X_u, gPsi2);
[gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, vardistx, gPsi0);

gVarcovs0 = (gVarcovs0(:).*vardistx.covars(:))';
gVarcovs1 = (gVarcovs1(:).*vardistx.covars(:))';
gVarcovs2 = (gVarcovs2(:).*vardistx.covars(:))';

% KL divergence terms
gVarmeansKL = - vardistx.means(:)';
% !!! the covars are optimized in the log space
gVarcovsKL = 0.5 - 0.5*vardistx.covars(:)';

gVarmeans = gVarmeans0 + gVarmeans1 + gVarmeans2 + gVarmeansKL;
gVarcovs = gVarcovs0 + gVarcovs1 + gVarcovs2 + gVarcovsKL;

g = [gVarmeans gVarcovs];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
function [gPsi0, gPsi1, gPsi2] = vargpCovGrads(model, vardistx, my, indexPresent)
%
%

d = prod(size(indexPresent));

% change the data (by including the new point and taking only the present indices)
model.m = model.m(:,indexPresent);
model.m = [my; model.m];

pointPsi1 = kernVardistPsi1Compute(model.kern, vardistx, model.X_u);
pointPsi2 = kernVardistPsi2Compute(model.kern, vardistx, model.X_u);

model.Psi1 = [pointPsi1; model.Psi1];

model.C = model.invLm * (model.Psi2 + pointPsi2) * model.invLmT;
model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C;
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
model.invLatT = model.invLat';
model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
P1TP1 = (model.P1' * model.P1);
Tb = (1/model.beta) * d * P1TP1;
Tb = Tb + (model.B * model.B');
model.T1 = d * model.invK_uu - Tb;

gPsi2 = (model.beta/2) * model.T1;

gPsi0 = -0.5 * model.beta * d;

gPsi1 = model.beta*(P1TP1*model.Psi1'*model.m*my');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
function [gPointDyn, model] = dynPointLogLikeGradient(model, vardistx, y, indexPresent, indexMissing)
jitter = 1e-6;

%%% RE-OPT-CODE-NEW_
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    % In this case the inducing points and theta_t are optimised and
    % since updateStats is not called, any computations which need X_u
    % or theta_t must be done here.
    model = vargplvmDynamicsUpdateStats(model);
    model.K_uu = kernCompute(model.kern, model.X_u);
    model.K_uu = model.K_uu ...
        + sparseDiag(repmat(jitter, size(model.K_uu, 1), 1));
    model.Lm = jitChol(model.K_uu)';
    model.invLm = model.Lm\eye(model.k);
    model.invLmT = model.invLm';
    model.invK_uu = model.invLmT * model.invLm;
end %%% _RE-OPT-CODE-NEW

%%%%%%%%% Only the missing parts and from the training data only
mOrig = model.m;
vardistDynTemp = model.dynamics.vardist;

N = model.N;
Nstar = size(y,1);
Kt = zeros(N+Nstar, N+Nstar);

%model.y = model.y(:,indexMissing);
model.m = model.m(:,indexMissing);
model.d = prod(size(indexMissing));
% Augment the time vector to include the timestamp of the new point
model.dynamics.t = [model.dynamics.t; model.dynamics.t_star];
% Augment the reparametrized variational parameters mubar and lambda
model.dynamics.vardist.means = [model.dynamics.vardist.means; vardistx.means];
model.dynamics.vardist.covars = [model.dynamics.vardist.covars; vardistx.covars];
model.dynamics.N = model.dynamics.N + Nstar;
model.vardist.numData = model.dynamics.N;
model.dynamics.vardist.numData = model.dynamics.N;
model.vardist.nParams = 2*prod(size(model.dynamics.vardist.means));
model.dynamics.vardist.nParams = 2*prod(size(model.dynamics.vardist.means));
%model.dynamics.seq = model.dynamics.N; %%%%%% Chec
% Faster computation of the Kt matrix
% (all this could have been avoided as Kt is constant... we need to do these
% precomputations outside of this function)
% construct Kt
Kt(1:N,1:N) = model.dynamics.Kt;
% new diagonal block
Kt(N+1:end, N+1:end) = kernCompute(model.dynamics.kern, model.dynamics.t_star);
% cross block
if ~isfield(model.dynamics, 'seq') || isempty(model.dynamics.seq)
    Kt(1:N, N+1:end) = kernCompute(model.dynamics.kern, model.dynamics.t(1:N), model.dynamics.t_star);
    Kt(N+1:end, 1:N) = Kt(1:N, N+1:end)';
end
model.dynamics.Kt = Kt;
model.dynamics.fixedKt = 1;
model = vargpTimeDynamicsUpdateStats(model);

vardist2 = model.vardist;
vardist2.means = model.vardist.means(1:end-Nstar,:);
vardist2.covars = model.vardist.covars(1:end-Nstar,:);
vardist2.nParams = 2*prod(size(vardist2.means));
vardist2.numData = size(vardist2.means,1);
model.Psi0 = kernVardistPsi0Compute(model.kern, vardist2);
model.Psi1 = kernVardistPsi1Compute(model.kern, vardist2, model.X_u);
model.Psi2 = kernVardistPsi2Compute(model.kern, vardist2, model.X_u);

model.C = model.invLm * model.Psi2 * model.invLmT;
%model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C; % At = beta^{-1} I + C
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
%model.invLatT = model.invLat';
%model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
%model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
%model.invK_uu = model.invLmT * model.invLm;
Tb = (1/model.beta) * model.d * (model.P1' * model.P1);
Tb = Tb + (model.B * model.B');
model.T1 = model.d * model.invK_uu - Tb;

modelTmp = model;
modelTmp.vardist = vardist2;
modelTmp.dynamics.vardist = vardistDynTemp;
modelTmp.dynamics.Kt = modelTmp.dynamics.Kt(1:end-Nstar,1:end-Nstar);
modelTmp.dynamics.N = N;
modelTmp.dynamics.t = modelTmp.dynamics.t(1:end-Nstar);
modelTmp.dynamics.vardist.numData = modelTmp.dynamics.N; %%%% new...
modelTmp.vardist.numData = modelTmp.N;%%%% new...
%modelTmp.TrYY = sum(sum(model.m .* model.m));

% GRADIENT OF THE LL1 TERM mu,S from the training data

% To understand the following code it's helpful to see
% vargpTimeDynamicsPriorReparamGrads.m; LL1 term now is not computed
% together with KL so we cannot use that function here; however, it does
% contain theta_t so these computations must be done but only for the part
% corresponding to modelTmp. Thus, the following code actually is a copy of
% vargpTimeDynamicsPriorReparamGrads but is applied only on the appropriate
% part of Kt.
% i.e: gPointDyn1Tmp(:,1:model.dynamics.q) -> gVarmeansLik
%      gPointDyn1Tmp(:,model.dynamics.q+q) -> gVarcovsLik(:,q)

%gPointDyn1 = zeros(Nstar,modelTmp.dynamics.q*2);
gPointDyn1 = zeros(N+Nstar,modelTmp.dynamics.q*2);
if ~isempty(indexMissing)
    if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise %%% RE-OPT-CODE-NEW
        % gDynKern will be empty in that case (includeKL is 0) %%% RE-OPT-CODE-NEW
        [gPointDyn1Tmp, gDynKern, gInd1]= vargplvmLogLikeGradientsVar1(modelTmp, 0); %%% RE-OPT-CODE-NEW
        sumTrGradKL = 0; %%% RE-OPT-CODE-NEW
    else %%% RE-OPT-CODE-NEW
        gPointDyn1Tmp = vargplvmLogLikeGradientsVar1(modelTmp, 0);
    end %%% RE-OPT-CODE-NEW
    gPointDyn1Tmp = reshape(gPointDyn1Tmp, modelTmp.dynamics.vardist.numData, modelTmp.dynamics.q*2);

    % means (Note: Kt(1:N,:)' == Kt(:,1:N)  )
    gPointDyn1(:,1:modelTmp.dynamics.q) = model.dynamics.Kt(1:N,:)'*gPointDyn1Tmp(:,1:model.dynamics.q);

    % covars
    %gcovTmp = zeros(Nstar,modelTmp.dynamics.q);
    gcovTmp = zeros(N+Nstar,modelTmp.dynamics.q);
    sumTrGradKL1 = zeros(N+Nstar,N);
    sumTrGradKL2 = zeros(N+Nstar, N+Nstar);
    for q=1:model.dynamics.q
        LambdaH_q = model.dynamics.vardist.covars(:,q).^0.5;
        Bt_q = eye(model.dynamics.N) + LambdaH_q*LambdaH_q'.*model.dynamics.Kt;
        % Invert Bt_q
        Lbt_q = jitChol(Bt_q)';
        G1 = Lbt_q \ diag(LambdaH_q);
        G = G1*model.dynamics.Kt;
        % Find Sq
        Sq = model.dynamics.Kt - G'*G;
        Sq = - (Sq .* Sq);

        % only the cross matrix is needed as in barmu case
        %gcovTmp(:,q) = Sq(1:N,N+1:end)'*gPointDyn1Tmp(:,model.dynamics.q+q);

        % gPointDyn1Tmp contains [means covars]. model.dynamics.q+q gives
        % us only the covars.
        gcovTmp(:,q) = Sq(1:N,:)'*gPointDyn1Tmp(:,model.dynamics.q+q);

        %%% RE-OPT-CODE-NEW_
        if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            IBK = eye(model.dynamics.N) - BhatKt;

            diagVarcovs = repmat(gPointDyn1Tmp(:,model.dynamics.q+q)', model.dynamics.N,1);
            sumTrGradKL2 = sumTrGradKL2  +  (IBK(:,1:N).*diagVarcovs)*IBK(:,1:N)';

           %%%
         %   tmp = [gPointDyn1Tmp(:,model.dynamics.q+q)' zeros(1,Nstar)]; %tmp
         %   tmp2=IBK*diag(tmp)*IBK'; %tmp
         %   sumTrGradKL2 = sumTrGradKL2  +  tmp2; % tmp
           %%%

            sumTrGradKL1 = sumTrGradKL1 + model.dynamics.vardist.means(:,q)*gPointDyn1Tmp(:,q)';
            %if ~isempty(gInd)
            %    trGradKL = trGradKL + dynModel.vardist.means(:,q) * gInd(:,q)';
            %end
        end
        %%% _RE-OPT-CODE-NEW
    end

    gPointDyn1(:,(modelTmp.dynamics.q+1):(modelTmp.dynamics.q*2)) = gcovTmp;

    if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise                   %%% RE-OPT-CODE-NEW
        gDynKern1 = kernGradient(model.dynamics.kern, model.dynamics.t, model.dynamics.t(1:N),  sumTrGradKL1)  ; %%% RE-OPT-CODE-NEW
        gDynKern1 = gDynKern1 + kernGradient(model.dynamics.kern, model.dynamics.t, sumTrGradKL2); %%% RE-OPT-CODE-NEW
    end                                                                                %%% RE-OPT-CODE-NEW

end

% GRADIENT FOR LL2 plus KL TERMS
%
model.m = mOrig;
% normalize y exactly as model.m is normalized
my = y - repmat(model.bias(indexPresent), Nstar, 1);
my = my./repmat(model.scale(indexPresent), Nstar, 1);
model.m = model.m(:,indexPresent);
model.m = [model.m; my];
model.N = model.N + Nstar;
d = prod(size(indexPresent));
model.d = d;
model.dynamics.nParams = model.dynamics.nParams + 2*prod(size(vardistx.means));
model.nParams = model.nParams + 2*prod(size(vardistx.means));

vardist2 = model.vardist;
vardist2.means = model.vardist.means(end-Nstar+1:end,:);
vardist2.covars = model.vardist.covars(end-Nstar+1:end,:);
vardist2.nParams = 2*prod(size(vardist2.means));
vardist2.numData = size(vardist2.means,1);
pointPsi0 = kernVardistPsi0Compute(model.kern, vardist2);
pointPsi1 = kernVardistPsi1Compute(model.kern, vardist2, model.X_u);
pointPsi2 = kernVardistPsi2Compute(model.kern, vardist2, model.X_u);
model.Psi1 = [model.Psi1; pointPsi1];
model.Psi2 = model.Psi2 + pointPsi2;
model.Psi0 = model.Psi0 + pointPsi0;

model.C = model.invLm * model.Psi2 * model.invLmT;
%model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C; % At = beta^{-1} I + C
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
%model.invLatT = model.invLat';
%model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
%model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
%model.invK_uu = model.invLmT * model.invLm;
Tb = (1/model.beta) * model.d * (model.P1' * model.P1);
Tb = Tb + (model.B * model.B');
model.T1 = model.d * model.invK_uu - Tb;

if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise %%% RE-OPT-CODE-NEW
    [gPointDyn2, gDynKern2, gInd2] = vargplvmLogLikeGradientsVar1(model,1); %%% RE-OPT-CODE-NEW
else %%% RE-OPT-CODE-NEW
    gPointDyn2 = vargplvmLogLikeGradientsVar1(model,1);
end %%% RE-OPT-CODE-NEW

gPointDyn2 = reshape(gPointDyn2, model.dynamics.vardist.numData, model.dynamics.q*2);
%gPointDyn2 = gPointDyn2(N+1:end,:);

gPointDyn = gPointDyn1 + gPointDyn2;

%%% RE-OPT-CODE-NEW_
gPointDyn = gPointDyn(:)';
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    gDynKern = gDynKern1 + gDynKern2;
    gInd = gInd1 + gInd2;
    if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32')
        if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
            gDynKern(2) = 0;
        end
    end
    gPointDyn = [gPointDyn 0*gDynKern gInd]; %%%%%%%%%% TEMP!!!!
end
%%% _RE-OPT-CODE-NEW

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
%function gVar = vargplvmLogLikeGradientsVar1(model, includeKL) %%% RE-OPT-CODE-REM
function [gVar, gDynKern, gInd] = vargplvmLogLikeGradientsVar1(model, includeKL) %%% RE-OPT-CODE-NEW
% Like vargplvmLogLikeGradients but only for the variational parameters (ehm, not exactly). %%% RE-OPT-CODE-MOD

% Likelihood terms (coefficients)
gPsi1 = model.beta * model.m * model.B';
gPsi1 = gPsi1'; % because it is passed to "kernVardistPsi1Gradient" as gPsi1'...
gPsi2 = (model.beta/2) * model.T1;
gPsi0 = -0.5 * model.beta * model.d;

[gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1');
[gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2);
[gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);

gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;
if includeKL
    %[gVarmeans gVarcovs gDynKern] = modelPriorReparamGrads(model.dynamics, gVarmeansLik, gVarcovsLik);
    % TODO: This may have to be changed if we have model.fixInducing==1.
    [gVarmeans gVarcovs gDynKern] = modelPriorReparamGrads(model.dynamics, gVarmeansLik, gVarcovsLik,[]);
else
    dynModel = model.dynamics;
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);
    gVarmeans = gVarmeansLik;
    gVarcovs = gVarcovsLik;

    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';
end

gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';
gVar = [gVarmeans gVarcovs];

%%% RE-OPT-CODE-NEW_
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    % Compute Gradients with respect to X_u
    gK_uu = 0.5 * (model.T1 - (model.beta * model.d) * model.invLmT * model.C * model.invLm);
    gKX = kernGradX(model.kern, model.X_u, model.X_u);
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end
    gX_u = zeros(model.k, model.q);
    for i = 1:model.k
        for j = 1:model.q
            gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
        end
    end
    gInd = gInd1 + gInd2 + gX_u(:)';
    if ~includeKL
        gDynKern = [];
    end
end
%%% _RE-OPT-CODE-NEW

repo: vargplvm


text:
function beta=calcStarBetaskBest(A,K)
%%CALCSTARBETASKBEST Calculate target-measurement association probabilities,
%                    as in the JPDAFStar given an association matrix using
%                    only the k-best hypotheses for the computation of the
%                    target-measurement association probabilities rather
%                    than goign through all joint association events.
%
%INPUTS:   A  A numTar X numMeas matrix of all-positive likelihood
%              ratios for assigning the target specified by the row to the
%              measurement specified by the column.
%          K  The number of hypotheses to generate.
%
%OUTPUTS:  beta A numTar X (numMeas+1) matrix of probabilities of assigning
%               the target given by the row to the measurement given by the
%               column. The final column is a set of missed detection
%               probabilities.
%
%The general idea behind this function is the same as that of the function
%calcStarBetasBF. However, rather than generating all possible
%joint association events, only the k-best events are generated.
%
%To reformulate the problem such that a 2D assignment algorithm can be
%used, the logarithm of the likelihood ratios must be taken, then the cost
%values are exponentiated to get traditional probabilities.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%In order to be able to use the k-Best 2D assignment algorithm, the
%association matrix must be augmented with extra columns that represent
%missed detection events. When the likelihood ratios in A have been
%appropriately computed, the likelihood ratio of a missed detection event
%is equal to one.

numTar=size(A,1);
numMeas=size(A,2);

%Adjust the cost matrix so that 2D assignment can be use.
A=log(A);

%Alocate space for the results.
beta=zeros(numTar,numMeas+1);

%Augment the A matrix to handle missed detection hypotheses. These are
%supposed to have a likelihood ratio of 1, so  the logarithm should be
%zero.
AClut=1-eye(numTar);
AClut(AClut==1)=-inf;
[col4rowBest,row4ColBest,gainBest]=kBest2DAssignment([A,AClut],K,true);

%Undo the logarithm from before.
gainBest=exp(gainBest);

%Adjust K to reflect the fact that fewer than K hypotheses might have been
%present.
K=size(col4rowBest,2);

%For each hypothesis, determine the set of measurements that are
%target-originated and save them order in tarMeas. Also find the set of
%targets that are observed and save them in obsTar.
tarMeas=zeros(numTar,K);
obsTar=zeros(numMeas,K);
for curHyp=1:K
    tarMeas(:,curHyp)=col4rowBest(:,curHyp);

    %Remove elements that are missed detections.
    misSel=col4rowBest(:,curHyp)>numMeas;
    tarMeas(misSel,curHyp)=0;

    %Sort the result.
    tarMeas(:,curHyp)=sort(tarMeas(:,curHyp));
    obsTar(:,curHyp)=sort(row4ColBest(1:numMeas,curHyp));
end

%For each set of observed targets and measurements that are
%target-originated, go through and only keep the most likely hypothesis.
%Since everything is already ordered in decreasing gain, the first
%occurrence of each set of observed targets and target-originated
%measurements is the most likely.
validHyps=true(K,1);
for curHyp=1:K
    if(validHyps(curHyp)==false)
        continue;
    end
    validHyps(curHyp)=false;

    for HypS=(curHyp+1):K
        if(isequal(tarMeas(:,curHyp),tarMeas(:,HypS))&&isequal(obsTar(:,curHyp),obsTar(:,HypS)))
            validHyps(HypS)=false;
        end
    end

    %Add the ML hypothesis with the given set of observed targets and
    %target-originated measurements to the beta terms.
    for curTar=1:numTar
        curMeas=col4rowBest(curTar,curHyp);
        if(curMeas>numMeas)
            %If it is a missed detection.
             beta(curTar,end)=beta(curTar,end)+gainBest(curHyp);
        else
            beta(curTar,curMeas)=beta(curTar,curMeas)+gainBest(curHyp);
        end
    end
end

%Normalize the betas so that sum of the probabilities of each target being
%assigned to a measurement or a missed detection sums to one.
beta=bsxfun(@rdivide,beta,sum(beta,2));

end

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

author: USNavalResearchLaboratory

repo: TrackerComponentLibrary


text:
function [out] = percolation_5(p1,p2,S,Smax,dt)
%percolation_5

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Non-linear percolation
% Constraints:  f <= S/dt
%               S >= 0      prevents complex numbers
% @(Inputs):    p1   - base percolation rate [mm/d]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               dt   - time step size [d]

out = min(S/dt,p1.*((max(S,0)./Smax).^p2));

end

author: wknoben

repo: MARRMoT

save_path: github-repos/MATLAB/wknoben-MARRMoT


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

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

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

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

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

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

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

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

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

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

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

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

fprintf(1,'\n');

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

check_active_images;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

% Conditioning threshold for view rejection
thresh_cond = 1e6;

% Initialization of the intrinsic parameters (if necessary)

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

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

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

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

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

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

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

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

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

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

%%% Initialization of the global parameter vector:

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

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

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

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

param = init_param;
change = 1;

iter = 0;

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

param_list = param;

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

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

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

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

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

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

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

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

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

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

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

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

        Np = N_points_views(kk);

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

        exkk = x_kk - x;

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

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

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

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

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

        %end;

    end;

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

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

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

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

    % Smoothing coefficient:

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

    param_innov = alpha_smooth2*JJ2_inv*ex3;

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

    % New intrinsic parameters:

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

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

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

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

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

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

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

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

end;

fprintf(1,'done\n');

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

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

check_active_images;

solution = param;

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

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

for kk = 1:n_ima,

    if active_images(kk),

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

    else

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

    end;

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

end;

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

sigma_x = std(ex(:));

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

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

for kk = ind_active,

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

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

    Np = N_points_views(kk);

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

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

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

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

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

end;

JJ3 = JJ3(ind_Jac,ind_Jac);

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

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

solution_error = param_error;

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

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

extract_parameters;

fprintf(1,'done\n');

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

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

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

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

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

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

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

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

return;

repo: ekfmonoslam


text:
function model = vargplvmDynamicsUpdateStats(model)
% VARGPLVMDYNAMICSUPDATESTATS wrapper function which according to the type
% of the model dynamics calls the appropriate function to perform the
% precomputations needed for the dynamics model
% DESC
% ARG model: the model that contains the dynamics for which the statistics
% must be updated
% RETURN model : the updated model
%
% COPYRIGHT Andreas C. Damianou, Michalis Titsias, Neil Lawrence, 2010-2011
%
% VARGPLVM

fhandle = str2func([model.dynamics.type 'UpdateStats']);
model = fhandle(model);

author: SheffieldML


text:
classdef MapFeature < handle
    %MapFeature used to track and plot features on a map (works in 3d)
    %
    %   sample usage:
    %       load_fn = @get_earthquakes;
    %       save_fn = [];
    %       plot_defaults = struct('Tag','important_feature',...
    %            'DisplayName','importantFeature',...
    %            'LineWidth', 3.0,...
    %            'Color',[.1 .2 .5]);
    %
    %       f = MapFeature('feature', load_fn, save_fn, plot_defaults);
    %       ax = axes; % create new axes
    %       f.plot(ax);
    %
    %       % .. do something here that might change the axes limits or something
    %
    %       f.refreshPlot();
    %
    %
    %
    % lakes = ZG.features('lakes')
    %
    % % COMPARING PLOT and PLOTM in same figure
    % fig=figure;
    %
    % % PLOT FEATURE ON "NORMAL" PLOT AXES
    % % - SET UP THE AXES
    % ax1=subplot(1,2,1);
    % ylim(ax1,bounds2(ZG.primeCatalog.Y));
    % xlim(ax1,bounds2(ZG.primeCatalog.X));
    % title(ax1,'normal plot')
    %
    % % PLOT FEATURE ON "MAP" PLOT AXES
    % % - SET UP THE AXES
    % ax2=subplot(1,2,2);
    % ax2m=axesm('lambert','MapLatLimit',bounds2(ZG.primeCatalog.Latitude),...
    %  'MapLonLimit',bounds2(ZG.primeCatalog.Longitude));
    % setm(ax2m,'MLineLocation',.25, 'PLineLocation',.25); % degrees for grid lines
    % setm(ax2m,'Grid','on')
    % setm(ax2m,'MeridianLabel','on','ParallelLabel','on')
    % setm(ax2m,'PLabelLocation',.5,'MLabelLocation',.5)  % degrees for labeling
    % setm(ax2m,'LabelFormat','signed','LabelUnits','dms') % display as +7˚ 00' 00"
    % title(ax2m,'lambert plot')
    %
    % lakes.plot(ax2m) % plot into map
    % lakes.plot(ax1) % plot onto "normal" axes

    properties(Transient, Hidden)
        Loadfn function_handle = @do_nothing % used to load/import this feature's data
        Savefn function_handle = @do_nothing % used to save/export this feature's data
    end
    properties
        Name (1,:) char        % name of this feature
        PlottingDefaults struct  % properties used on this layer
        Value struct           % raw values to plot, as struct, class, or Table containing a "Longitude" and "Latitude" field
        ParentAxis matlab.graphics.axis.Axes % handle to a parent axis. assigned when this is plotted
        MenuToggle matlab.ui.container.Menu % handle to the uimenu used to show/hide this layer
        WasLoaded logical = false;
    end
    properties(Dependent)
        Handle              % handle to this layer
        Longitude           % vector of Longitudes [deg] (to be plotted on X)
        Latitude            % vector of Latitudes [deg] (to be plotted on Y)
        Depth               % vector of Depths [km] (to be plotted on -Z)
        Names
    end

    methods
        function obj= MapFeature(name, load_fn, save_fn, plot_defaults)
            % create a MapFeature
            % obj= MAPFEATURE(name, load_fn, save_fn, plot_defaults)

            assert(isfield(plot_defaults,'Tag'));
            obj.Name = name;
            if ~isempty(load_fn)
                obj.Loadfn = load_fn;
            end

            if ~isempty(save_fn)
                obj.Savefn = save_fn;
            end

            obj.PlottingDefaults = plot_defaults;
        end

        function obj=copyobj(orig,AX)
            % COPYOBJ copies a feature into a specified axes
            % feature.COPYOBJ(AX) where AX is the destination axes
            %
            % obj=feature.COPYOBJ(...) returns a handle to the copied Mapfeature.
            %
            % see also copyobj
            obj=MapFeature(orig.Name, orig.Loadfn, orig.Savefn, orig.PlottingDefaults);
            obj.Value=orig.Value;
            obj.WasLoaded=orig.WasLoaded;
            obj.plot(AX);
        end

        function h =get.Handle(obj)
            % get.HANDLE get the handle to this feature's layer
            % if this layer hasn't been plotted yet, or has been deleted, then
            % this returns []

            % make sure Parent Axis is still valid
            if ~isempty(obj.ParentAxis) && ~isvalid(obj.ParentAxis)
                obj.ParentAxis = [];
            end

            if ~isempty(obj.ParentAxis)
                h = findobj(obj.ParentAxis, 'Tag', obj.PlottingDefaults.Tag);
            else
                h=[];
                %error('This feature %s is not associated with a valid parent',obj.Name);
                % h = findobj(groot, 'Tag', obj.PlottingDefaults.Tag);
            end
        end

        function lons = get.Longitude(obj)
            lons = obj.Value.Longitude;
        end
        function lats = get.Latitude(obj)
            lats = obj.Value.Latitude;
        end
        function depths = get.Depth(obj)
            depths = obj.Value.Depth;
        end

        function names = get.Names(obj)
            try
                names = obj.Value.Names;
            catch ME
                names = {'no name'};
            end
        end

        function layer=plot(obj,ax)
            % plot this layer
            % will delete layer if it exists
            % note features will only plot the subset of features within the
            % currently visible axes
            %
            % see also refreshPlot

            assert(obj.WasLoaded,'load before plotting!')

            if ~exist('ax','var')
                ax = obj.ParentAxis;
            else
                obj.ParentAxis = ax;
            end

            if isempty(ax) || ~isvalid(ax)
                error('Feature "%s" ->plot has no associated axis',obj.Name);
            end

            if has_toolbox('Mapping Toolbox') && ismap(ax)
                obj.plotm(ax);
                return
            end
            % clear the existing layer
            h = obj.Handle;
            if ~isempty(h)
                delete(h);
            end
            if isempty(obj.Value)
                return % nothing to plot
            end

            holdstatus = ishold(ax); ax.NextPlot='add';

            val = obj.getTrimmedData();
            layer=line(ax,'XData',val.Longitude, 'YData',val.Latitude,'ZData',val.Depth');
            ax.ZDir='reverse';
            %stackorder_menu(layer)

            if ~holdstatus; ax.NextPlot='replace'; end

            % set properties for this layer
            set(layer, obj.PlottingDefaults);
        end

        function layer=plotm(obj,ax)
            % plot this layer onto a map (Requires mapping toolbox)
            % will delete layer if it exists
            % note features will only plot the subset of features within the
            % currently visible axes
            %
            % see also refreshPlot

            if ~exist('ax','var')
                ax = obj.ParentAxis;
            else
                obj.ParentAxis = ax;
            end

            if isempty(ax) || ~isvalid(ax)
                error('Feature "%s" ->plot has no associated axis',obj.Name);
            end

            % clear the existing layer
            h = obj.Handle;
            if ~isempty(h)
                delete(h);
            end

            if isempty(obj.Value)
                return % nothing to plot
            end

            holdstatus = ishold(ax); ax.NextPlot='add';

            val = obj.getTrimmedData();
            layer=plotm(val.Latitude, val.Longitude); %not allowed to specify axes
            zdatam(layer, reshape(val.Depth,1,[]));
            daspectm('km');
            if ~holdstatus; ax.NextPlot='replace'; end

            % set properties for this layer
            set(layer, obj.PlottingDefaults);
        end

        function refreshPlot(obj)
            % refreshPlot updates the X,Y,Z values without deleting the layer
            %
            % refreshPlot useful for when the axis limits have changed or
            % when the values have changed.  The plotted data will be trimmed, which reduces
            % the plotting overhead.
            %
            % see also plot

            %find this layer
            layer=obj.Handle;
            if isempty(layer)
                disp([obj.Name,' plot did not exist']);
            end
            val = obj.getTrimmedData();
            % set properties for this layer
            layer.XData=val.Longitude;
            layer.YData=val.Latitude;
            layer.ZData=val.Depth;
            set(layer,obj.PlottingDefaults);
        end

        function load(obj,varargin)
            % load this feature
            % uses the Loadfn provided during Feature Construction
            % loadFunction is expected to return a table or struct containing fields for
            % 'Latitude', 'Longitude', and 'Depth' (things with ELEVATION would have negative depth)
            item = obj.Loadfn(varargin{:});
            obj.Value = toValueStruct(item);
            obj.WasLoaded = true;
        end

        function save(obj)
            % save this feature
            % uses the Savefn provided during Feature construction
            obj.Savefn(obj)
        end

        function changeDefaults(obj, propname_struct)
            %changeDefaults changes this layer's properties and applies the change
            obj.PlottingDefaults = copyfields(obj.PlottingDefaults, propname_struct);
            %{
            fn=fieldnames(propname_struct);
            for i=1:numel(fn)
                obj.PlottingDefaults.(fn{i}) = propname_struct.(fn{i});
            end
            %}
            set(obj.Handle,obj.PlottingDefaults);
        end

        function addToggleMenu(obj, parentH, ax)
            % addToggleMenu add menu that hides/shows this Feature
            % addToggleMenu(parent) adds a toggle menu subordinate to the parent uimenu

            if ~isempty(obj.MenuToggle) && isvalid(obj.MenuToggle)
                error('ToggleMenu for Feature "%s" already exists.',obj.Name);
            end

            obj.MenuToggle = uimenu(parentH,...
                'Label',['Hide ' obj.Name],...
                'MenuSelectedFcn', @(s,~)obj.toggle_showhide_menu(s, ax));
        end
        function toggle_showhide_menu(obj, src, ax, contingencyFunction)
            % switch the show/hide menu between the "Show" and "Hide" stat
            % obj.toggle_showhide_menu(src, contingencyFunction
            %   this is intended to be the Callback for the addToggleMenu.
            %
            % if the plot this toggles no longer exists, then
            % the user-provided contingencyFunction will be run, which might be able to get things
            % back on track.
            h=obj.Handle;
            watchon
            drawnow;
            if isempty(h)
                if exist('contingencyFunction','var')
                    contingencyFunction();
                end
                h=findobj(ax,'Tag', obj.PlottingDefaults.Tag);
                errordlg('missing a layer %s', obj.PlottingDefaults.Tag);
            end
            axis(obj.ParentAxis,'manual')
            washeld = ishold(obj.ParentAxis); hold(obj.ParentAxis,'on');
            if startsWith(src.Label,'Hide ')
                src.Label = ['Show ' obj.Name];
                h.Visible = 'off';
            else % assume it starts with 'Show '
                src.Label = ['Hide ' obj.Name];
                h.Visible = 'on';
            end
            if ~washeld, hold(obj.ParentAxis,'off');end
            watchoff
            drawnow;
        end
    end % methods

    methods(Access=protected)
        function val = getTrimmedData(obj)
            % trim data to parent axis limits
            % obj.getTrimmedData()
            %
            % gaps where data was removed are replaced with a single "nan" value
            % this reduces the data size, and keeps segments from connecting when
            % they shouldn't.

            ax = obj.ParentAxis;
            if ismap(ax)
                trimLat=getm(ax,'maplatlimit');
                trimLon=getm(ax,'maplonlimit');
            else
                trimLat=ylim(ax);
                trimLon=xlim(ax);
            end
            idx  = obj.Value.Longitude >= trimLon(1) & obj.Value.Longitude <= trimLon(2);
            idx = idx & obj.Value.Latitude >= trimLat(1) & obj.Value.Latitude <= trimLat(2);

            val=obj.Value;

            % index used to delete any nan's after the first (per gap)
            todel = [false; ~idx(1:end-1)] & [false; ~idx(2:end)];
            if ~isfield(val,'Depth')
                val.Depth=zeros(size(val.Latitude));
            end
            % replace out-of-bounds values with NaN
            val.Longitude(~idx) = nan;
            val.Latitude(~idx) = nan;
            val.Depth(~idx)=nan;

            % reduce dataset by removing all nan's EXCEPT for one (per gap)
            val.Longitude(todel) = [];
            val.Latitude(todel) = [];
            val.Depth(todel)=[];
        end

    end %protected methods

    methods(Static)
        function foreach(features,funcname,varargin)
            %FOREACH run a function on a Map or array full of MapFeature
            % FOREACH(features) where features is either a map or array of MapFeature
            % FOREACH(features,arg1,...) arguments to pass to the function
            if isa(features,'containers.Map')
                k=features.keys;
                for i=1:numel(k)
                    ftr=features(k{i});
                    ftr.(funcname)(varargin{:});
                end
            else
                % treat as an array
                for i=1:numel(features)
                    ftr=features(i);
                    ftr.(funcname)(varargin{:});
                end
            end
        end

        function foreach_waitbar(features,funcname,varargin)
            %FOREACH run a function on a Map or array full of MapFeature
            % FOREACH(features) where features is either a map or array of MapFeature
            % FOREACH(features,arg1,...) arguments to pass to the function

            h=waitbar(0,'Calculating features for map...');
            if isa(features,'containers.Map')
                k=features.keys;
                for i=1:numel(k)
                    h.Name=[funcname ' : ', k{i}];
                    ftr=features(k{i});
                    ftr.(funcname)(varargin{:});
                    waitbar(i/numel(k),h);
                end
            else
                % treat as an array
                k = numel(features);
                for i=1:k
                    h.Name=[funcname ' : ', features(i).Name];
                    ftr=features(i);
                    ftr.(funcname)(varargin{:});
                    waitbar(i/k,h);
                end
            end
            delete(h)
        end

    end %static methods
end

function [finder, namer] = how_to_deal_with(item)
    switch class(item)
        case 'table'
            xx = item.Properties.VariableNames;
            finder = @(item, fn) startsWith(item.Properties.VariableNames, fn,'IgnoreCase',true);
            namer = @(idx) xx{idx};
            %accessor = @(item,fn) item.(fn)(:);
        case 'struct'
            xx = fieldnames(item);
            finder = @(item, fn) startsWith(xx, fn, 'IgnoreCase',true);
            fidx = fieldnameFromIndex(xx);
            namer = @(idx) fidx(idx);% fieldnameFromIndex(fieldnames(item));
            %accessor = @(item,fn) item.(fn)(:);
        otherwise
            xx=properties(item);
            assert(~isempty(xx),'item is not a table, struct, or class');
            finder = @(item, fn) startsWith(xx, fn, 'IgnoreCase',true);

            fidx = fieldnameFromIndex(xx);
            namer = @(idx) fidx(idx);% fieldnameFromIndex(fieldnames(item));
            %accessor = @(item,fn) item.(fn)(:);
    end
    function fn=fieldnameFromIndex(flds)
        fn = @(idx) flds{idx};
    end
end

function value = toValueStruct(data)
    % set.Value does the painful work of importing data form a variety of data types
    % result will have Latitude, Longitude, probably Depth, and possibly Names

    value=struct('Longitude',[],'Latitude',[],'Depth',[],'Names','');

    if isnumeric(data)
        % do number crunching

        if isempty(data)
            return
        end

        if size(data,2)>3 || size(data,2) < 2
            error('Feature "%s" expected 2-3 columns of data, or something with Latitude and Longitude fields',obj.Name); %#ok<MCSUP>
        end

        % assume longitude in first column
        if max(abs(data(:,2))) > 90.000001
            error('Feature "%s" expected latitudes in range of -90 to 90 in second column',obj.Name); %#ok<MCSUP>
        end

        if max(abs(data(:,1))) > 180.000001
            error('Feature "%s" expected longitudes in -180 to 180 range [%f]',obj.Name, max(abs(data(:,1)))); %#ok<MCSUP>
        end

        value.Longitude = data(:,1);
        value.Latitude = data(:,2);

        if size(data,2)==3
            value.Depth = data(:,3);
        else
            value.Depth = zeros(size(value.Longitude));
        end

        return
    end

    % data is in a class, struct, or table

    [searchFor, nameFromIdx] = how_to_deal_with(data);
    x = searchFor(data,'Longitude');
    y = searchFor(data,'Latitude');

    % assume parallel structure between x & y, so check x only and treat y accordingly

    if sum(x)==1
        % all is ok
    elseif ~any(x)
        x=searchFor(data,'Lon');
        y=searchFor(data,'Lat');
    end

    if ~any(x) || ~any(y)
        error('Feature %s Looking for data with a "Longitude" and "Latitude" fields', obj.Name); %#ok<MCSUP>
    elseif sum(x)>1 || sum(y)>1
        error('Feature %s Found too many "Longitude"-ish and "Latitude"-ish fields', obj.Name); %#ok<MCSUP>
    end

    lon_field = nameFromIdx(x);
    lat_field = nameFromIdx(y);

    d = searchFor(data,'Depth');
    el = searchFor(data,'Elev');

    if any(d)
        dep_field = nameFromIdx(d);
        value.Longitude = [data.(lon_field)];
        value.Latitude = [data.(lat_field)];
        value.Depth = [data.(dep_field)];
    elseif any(el)
        dep_field = nameFromIdx(el);
        value.Longitude = [data.(lon_field)];
        value.Latitude = [data.(lat_field)];
        value.Depth = -[data.(dep_field)];
    else
        value.Longitude = [data.(lon_field)];
        value.Latitude = [data.(lat_field)];
        value.Depth = zeros(size(value.Latitude));
    end

    value.Longitude = value.Longitude(:);
    value.Latitude = value.Latitude(:);
    value.Depth = value.Depth(:);

    nm = searchFor(data,'Name');
    if any(nm)
        value.Names = data.(nameFromIdx(nm));
    end

end

author: CelsoReyes


text:
function mia_Zpixval(arg1, arg2)
% mia_Zpixval display information about image pixels
% in coronal image in the case of 3Dcursor mode
%
% Matlab library function for mia_gui utility.
% University of Debrecen, PET Center/LB 2003

%   mia_Zpixval Display information about image pixels.
%   PIXVAL ON turns on interactive display of information about image pixels
%   in the current figure. PIXVAL installs a black bar at the bottom of the
%   figure which displays the (x,y) coordinates for whatever pixel the
%   cursor is currently over, and the color information for that pixel. If
%   the image is binary or intensity, the color information is a single
%   intensity value. If the image is indexed or RGB, the color information
%   is an RGB triplet. The values displayed are the actual data values,
%   regardless of the class of the image array, or whether the data is in
%   normal image range.
%
%   If you click on the image and hold down the mouse button while you move
%   the cursor, PIXVAL also displays the Euclidean distance between the
%   point you clicked on and current cursor location. PIXVAL draws a line
%   between these points to indicate the distance being measured. When you
%   release the mouse button, the line and the distance display disappear.
%
%   You can move the display bar by clicking on it and dragging it to
%   another place in the figure.
%
%   PIXVAL OFF turns interactive display off in the current figure. You can
%   also turn off the display by clicking the button on the right side of
%   the display bar.
%
%   PIXVAL toggles interactive display on or off in the current figure.
%
%   PIXVAL(FIG,OPTION) applies the PIXVAL command to the figure specified
%   by FIG. OPTION is string containing 'on' or 'off'.
%
%   See also IMPIXEL, IMPROFILE.

%   Copyright 1993-2001 The MathWorks, Inc.
%   $Revision: 1.19 $  $Date: 2001/01/18 15:30:19 $

error(nargchk(0,2,nargin));

if nargin==0
   arg1 = gcf;
end

if ischar(arg1) % execute a callback
   switch lower(arg1)
   case 'pixvalmotionfcn'
      PixvalMotionFcn;
   case 'buttondownonimage'
      ButtonDownOnImage;
   case 'backtonormalpixvaldisplay'
      BackToNormalPixvalDisplay;
   case 'movedisplaybar'
      MoveDisplayBar;
   case 'on'
      mia_Zpixval(gcf, 'on');
   case 'off'
      mia_Zpixval(gcf, 'off');
   case 'deletedisplaybar'
      DeleteDisplayBar;
   otherwise
      error(['Unknown PIXVAL command or callback: ' arg1 '.']);
   end

elseif ishandle(arg1)
   images = findobj(arg1, 'type', 'image');
   if isempty(images)
      return
   end

   figureHandle = get(get(images(1), 'parent'), 'parent');

   if nargin>1   % mia_Zpixval(handle, 'on|off')
      if ischar(arg2)
         if strcmp(lower(arg2), 'off')
            action = 'destroy';
         elseif strcmp(lower(arg2), 'on')
            action = 'create';
         else
            error(['Unknown PIXVAL input string: ' arg2]);
         end
      else
         error('Invalid input arguments');
      end
   else  % mia_Zpixval(handle) or mia_Zpixval
      % Toggle the state
      displayBar = findobj(figureHandle, 'tag', 'pixel value display bar');
      if isempty(displayBar)
         action = 'create';
      else
         action = 'destroy';
      end
   end

   % Put things in the Images' UserData so that pixel value display works
   switch action
   case 'destroy'
      displayBar = findobj(figureHandle, 'tag', 'pixel value display bar');
      if ~isempty(displayBar)
         DeleteDisplayBar(displayBar);
      end

   case 'create'
      otherDisplayBar = findobj(figureHandle, 'tag', 'pixel value display bar');
      if ~isempty(otherDisplayBar)
         % mia_Zpixval is already ON for this figure.
         return
      end

      % Save the interactive state of the figure. UICLEARMODE now
      % does everything that UISUSPEND used to do. It calls
      % SCRIBECLEARMODE to take care of SCRIBE.  We WILL be
      % restoring the state - this is a change.  UICLEARMODE also
      % registers a way to disable mia_Zpixval
      dbud.oldFigureUIState = uiclearmode(figureHandle,'mia_Zpixval',figureHandle,'off');
      AxisHandle = findobj(figureHandle, 'type', 'axes');
      dbud.xLim = get(AxisHandle,'XLim');% for zoom mode
      dbud.yLim = get(AxisHandle,'YLim');% for zoom mode
      dbud.old_pt = 0;% for zoom mode

      % Make sure the SCRIBE toolbar doesn't go away
      plotedit(figureHandle,'locktoolbarvisibility');

      % Make sure that the Motion function doesn't interrupt itself
      set(figureHandle, 'Interruptible', 'off', 'busyaction', 'queue');

      % Make sure that the Image's Button Down & Up functions will queue
      set(images, 'Interruptible', 'off', 'BusyAction', 'Queue');

      % dbud is the UserData structure which is going into the display bar
      dbud.figureHandle = figureHandle;
      dbud.displayMode = 'normal';

      figPos = get(dbud.figureHandle, 'position');
      tempStr = sprintf(' %g, %g = %6.4f,%6.4f,%6.4f  dist = %3.3f', ...
        -0.9999,-0.9999,pi/10,pi/10,pi/10,pi*100);

      % Calculate the position vector of the display bar
      tempTxt = uicontrol('Parent', dbud.figureHandle, 'horiz', 'left', ...
         'Style', 'text', 'Units', 'pixels', 'visible', 'off', 'fontname', ...
         'FixedWidth','String', tempStr, 'position', [0 0 figPos(3) 20]);
      extent= get(tempTxt, 'extent');
      txtHt = extent(4);
      txtWid = extent(3)+txtHt; % Leave room for the close button
      delete(tempTxt);

      % Create position vectors for Text, Frame, and Close Button
      pos = [0 0 (min(txtWid,figPos(3))-txtHt)*1/2 txtHt];
      btnSide = pos(4)-4;  % In pixels
      btnPos = [pos(1)+pos(3)+2 pos(2)+2 btnSide btnSide];
      frmPos = [pos(1)+pos(3) 0 txtHt txtHt];

      % Create a frame for the display bar. I do this because of a bug in
      % MATLAB 5.2 where the text field pops to the front of the button each
      % time it is updated.  It should stay behind in which case, all we would
      % need is the text uicontrol and the button.  We can take the frame out
      % if the bug gets fixed.  Make sure the frame doesn't fall behind the
      % text control, otherwise you can't execute the text control's buttondown
      % function (another MATLAB 5.2 bug).

      dbud.DisplayFrame = uicontrol('Parent', figureHandle, ...
         'style', 'frame', ...
         'Units', 'pixels', ...
         'Position', frmPos, ...
         'Background', [0 0 0], ...
         'BusyAction', 'queue', ...
         'Interruptible', 'off');

      % Create the display bar
      dbud.buttonDownImage = 0;  % Image 0 never exists
      DisplayBar = uicontrol('Parent', figureHandle, ...
         'Style','text', ...
         'Units','pixels', ...
         'Position',pos, ...
         'Foreground', [1 1 .5], ...
         'Background', [0 0 0], ...
         'Horiz','left', ...
         'Tag', 'pixel value display bar', ...
         'String','', ...
         'fontname', 'FixedWidth', ...
         'BusyAction', 'queue', ...
         'enable', 'inactive', ...
         'Interruptible', 'off');
%          'ButtonDownFcn', 'mia_Zpixval(''MoveDisplayBar'')', ...
%          'DeleteFcn', 'mia_Zpixval(''DeleteDisplayBar'')', ...

      % Create the close button
      dbud.CloseButton = uicontrol('Parent', figureHandle, ...
         'style', 'pushbutton', ...
         'units', 'pixels', ...
         'position', btnPos, ...
         'BusyAction', 'queue', ...
         'Interruptible', 'off');
%          'String', 'X', ...
%          'Callback', 'mia_Zpixval(''DeleteDisplayBar'')', ...
%          'DeleteFcn', 'mia_Zpixval(''DeleteDisplayBar'')', ...

      % Register the motion function and button up function
      set(figureHandle, 'WindowButtonMotionFcn',...
         'mia_Zpixval(''PixvalMotionFcn'')')

      set(DisplayBar, 'UserData', dbud);
      PixvalMotionFcn(DisplayBar);
   end

else
   error('Invalid input argument.')
end

%%%
%%%  Sub-function - PixvalMotionFcn
%%%

function PixvalMotionFcn(displayBar)

if nargin < 1
   displayBar = findobj(gcbf, 'Tag', 'pixel value display bar');
end

if isempty(displayBar)
   % Pixval is in a half-broken state.  Another function (like zoom) has
   % restored pixval callbacks and PIXVAL has already been uninstalled.
   % Call uisuspend to gracefully get rid of the callbacks.

   % Note 7/21/98 - Since we are now using UICLEARMODE instead of
   % UISUSPEND, I think that we should never get into this
   % state.  It will only happen if a user writes a function
   % which calls UIRESTORE without ever calling UICLEARMDOE or
   % SCRIBECLEARMODE.  We'll leave this code here just to be safe.
   uisuspend(gcbf);
   return
end

dbud = get(displayBar, 'UserData');
pt = get(dbud.figureHandle, 'CurrentPoint');
x = pt(1,1); y = pt(1,2);

if strcmp(dbud.displayMode, 'normal')
   % See if we're above the displayBar
   dbpos = get(displayBar, 'Position');
   left   = dbpos(1);
   bottom = dbpos(2);
   width  = dbpos(3);
   height = dbpos(4);
   % On the line below, we look to see if x<=left+width+height because
   % there is a frame next to the text object, see note above about the
   % MATLAB 5.2 bug we're working around
   if x>=left & x<=left+width+height & y>=bottom & y<=bottom+height
      % We're hovering above the display bar

      % Look to see if we are above the Close button
      cp = get(dbud.CloseButton, 'Position');
      if  x>=cp(1) & x<=cp(1)+cp(3) & y>=cp(2) & y<=cp(2)+cp(4)
         if isfield(dbud, 'oldPointer') % Switch back to the default pointer
            set(dbud.figureHandle, 'Pointer', dbud.oldPointer, ...
               'PointerShapeCData', dbud.oldPointerShapeCData, ...
               'PointerShapeHotSpot', dbud.oldPointerShapeHotSpot);
            dbud = rmfield(dbud, {'oldPointer', ...
                  'oldPointerShapeCData', 'oldPointerShapeHotSpot'});
         end
      else
         % Save the default pointer if there is one
         if ~isfield(dbud, 'oldPointer') % We still have the default pointer
            dbud.oldPointer = get(dbud.figureHandle, 'Pointer');
            dbud.oldPointerShapeCData = get(dbud.figureHandle, 'PointerShapeCData');
            dbud.oldPointerShapeHotSpot = get(dbud.figureHandle, 'PointerShapeHotSpot');
         end
         setptr(dbud.figureHandle, 'hand');
      end
   else
      % We're not hovering over the display bar
      [imageHandle,imageType,img,x,y] = OverImage(dbud.figureHandle);
      if imageHandle ~= 0
         % Save old pointer and get a new one (use Custom pointer for non-Windows machine)
         if ~isfield(dbud, 'oldPointer') % We still have the default pointer
            dbud.oldPointer = get(dbud.figureHandle, 'Pointer');
            dbud.oldPointerShapeCData = get(dbud.figureHandle, 'PointerShapeCData');
            dbud.oldPointerShapeHotSpot = get(dbud.figureHandle, 'PointerShapeHotSpot');
         end
         if strcmp(computer, 'PCWIN')
            set(dbud.figureHandle, 'Pointer', 'cross');
         else
            [pointerShape, pointerHotSpot] = CreatePointer;
            set(dbud.figureHandle, 'Pointer', 'custom', ...
               'PointerShapeCData', pointerShape, ...
               'PointerShapeHotSpot', pointerHotSpot);
         end

         % Update the Pixel Value display
         UpdatePixelValues(imageHandle, imageType, displayBar,img,x,y );
      else
         if isfield(dbud, 'oldPointer') % Switch back to the default pointer
            set(dbud.figureHandle, 'Pointer', dbud.oldPointer, ...
               'PointerShapeCData', dbud.oldPointerShapeCData, ...
               'PointerShapeHotSpot', dbud.oldPointerShapeHotSpot);
            dbud = rmfield(dbud, {'oldPointer', ...
                  'oldPointerShapeCData', 'oldPointerShapeHotSpot'});
            set(displayBar, 'String', '');
         end
      end
   end
elseif strcmp(dbud.displayMode, 'distance')
   % If we're in distance mode and in another image, clean up a bit.
   [imageHandle,imageType,img,x,y]= OverImage(dbud.figureHandle);
   if imageHandle~=0
      if imageHandle==dbud.buttonDownImage
         UpdatePixelValues(imageHandle, imageType, displayBar,img,x,y);
      end
   end
end
set(displayBar, 'UserData', dbud);

%%%
%%%  Sub-function - OverImage
%%%

function [imageHandle,imageType,img,x,y] = OverImage(figHandle)
% Return the index of which image we are over, and return a 0 if we
% aren't above an image.

images = findobj(figHandle, 'type', 'image');
if isempty(images)
   imageHandle=0; imageType=''; img=[]; x=0; y=0;
   return
end
% Make sure that the Image's Button Down & Up functions will queue
set(images, 'ButtonDownFcn', 'mia_Zpixval(''ButtonDownOnImage'')', ...
   'Interruptible', 'off', 'BusyAction', 'Queue');
axHandles = get(images, {'Parent'});
axPositions = get([axHandles{:}], {'Position'});
axCurPt = get([axHandles{:}], {'CurrentPoint'});

% Loop over the axes, see if we are above any of them
imageHandle = 0;
for k=1:length(axHandles)
   XLim = get(axHandles{k}, 'XLim');
   YLim = get(axHandles{k}, 'YLim');
   pt = axCurPt{k};
   x = pt(1,1); y = pt(1,2);
   if x>=XLim(1) & x<=XLim(2) & y>=YLim(1) & y<=YLim(2)
      imageHandle = images(k);
      break;
   end
end

% Figure out image type
if imageHandle ~= 0
   [img,flag] = getimage(imageHandle);

   switch flag
   case 1
      imageType = 'indexed';
   case {2,3}  % Grayscale or binary
      imageType = 'intensity';
   case 4
      imageType = 'rgb';
   otherwise
      error(['Invalid image, GETIMAGE returned flag = ' flag '.']);
   end
else
   imageHandle=0; imageType=''; img=[]; x=0; y=0;
end

%%%
%%%  Sub-function - UpdatePixelValues
%%%

function UpdatePixelValues(imageHandle, imageType, displayBar, img, x, y)
%   This is the motion callback for when we are displaying
%   pixels.  Either we are in automatic display mode and the
%   mouse pointer is moving around or we are in normal mode
%   and there has been a button-down but not yet a button up.
%   I get the current point and update the string.

global old_pt_zoom_z x0_pan_z;%temp for zoom and pan

dbud = get(displayBar, 'UserData');

[rows, cols, colors] = size(img);
y = min(y,rows);% y > rows happens in some case /LB
rp = axes2pix(rows, get(imageHandle,'YData'),y);
cp = axes2pix(cols, get(imageHandle,'XData'),x);
r = min(rows, max(1, round(rp)));
c = min(cols, max(1, round(cp)));

if strcmp(imageType,'indexed')
   map=get(dbud.figureHandle, 'Colormap');
   idx = img(r,c);
   if ~isa(idx, 'double'),
      idx = double(idx)+1;
   end
   idx = round(idx);
   if idx<=size(map,1)
      pixel = map(idx,:);
   else
      pixel = map(end,:);
   end
else
   pixel = double(img(r,c,:));
end

% figure out the new string
figureHandle = get(get(imageHandle, 'parent'), 'parent');
switch dbud.displayMode
case 'normal'   % Just display intensity information
	FigDataIn = get(figureHandle,'Userdata');
    ImVolume = get(FigDataIn.ImaHandlerX,'UserData');
    %x = ImVolume.X;
    %y = ImVolume.Y;
    %z = ImVolume.Z;
    %pixel(1) = ImVolume.PixInt;

	if isa(img, 'uint8')
     pixval_str = sprintf(' %g,%g,%g = %3d',round(x),round(y), round(ImVolume.Z),pixel(1));
	elseif isa(img, 'int16')
     pixval_str = sprintf(' %g,%g,%g = %5d',round(x),round(y), round(ImVolume.Z), pixel(1));
	else
     pixval_str = sprintf(' %g,%g,%g = %5.1f',round(x),round(y), round(ImVolume.Z),pixel(1));
	end
    set(displayBar, 'String', pixval_str, 'UserData', dbud);

case 'distance'
    mouseclick = get(gcf,'SelectionType');
    if strcmp(mouseclick,'normal')
		FigDataIn = get(figureHandle,'Userdata');
		fighandlerX = FigDataIn.ImaHandlerX;
		fighandlerY = FigDataIn.ImaHandlerY;
		FigData = FigDataIn.CData;

		set(fighandlerX,'CData',rot90(squeeze(FigData(:,round(cp),:,:))));
		set(fighandlerY,'CData',rot90(squeeze(FigData(round(rp),:,:))));

        ImVolume = get(FigDataIn.ImaHandlerX,'UserData');
        ImVolume.X = x;
        ImVolume.Y = y;
        ImVolume.PixInt = pixel(1);
        set(FigDataIn.ImaHandlerX,'UserData',ImVolume);

		if isa(img, 'uint8')
         pixval_str = sprintf(' %g,%g,%g = %3d',round(x),round(y), round(ImVolume.Z),pixel(1));
		elseif isa(img, 'int16')
         pixval_str = sprintf(' %g,%g,%g = %5d', round(x),round(y), round(ImVolume.Z), pixel(1));
		else
         pixval_str = sprintf(' %g,%g,%g = %5.1f',round(x),round(y), round(ImVolume.Z),pixel(1));
		end
        set(displayBar, 'String', pixval_str, 'UserData', dbud);
    elseif strcmp(mouseclick,'extend') % zoom mode
        CurrentAxes = findobj(figureHandle, 'type', 'axes');
        %set(CurrentAxes,'Units','pixel');
        new_pt = get(0,'PointerLocation');
        dy = (new_pt(2) - old_pt_zoom_z(2))/abs(old_pt_zoom_z(2))*5;
        zoomFactor = (1-dy);
        xLim=get(CurrentAxes,'XLim');
        yLim=get(CurrentAxes,'YLim');
     	xLimNew = [0 zoomFactor*diff(xLim)] + xLim(1) + (1-zoomFactor)*diff(xLim)/2;
        yLimNew = [0 zoomFactor*diff(yLim)] + yLim(1) + (1-zoomFactor)*diff(yLim)/2;
		set(CurrentAxes,'XLim',xLimNew,'YLim',yLimNew);
        old_pt_zoom_z  = new_pt;
    elseif strcmp(mouseclick,'alt') % pan mode
        currentPoint = get(figureHandle,'CurrentPoint');
        CurrentAxes = findobj(figureHandle, 'type', 'axes');
        set(CurrentAxes,'Units','pixel');
        dx = currentPoint - x0_pan_z;
        x0_pan_z = currentPoint;
        ap = get(CurrentAxes,'Position');
        xLim = get(CurrentAxes,'XLim');
        yLim = get(CurrentAxes,'YLim');
        set(CurrentAxes,'XLim',xLim-(diff(xLim)*dx(1)/ap(3)), ...
           'YLim',yLim+(diff(yLim)*dx(2)/ap(4)));
       set(CurrentAxes,'Units','normalized');
    elseif strcmp(mouseclick,'open')% restore the original image
        xLimNew = dbud.xLim;
        yLimNew = dbud.yLim;
        CurrentAxes = findobj(gcf, 'type', 'axes');
        set(CurrentAxes,'XLim',xLimNew,'YLim',yLimNew);
    end

end

%%%
%%%  Sub-function - ButtonDownOnImage
%%%

function ButtonDownOnImage

global old_pt_zoom_z x0_pan_z;%temp for zoom and pan operation

%imageHandle = gcbo;
currentHandle = gcbo;
if strcmp(get(currentHandle,'type'),'image')
    imageHandle = currentHandle;
else
    currentParentHandle = get(currentHandle,'Parent');
    currentChildrensHandles = get(currentParentHandle,'children');
    imageHandle = currentChildrensHandles(3);
end
figureHandle = get(get(imageHandle,'Parent'),'Parent');
displayBar = findobj(figureHandle, 'Tag', 'pixel value display bar');
dbud = get(displayBar, 'UserData');
axesHandle = get(imageHandle, 'Parent');

mouseclick = get(gcf,'SelectionType');
if strcmp(mouseclick,'normal')% case of distance measuring
	%
elseif strcmp(mouseclick,'alt') % pan mode
    setptr(figureHandle,'closedhand');
elseif strcmp(mouseclick,'extend') % zoom mode
    setptr(figureHandle,'glass');
end
%
% Turn on the PlotEdit options on X,Y slice images
%
FigDataIn = get(figureHandle,'Userdata');

%delete the positioning line
ImVolume = get(FigDataIn.ImaHandlerX,'UserData');
if ishandle(ImVolume.Xxline)
    delete(ImVolume.Zxline);
    delete(ImVolume.Zyline);
    delete(ImVolume.Yxline);
    delete(ImVolume.Yyline);
    delete(ImVolume.Xxline);
    delete(ImVolume.Xyline);
end
set(FigDataIn.ImaHandlerX,'UserData',ImVolume);

% Set the initial point (x0,y0)

%pt = get(axesHandle, 'CurrentPoint');
%dbud.x0 = pt(1,1);
%dbud.y0 = pt(1,2);
%dbud.line = line('Parent', axesHandle, ...
%  'erasemode', 'xor', ...
%   'color', [1 0 0], ...
%   'Xdata', [dbud.x0 dbud.x0], ...
%   'Ydata', [dbud.y0 dbud.y0]);
dbud.displayMode = 'distance';
old_pt_zoom_z = get(0,'PointerLocation');% for zoom mode
x0_pan_z = get(figureHandle,'CurrentPoint');% for pan mode
dbud.buttonDownImage = imageHandle;
set(displayBar, 'UserData', dbud);
set(dbud.figureHandle, 'WindowButtonUpFcn', 'mia_Zpixval(''BackToNormalPixvalDisplay'')');
PixvalMotionFcn(displayBar);

%%%
%%%  Sub-function - BackToNormalPixvalDisplay
%%%

function BackToNormalPixvalDisplay

displayBar = findobj(gcbo, 'Tag', 'pixel value display bar');
dbud = get(displayBar, 'UserData');
imageHandle = dbud.buttonDownImage;
%
% Turn off the PlotEdit options on X,Y slice images and
% write the last X,Y,Z coordinates to the FigDataIn struct
%
[imageHandleTmp,imageType,img,x,y]= OverImage(dbud.figureHandle);
figureHandle = get(get(imageHandle,'Parent'),'Parent');
FigDataIn = get(figureHandle,'Userdata');
axesHandleX = get(FigDataIn.ImaHandlerX, 'Parent');
axesHandleY = get(FigDataIn.ImaHandlerY, 'Parent');
axesHandleZ = get(FigDataIn.ImaHandlerZ, 'Parent');

%draw the positioning line
Xyrange = get(axesHandleX,'Ylim');
Xxrange = get(axesHandleX,'Xlim');
Yyrange = get(axesHandleY,'Ylim');
Yxrange = get(axesHandleY,'Xlim');
Zyrange = get(axesHandleZ,'Ylim');
Zxrange = get(axesHandleZ,'Xlim');

ImVolume = get(FigDataIn.ImaHandlerX,'UserData');
if ishandle(ImVolume.Xxline)
    delete(ImVolume.Xxline);
    delete(ImVolume.Xyline);
    delete(ImVolume.Yxline);
    delete(ImVolume.Yyline);
    delete(ImVolume.Zxline);
    delete(ImVolume.Zyline);
end

LineWidthCur = 2;
%lines in saggital slice
ImVolume.Xxline=line('Parent', axesHandleX,'color', [0 1 1],'EraseMode','xor', ....
    'LineWidth',LineWidthCur,'Xdata',[ImVolume.Y ImVolume.Y],'Ydata',[0 Xyrange(2)], ...
    'ButtonDownFcn','mia_Xpixval(''ButtonDownOnImage'')');
ImVolume.Xyline=line('Parent', axesHandleX,'color', [0 1 1],'EraseMode','xor', ...
'LineWidth',LineWidthCur,'Xdata',[0 Xxrange(2)],'Ydata', [ImVolume.Z ImVolume.Z], ...
    'ButtonDownFcn','mia_Xpixval(''ButtonDownOnImage'')');

%lines in axial slice
ImVolume.Zxline=line('Parent', axesHandleZ,'color', [0 1 1],'EraseMode','xor', ....
    'LineWidth',LineWidthCur,'Xdata',[ImVolume.X ImVolume.X],'Ydata',[0 Zyrange(2)], ...
    'ButtonDownFcn','mia_Zpixval(''ButtonDownOnImage'')');
ImVolume.Zyline=line('Parent', axesHandleZ,'color', [0 1 1],'EraseMode','xor', ...
    'LineWidth',LineWidthCur,'Xdata',[0 Zxrange(2)],'Ydata', [ImVolume.Y ImVolume.Y], ...
    'ButtonDownFcn','mia_Zpixval(''ButtonDownOnImage'')');

% set(FigDataIn.ImaHandlerX,'UserData',ImVolume);
%lines in coronal slice
ImVolume.Yxline=line('Parent', axesHandleY,'color', [0 1 1],'EraseMode','xor', ....
    'LineWidth',LineWidthCur,'Xdata',[ImVolume.X ImVolume.X],'Ydata',[0 Xyrange(2)], ...
    'ButtonDownFcn','mia_Ypixval(''ButtonDownOnImage'')');
ImVolume.Yyline=line('Parent', axesHandleY,'color', [0 1 1],'EraseMode','xor', ...
    'LineWidth',LineWidthCur,'Xdata',[0 Yxrange(2)],'Ydata', [ImVolume.Z ImVolume.Z], ...
    'ButtonDownFcn','mia_Ypixval(''ButtonDownOnImage'')');

set(FigDataIn.ImaHandlerX,'UserData',ImVolume);

%delete(dbud.line);
%dbud.line = [];
%dbud.x0 = []; dbud.y0 = [];
set(dbud.figureHandle, 'WindowButtonUpFcn', '');
dbud.displayMode = 'normal';
dbud.buttonDownImage = 0;
set(displayBar, 'UserData', dbud);
PixvalMotionFcn( displayBar);

%%%
%%%  Sub-function - MoveDisplayBar
%%%

function MoveDisplayBar

displayBar = gcbo;
dbud = get(displayBar, 'UserData');
oldWindowMotionFcn = get(dbud.figureHandle, 'WindowButtonMotionFcn');
set(dbud.figureHandle, 'WindowButtonMotionFcn', '');
oldPointer = get(dbud.figureHandle, 'Pointer');
oldPointerShapeCData = get(dbud.figureHandle, 'PointerShapeCData');
oldPointerShapeHotSpot = get(dbud.figureHandle, 'PointerShapeHotSpot');
setptr(dbud.figureHandle,'closedhand');

txtpos = get(displayBar, 'Position');
frmPos = get(dbud.DisplayFrame, 'Position');
position = txtpos + [0 0 frmPos(3) 0];
position2 = dragrect(position);
dx = position2(1)-position(1);
dy = position2(2)-position(2);

txtpos = txtpos + [dx dy 0 0];
set(displayBar, 'Position', txtpos);

frmPos = frmPos + [dx dy 0 0];
set(dbud.DisplayFrame, 'Position', frmPos);

btnPos = get(dbud.CloseButton, 'Position');
btnPos = btnPos + [dx dy 0 0];
set(dbud.CloseButton, 'Position', btnPos);

set(dbud.figureHandle, 'Pointer', oldPointer, ...
   'PointerShapeCData', oldPointerShapeCData, ...
   'PointerShapeHotSpot', oldPointerShapeHotSpot)
set(dbud.figureHandle, 'WindowButtonMotionFcn', oldWindowMotionFcn);

%%%
%%%  Sub-function - DeleteDisplayBar
%%%

function DeleteDisplayBar(displayBar)
%  Take apart the pixel value display - get rid of the text
%  uicontrol and clean up the image objects (remove their
%  UserData's and get rid of their ButtonDown & Delete Fcn's)

if nargin<1
   displayBar = findobj(gcbf, 'Tag', 'pixel value display bar');
end

if ~isempty(displayBar) % if it's empty, there's no work to do.
   dbud = get(displayBar, 'UserData');
   set(displayBar, 'DeleteFcn', '');  % Make sure there's no recursion
   if ishandle(displayBar)
      delete(displayBar);
   end
   if ishandle(dbud.CloseButton)
      delete(dbud.CloseButton);
   end
   if ishandle(dbud.DisplayFrame)
      delete(dbud.DisplayFrame);
   end

   % We are once again restoring the figure state, since
   % UIRESTORE also will reactivate the SCRIBE toolbar.
   uirestore(dbud.oldFigureUIState);

   % This is what we did for IPT 2.1/ML 5.2 :
   %   uisuspend(dbud.figureHandle);
end

%%%
%%%  Sub-function - CreatePointer
%%%

function [pointerShape, pointerHotSpot] = CreatePointer

pointerHotSpot = [8 8];
pointerShape = [ ...
      NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   1   1   1   1   1   1   2 NaN   2   1   1   1   1   1   1   1
   2   2   2   2   2   2   2 NaN   2   2   2   2   2   2   2   2
   NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN
   2   2   2   2   2   2   2 NaN   2   2   2   2   2   2   2   2
   1   1   1   1   1   1   2 NaN   2   1   1   1   1   1   1   1
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN   1   2 NaN   2   1 NaN NaN NaN NaN NaN NaN
   NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN];

author: Sable

repo: mcbench-benchmarks


text:
function [ vX ] = ProxL1NormSum( vY, paramGamma, paramB )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProxL1NormSum( vY, paramGamma, paramB )
%   Solving the Least Squares Problem with L1 Regualarization (LASOO) with
%   Linear Equality Constraints (Sum of Elements) - Prox Operator.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - paramGamma    -   Parameter Gamma - L2 Regularization Factor.
%                       Sets the coefficient of the L1 Term.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
%   - paramB        -   Parameter B - Equality Constarint Parameter.
%                       Sets the scalar constarint of the equality
%                       condition.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - vX            -   Output Vector.
%                       The vector whichi minizes teh objective function
%                       and its sum of elements equals to paramB.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Efficient Solvers for Sparse Subspace Clustering - https://arxiv.org/abs/1804.06291.
%   2.  https://math.stackexchange.com/a/2886715/33.
% Remarks:
%   1.  S
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     18/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

debugMode = OFF;

numElements = size(vY, 1);

vB      = sort([vY - paramGamma; vY + paramGamma], 'ascend');
iMin    = 1; %<! Bisection Lower Bound
iMax    = (2 * numElements) + 1; %<! Bisection Upper Bound
maxItr  = ceil(log2(iMax)) + 1;

if(debugMode == ON)
    vParamBeta = linspace(vB(1), vB(end), 1000);

    for ii = 1:length(vParamBeta)
        vZ(ii) = sum( ProxL1Norm(vY - vParamBeta(ii), paramGamma) ) - paramB;
    end
    for ii = 1:length(vB)
        vT(ii) = sum( ProxL1Norm(vY - vB(ii), paramGamma) ) - paramB;
    end

    figure();
    plot(vParamBeta, vZ);
    hold('on');
    plot(vB, vT, '*');
end

% Bisection
for ii = 1:maxItr
    if((iMax - iMin) <= 1)
        break;
    end
    idxJ = round((iMin + iMax) / 2);
    idxJ = min(max(idxJ, iMin + 1), iMax - 1);

    vX = ProxL1Norm(vY - vB(idxJ), paramGamma);
    if(sum(vX) > paramB)
        iMin = idxJ;
    else
        iMax = idxJ;
    end
end

if(debugMode == ON)
    plot(vB(iMin), sum( ProxL1Norm(vY - vB(iMin), paramGamma) ) - paramB, 'o');
    plot(vB(iMax), sum( ProxL1Norm(vY - vB(iMax), paramGamma) ) - paramB, 'o');
end

% Once the section is found there a closed form solution.
% Within the section [vB(iMin), vB(iMax)] the function has constant slope
% and for any paramBeta \in (vB(iMin), vB(iMax)) the function sign(vY -
% paramBeta) is constant.
% Hence sign(vY - paramBeta) .* max(abs(vY - paramBeta) - paramGamma), 0)
% equals to (Only at the support, where max(abs(vY - paramBeta) -
% paramGamma), 0) doesn't vanish) vY - paramBeta - paramGamam * sign().
% Looking when this sum equals to paramB happens:
% 1 / length(vSupport) * sumvY(vSupport) -

paramBeta   = (vB(iMin) + vB(iMax)) / 2;
vX          = ProxL1Norm(vY - paramBeta, paramGamma);
vS          = (vX ~= 0);
paramBeta   = (sum(vY(vS) - paramGamma * sign(vX(vS))) - paramB) / sum(vS);
vX          = ProxL1Norm(vY - paramBeta, paramGamma);

end

function [ vX ] = ProxL1Norm( vY, paramLambda )

vX = sign(vY) .* max(abs(vY) - paramLambda, 0);

end

author: RoyiAvital

repo: StackExchangeCodes


text:
function [ J, grad ] = cost_function ( x, e_conn, u, q, M, lam, c )

%*****************************************************************************80
%
%% COST_FUNCTION evaluate the cost function and its gradient.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard, John Burkardt, Catalin Trenchea, Clayton Webster
%
%  Parameters:
%
%    Input, real X(N_NODES,1), the nodes of the mesh.
%
%    Input, integer E_CONN(N_ELEMENTS,NEL_DOF),
%
%    Input, real U(N_NODES), the value of the finite element solution at
%    each node.
%
%    Input, real Q(N_NODES,1), the value of the control function at
%    each node of the mesh.
%
%    Input, real M(N_NODES,N_NODES), the mass matrix.
%    M(I,J) = Integral PHI(I) PHI(J).
%
%    Input, real LAM(N_NODES), the value of the adjoint variable at each node.
%
%    Input, real C(N_EQUATIONS), the value of the adjoint equation right hand
%    side at each internal node.
%
%    Output, real J, the cost function, 1/2 sqrt ( integral ( U - U_HAT )^2 ).
%
%    Output, real GRAD(N_NODES,1), the gradient of the cost function (?).
%
%  Local Parameters:
%
%    Local, real ALPHA, a weighting factor in the computation of GRAD.
%
%    Local, real GRAD_LOCAL(NEL_DOF), the local contribution to the gradient
%    at the nodes of a given element.
%
%    Local, real LAMX_G(N_GAUSS), the dLAMdX function at the Gauss points
%    in a given element.
%
%    Local, integer N_ELEMENTS, the number of elements.
%
%    Local, integer N_GAUSS, the number of Gauss points.
%
%    Local, integer N_NODES, the number of nodes.
%
%    Local, integer NEL_DOF, the number of degrees of freedom per element.
%
%    Local, integer NODES_LOCAL, the indices of the nodes in a given element.
%
%    Local, real P_X, the derivative of the basis function evaluated at
%    the Gauss points in a given element.
%
%    Local, real PHI, the basis function evaluated at
%    the Gauss abscissas in a given element.
%
%    Local, real QX_G(N_GAUSS), the value of dQdX at the Gauss points in a
%    given element.
%
%    Local, real R(N_GAUSS), the Gauss points.
%
%    Local, real UX_G(N_GAUSS), the dUdX function at the Gauss points in a
%    given element.
%
%    Local, real W(N_GAUSS), the Gauss weights.
%
%    Local, real W_G(N_GAUSS), the Gauss weights mapped to a given element.
%
%    Local, real X_G(N_GAUSS), the Gauss points mapped to a given element.
%
%    Local, real X_LOCAL(NEL_DOF), the nodes in a given element.
%
  [ n_nodes, n_dimensions ] = size ( x );
  [ n_elements, nel_dof ] = size ( e_conn );
  n_gauss = 3;
  [ r, w ] = oned_gauss ( n_gauss );

  J = 0.5 * sqrt ( c' * M(2:end-1,2:end-1) * c );
%
%  Compute the gradient.
%
  grad = zeros(n_nodes,1);

  alpha = 0.000003;

  for n_el = 1 : n_elements

    nodes_local = e_conn(n_el,:);
    x_local = x(nodes_local,:);
    [ x_g, w_g, phi, p_x ] = oned_shape ( x_local, r, w );

    ux_g = p_x * u(nodes_local);
    qx_g = p_x * q(nodes_local);
    lamx_g = p_x * lam(nodes_local);

    grad_local = alpha * oned_f_int ( qx_g,           p_x, w_g ) ...
                       - oned_f_int ( lamx_g .* ux_g, phi, w_g );
%
%  Assemble local contribution to gradient.
%
    for n_t = 1 : nel_dof
      n_test = nodes_local(n_t);
      grad(n_test) = grad(n_test) + grad_local(n_t);
    end

  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function varargout = quad2d(varargin)
%QUAD2D  Compute definite integral of a CHEBFUN2.
%   I = QUAD2D( F ) returns the definite integral of a CHEBFUN2
%   over its domain of definition.
%
%   I = QUAD2D(F, a, b, c, d) returns the definite integral of a CHEBFUN2
%   over the rectangle [a,b] x [c,d].
%
% See also INTEGRAL2, SUM2, INTEGRAL.

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

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

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
function doseV = applyIMRTCompression(params, doseV)
%"applyCompression"
%   Applies compression specified in the IM structure to the doseV vector
%   and returns it.
%
%   Based on code by JOD and CZ
%
%JRA 30 Aug 04
%
%Usage:
%   function doseV = applyCompression(IM, doseV)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

switch lower(params.ScatterMethod)
    case 'exponential'
        %Apply "exponential" compression.
        maxD = max(doseV);
        doseVLow = doseV;
        doseVLow(doseVLow > params.Scatter.Threshold*maxD) = 0;
        lowInd = find(doseVLow);

        if ~isempty(lowInd)
            [sortDose, ind] = sort(doseVLow(lowInd)); %%
            maxOfSorted = max(sortDose(:));
            normSortDose = sortDose/maxOfSorted;
            coins = rand(length(normSortDose), 1);
            keepers = find(normSortDose > coins);

            doseV(doseV < params.Scatter.Threshold*maxD) = 0;
            doseV(lowInd(ind(keepers))) = doseVLow(lowInd(ind(keepers)));% + evenDose;
        else
            doseV;
        end

    case 'random' %Also known as "probabilistic".
        maxD = max(doseV);
        Step = params.Scatter.RandomStep;
        doseVLow = doseV;
        doseVLow(doseVLow > params.Scatter.Threshold*maxD) = 0;
        lowInd = find(doseVLow);
        N = floor(length(lowInd)/Step);

        downInd = round(rand(N,1) * (Step-1) + 1);
        tmp = cumsum([0;ones(N-1,1)]*Step);
        downInd = downInd + tmp;
%         for p = 1 : N-1;
%             downInd(p) = lowInd(ceil(Step*rand) + Step*(p-1));
%         end
        if exist('doseVLowDown')
            clear doseVLowDown;
        end
        doseVLowDown  = zeros(size(doseV));
        f = 1:N-1;
%         for f = 1:N-1
            doseVLowDown(downInd(f)) = doseVLow(downInd(f));
%         end
        doseV(doseV < params.Scatter.Threshold*maxD) = 0;
        doseV = doseV + doseVLowDown;
    case 'threshold'
        maxD = max(doseV);
        doseV(doseV < params.Scatter.Threshold*maxD) = 0;
    otherwise
        error('Invalid compression method in IM.params.ScatterMethod.');
end

author: cerr

repo: CERR

save_path: github-repos/MATLAB/cerr-CERR


text:
%GETFG Query function for the Framegrabber.
%
%SYNTAX parvalue = getfg(devinfo, 'parname');
%
%DESCRIPTION This function will let the user get a
%parameter of DT framegrabber device.
%
%INPUT The device information structure (devinfo)
%of the opened framegrabber device and the parameter
%name (parname) and if applicable the field name
%(fieldname).
%
%REMARKS It will be verified if the first input
%argument is a valid device information structure and
%if the second input argument is a parameter name and
%if the third input argument is a fieldname
%(if applicable). If it's valid, it will try to get
%the device id from the device information structure.
%
%OUTPUT If the input isn't valid or the framegrabber
%can't get the parameter value of the chosen parameter
%name, some error message will be shown to the user.
%If everything's ok, it will return the parameter value
%(parvalue) of the chosen parameter name of the device.
%
%EXAMPLE To get the blacklevel which will be stored in
%the variable bl, and assuming that the variable m
%stores the device info of the current framegrabber, type:
%
%	bl = getfg(m,'blacklevel');

disp('Error: getfg not found.')