text:
function [sol,info] = solvebilevel(OuterConstraints,OuterObjective,InnerConstraints,InnerObjective,InnerVariables,options)
%SOLVEBILEVEL Simple global bilevel solver
%
%   min        OO(x,y)
%   subject to CO(x,y)>=0
%              y = arg min OI(x,y)
%              subject to CI(x,y)>=0
%
%   [DIAGNOSTIC,INFO] = SOLVEBILEVEL(CO, OO, CI, OI, y, options)
%
%   diagnostic : Struct with standard YALMIP diagnostics
%   info       : Bilevel solver specific information
%
%   Input
%      CO       : Outer constraints (linear elementwise)
%      OO       : Outer objective (convex quadratic)
%      CI       : Inner constraints (linear elementwise)
%      OI       : Inner objective (convex quadratic)
%      y        : Inner variables
%      options  : solver options from SDPSETTINGS.
%
%   The behaviour of the bilevel solver can be controlled
%   using the field 'bilevel' in SDPSETTINGS
%
%      bilevel.outersolver : Solver for outer problems with inner KKT removed
%      bilevel.innersolver : Solver for inner problem
%      bilevel.rootcut     : Number of cuts (based on complementary
%                            constraints) added in root (experimental)
%      bilevel.relgaptol   : Termination tolerance
%      bilevel.compslacktol: Tolerance for accepting complementary slackness
%      bilevel.feastol     : Tolerance for feasibility in outer problem
%
%
%   See also SDPVAR, SDPSETTINGS, SOLVESDP

% min f(x,y) s.t g(x,y)<0, y = argmin  [x;y]'*H*[x;y]+e'[x;y]+f, E[x;y]<d

if nargin<6
    options = sdpsettings;
elseif isempty(options)
    options = sdpsettings;
end

y = InnerVariables;

if ~isempty(InnerConstraints)
    if any(is(InnerConstraints,'sos2'))
        error('SOS2 structures not allowed in inner problem');
    end
end

% User wants to use fmincon, cplex or something like
if strcmp(options.bilevel.algorithm,'external')
    % Derive KKT conditions of inner problem, append with outer, and solve
    % using standard solver
    z = [depends(OuterConstraints) depends(OuterObjective) depends(InnerObjective) depends(InnerConstraints)];
    z = setdiff(z,depends(y));
    z = recover(unique(z));
    [K,details] = kkt(InnerConstraints,InnerObjective,z,options);
    Constraints = [K,OuterConstraints];
    options.solver = options.bilevel.outersolver;
    sol = solvesdp(Constraints,OuterObjective,options);
    info = [];
    return
end

% Export the inner model, and select solver
options.solver = options.bilevel.innersolver;
if isa(InnerObjective, 'double') || is(InnerObjective,'linear')
    [Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,InnerObjective,options,[],[],0);
elseif is(InnerObjective,'quadratic')
    % We have to be a bit careful about cases such as x'y. This is convex in
    % the inner problem, since x is constant there.
    % [Q,c,f,dummy,nonquadratic] = vecquaddecomp(InnerObjective);
    % Extract model for a fake quadratic model
    % [InnerConstraints,failure] = expandmodel(InnerConstraints,InnerObjective)
    %[Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,dummy'*diag(1+diag(Q{1}))*dummy+c{1}'*dummy,options,[],[],0);
   % toptions = options;
   % toptions.expandbilinear = 1;
    yy = recover(setdiff(depends(y),setdiff(depends(InnerObjective),depends(y))));
    [Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,yy'*yy+sum(recover(depends(InnerObjective))),options,[],[],0);
    [Q,c,f,dummy,nonquadratic] = vecquaddecomp(InnerObjective,recover(inner_p.used_variables));
    %[Imodel,Iax1,Iax2,inner_p] = export(InnerConstraints,InnerObjective,options,[],[],0);
    % Now plug in the real quadratic function
    if ~isequal(getvariables(dummy),inner_p.used_variables)
        error('This quadratic form is not supported yet. Please make feature request')
    else
        inner_p.Q = Q{1};
        inner_p.c = c{1};
        inner_p.f = f{1};
    end
else
    error('Only LPs or convex QPs allowed as inner problem');
end

% Modeling of inner problem might have lead to more decision variables in
% the inner problem. Append these
v1 = getvariables(y);
v2 = inner_p.used_variables(inner_p.extended_variables);
v3 = inner_p.used_variables(inner_p.aux_variables(:));
y = recover(unique([v1(:);v2(:);v3(:)]));

% Export the outer model, and select solver
options.solver = options.bilevel.outersolver;
options.bmibnb.diagonalize = 0;
[Omodel,Oax1,Oax2,outer_p] = export(OuterConstraints,OuterObjective,options,[],[],0);
if isstruct(Oax2)
   sol = Oax2;
   info = 2;
   return
end
% Export a joint model with KKT removed, to simplify some setup later
% [Auxmodel,Auxax1,Auxax2,outerinner_p] = export([OuterConstraints,InnerConstraints],OuterObjective+pi*InnerObjective,options,[],[],0);

if ~all(inner_p.variabletype==0) | ~isequal(inner_p.K.s,0) | ~isequal(inner_p.K.q,0)
    error('Only LPs or convex QPs allowed as inner problem');
end

if options.bilevel.rootcuts & (~(isequal(outer_p.K.s,0) & isequal(outer_p.K.q,0)))
    disp('Disjunctive cuts currently only supported when inner is a QP')
    options.bilevel.rootcuts = 0;
end

FRP0 = inner_p;

[merged_mt,merged_vt] = mergemonoms(inner_p,outer_p);

if ~isequal(inner_p.used_variables,outer_p.used_variables)
    invar  = inner_p.used_variables;
    outvar = outer_p.used_variables;

    binary_variables = unique([inner_p.used_variables(inner_p.binary_variables) outer_p.used_variables(outer_p.binary_variables)]);
    integer_variables = unique([inner_p.used_variables(inner_p.integer_variables) outer_p.used_variables(outer_p.integer_variables)]);
    semi_variables = unique([inner_p.used_variables(inner_p.semicont_variables) outer_p.used_variables(outer_p.semicont_variables)]);
    all_variables = unique([inner_p.used_variables outer_p.used_variables]);
    if ~isequal(all_variables,inner_p.used_variables )
        inner_p = pad(inner_p,all_variables);
        FRP0 = inner_p;
        FRP0.monomtable = speye(length(inner_p.c));
    end
    if ~isequal(all_variables,outer_p.used_variables )
        outer_p = pad(outer_p,all_variables);
    end
else
    binary_variables = unique([inner_p.used_variables(inner_p.binary_variables) outer_p.used_variables(outer_p.binary_variables)]);
    integer_variables = unique([inner_p.used_variables(inner_p.integer_variables) outer_p.used_variables(outer_p.integer_variables)]);
    semi_variables = unique([inner_p.used_variables(inner_p.semicont_variables) outer_p.used_variables(outer_p.semicont_variables)]);
    all_variables = inner_p.used_variables;
end

outer_p.monomtable   = merged_mt;
outer_p.variabletype = merged_vt;
inner_p.variabletype = merged_vt;
inner_p.monomtable   = merged_vt;
% Index to inner variables
for i = 1:length(y)
    y_var(i) = find(all_variables == getvariables(y(i)));
end

% Index to outer variables
x_var = setdiff(1:length(all_variables),y_var);

% Index to binary variables
bin_var = [];
for i = 1:length(binary_variables)
    bin_var(i) = find(all_variables == binary_variables(i));
end
int_var = [];
for i = 1:length(integer_variables)
    int_var(i) = find(all_variables == integer_variables(i));
end
semi_var = [];
for i = 1:length(semi_variables)
    semi_var(i) = find(all_variables == semi_variables(i));
end

if ~isempty(intersect(y_var,bin_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be binary)');
end
if ~isempty(intersect(y_var,int_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be integer)');
end
if ~isempty(intersect(y_var,semi_var))
   error('Only LPs or convex QPs allowed as inner problem (inner variables can not be semi-continuous)');
end
inner_p.binary_variables = bin_var;
outer_p.binary_variables = bin_var;
inner_p.integer_variables = int_var;
outer_p.integer_variables = int_var;
inner_p.semicont_variables = semi_var;
outer_p.semicont_variables = semi_var;

% Number of inequalities in inner model = #bounded dual variables
ninequalities = inner_p.K.l;
nequalities   = inner_p.K.f;

% Add dual related to inequalities in inner model to the model
dual_var = length(all_variables)+1:length(all_variables)+ninequalities;
%dual_var = length(inner_p.c)+1:length(inner_p.c)+ninequalities;

% Add dual related to inequalities in inner model to the model
eqdual_var = dual_var(end)+1:dual_var(end)+inner_p.K.f;

% No cost of duals in outer objective
p = outer_p;
if ~isempty(dual_var)
    p.c(dual_var(end))=0;
    p.Q(dual_var(end),dual_var(end)) = sparse(0);
end

if ~isempty(eqdual_var)
    p.c(eqdual_var(end))=0;
    p.Q(eqdual_var(end),eqdual_var(end)) = sparse(0);
end

% Structure of the constraints
%
%  Stationary equalities
%  Outer equalities
%  Inner equalities
%  Inner LP inequalities
%  Duals positive
%  Outer inequalities (LP, SOCP, SDP)

% Add stationarity to outer model
stationary = [inner_p.c(y_var) 2*inner_p.Q(y_var,:)];
if length(dual_var)>0
    stationary = [stationary -inner_p.F_struc(1+inner_p.K.f:inner_p.K.f+inner_p.K.l,1+y_var)'];
end
if length(eqdual_var)>0
    stationary = [stationary  -inner_p.F_struc(1:inner_p.K.f,1+y_var)'];
end

p.F_struc = [stationary;p.F_struc spalloc(size(p.F_struc,1),length(dual_var) + length(eqdual_var),0)];
p.K.f = p.K.f + length(y_var);

% Add dual>0 to outer model
p.F_struc = [p.F_struc(1:p.K.f,:);spalloc(ninequalities,length(x_var)+length(y_var)+1,0) speye(ninequalities) spalloc(ninequalities,nequalities,0);p.F_struc(1+p.K.f:end,:)];
p.K.l = p.K.l + ninequalities;

% Add inner level constraints to outer model
p.F_struc = [p.F_struc(1:p.K.f,:);inner_p.F_struc spalloc(ninequalities+nequalities,ninequalities+nequalities,0);p.F_struc(1+p.K.f:end,:)];
p.K.f = p.K.f + inner_p.K.f;
p.K.l = p.K.l + inner_p.K.l;
slack_index = p.K.f+1:+p.K.f+ninequalities;

%p.lb = outerinner_p.lb;
%p.ub = outerinner_p.ub;
p.lb(dual_var) = 0;
p.ub(dual_var) =  inf;
p.lb(eqdual_var) = -inf;
p.ub(eqdual_var) =  inf;
p.x0 = [];

%p.variabletype = outerinner_p.variabletype;
%p.monomtable   = outerinner_p.monomtable;
%p.evalMap = outerinner_p.evalMap;
%p.evalVariables = outerinner_p.evalVariables;
for i = 1:length(dual_var)
    p.monomtable(dual_var(i),dual_var(i))=1;
    p.variabletype(dual_var(i)) = 0;
end
for i = 1:length(eqdual_var)
    p.monomtable(eqdual_var(i),eqdual_var(i))=1;
    p.variabletype(eqdual_var(i)) = 0;
end

% xy = sdpvar(length(x_var)+length(y_var),1);
% z = sdpvar(length(dual_var),1);
% res = p.F_struc*[1;xy;z]
%
% F_bilevel = [res(1:p.K.f) == 0,res(p.K.f+1:end)>0]
%

% Enable outer problem to be nonconvex etc
p = build_recursive_scheme(p);
% Turned off, generates crash. Unit test in test_bilevel_1
% p = compress_evaluation_scheme(p);

p.lower = -inf;
p.options.verbose = max([0 options.verbose-1]);
p.level = 0;
p.as_free = true(ninequalities,1);
list{1} = p;
lower = -inf;
upper = inf;
iter = 0;
tol = 1e-8;
ndomcuts = 0;
ninfeascuts = 0;

% Extract the inequalities in the inner problem. These are really the
% interesting ones
inner_p.F_struc = [inner_p.F_struc(1+inner_p.K.f:end,:) spalloc(inner_p.K.l,ninequalities+nequalities,0)];

if options.verbose
    disp('* Starting YALMIP bilevel solver.');
    disp(['* Outer solver   : ' outer_p.solver.tag]);
    disp(['* Inner solver   : ' inner_p.solver.tag]);
    disp(['* Max iterations : ' num2str(p.options.bnb.maxiter)]);
    disp(' Node       Upper       Gap(%)      Lower    Open');
end
gap = inf;
xsol = [];
sol.problem = 0;
iter = 0;
inner_p = detectdisjoint(inner_p);

while length(list)>0 & gap > options.bilevel.relgaptol & iter < options.bilevel.maxiter
    iter = iter + 1;
    [p,list,lower] = select(list);

    Comment = '';
    if p.lower<upper

        if strcmp(p.options.solver,'bmibnb') & ~isinf(upper)
            % Allow early termination in bmibnb if it is used in outer
            % probllem
            p.options.bmibnb.lowertarget = upper;
        end

        output  = feval(p.solver.call,p);

        if output.problem==2
            Comment = 'Unbounded node';
        end
        if output.problem==1
            % Infeasible
            ninfeascuts = ninfeascuts + 1;
            Comment = 'Infeasible in solver';
        else

            switch output.problem
                case 0
                    Comment = 'Solved to optimality';
                otherwise
                    Comment = yalmiperror(output.problem);
            end

            z = apply_recursive_evaluation(p,output.Primal);
            cost = z'*p.Q*z + p.c'*z + p.f;

            ActuallyFeasible = checkfeasiblefast(p,z,options.bilevel.feastol);
            if ~ActuallyFeasible
                % Try to solve the relaxed feasibility problem using the
                % inner solver (i.e. treat as LP. If infeasible, it is for
                % sure infeasible
                pAux = p;pAux.c = p.c*0;pAux.Q = p.Q*0;
                outputCheck = feval(inner_p.solver.call,pAux);
                if outputCheck.problem == 1
                    % We will not continue branching, and let the user now
                    % that this choice
                    Comment = ['Infeasible'];
                    cost = inf;
                    sol.problem = 4;
                else
                    % Hard to say anything
                    Comment = ['Infeasible solution returned, resolve => continue'];
                    sol.problem = 4;
                    cost = p.lower;
                end
            end
            if cost<inf

                 if strcmp(p.options.solver,'bmibnb')
                     if output.problem == -6
                         sol.problem = -6;
                         sol.info = yalmiperror(-6);
                         info = [];
                         return
                     end
                     p.lb = max([p.lb output.extra.propagatedlb],[],2);
                     p.ub = min([p.ub output.extra.propagatedub],[],2);
                 end

                % These are duals in the original inner problem
                lambda = output.Primal(dual_var);
                % Constraint slacks in original inner problem
                slack = inner_p.F_struc*[1;output.Primal];
                % Outer variables
                xi = z(x_var);
                % Inner variables
                yi = z(y_var);

                res = (slack).*lambda;
                if ActuallyFeasible
                    res = (slack).*lambda;
                else
                    % Generate a dummy residual, to make sure we branch on
                    % the first free
                    res = (slack).*lambda*0;
                    res(find(p.as_free)) = 1:length(find(p.as_free));
                end
                if (all(p.as_free==0) | max(abs(res(p.as_free)))<options.bilevel.compslacktol) & ActuallyFeasible
                    % Feasible!
                    if upper>cost
                        upper = cost;
                        xsol = xi;
                        zsol = yi;
                        dualsol = output.Primal(dual_var);
                    end
                elseif cost>upper-1e-10
                    ndomcuts = ndomcuts + 1;
                else

                    % No official code, just playing around
                    if  ActuallyFeasible & options.bilevel.solvefrp
                        FRP = FRP0;
                        if 0
                            FRP = fixvariables(FRP0,x_var,xi,y_var);
                        else
                            FRP.F_struc = [xi -sparse(1:length(x_var),x_var,ones(length(x_var),1),length(x_var),length(x_var)+length(y_var));FRP.F_struc];
                            FRP.K.f = FRP.K.f + length(xi);
                            FRP.options.verbose = 0;
                            QQ = sparse(FRP0.Q);
                            cc = sparse(FRP0.c);
                            FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
                            FRP.Q(x_var,y_var)=0;
                            FRP.Q(y_var,x_var)=0;
                            FRP.Q(x_var,x_var)=0;
                        end
                        outputFRP = feval(inner_p.solver.call,FRP);
                        if outputFRP.problem == 0
                            if 0
                                z = zeros(length(outer_p.c),1);
                                z(x_var) = xi;
                                z(y_var) = outputFRP.Primal;
                                z2 = apply_recursive_evaluation(p,z);
                            else
                                z2 = apply_recursive_evaluation(p,outputFRP.Primal);
                            end
                            costFRP = z2'*outer_p.Q*z2 + outer_p.c'*z2 + outer_p.f;
                            if costFRP < upper & isfeasible(outer_p,z2)
                                upper = costFRP;
                                xsol = z2(x_var);
                                zsol = z2(y_var);
                            end
                        end
                    end

                    [ii,jj_tmp] = max(res(p.as_free));
                    ind_tmp = (1:length(res))';
                    ind_tmp = ind_tmp(p.as_free);
                    jj = ind_tmp(jj_tmp);

                    if strcmp(p.options.solver,'bmibnb')
                        % Since BMIBNB solves a relaxation of relaxation, it
                        % can generate a lower bound which is lower than
                        % the lower bound before a compl. slack constraint
                        % was added.
                        p.lower = max(output.lower,lower);
                    else
                        p.lower = cost;
                    end

                    if iter<=options.bilevel.rootcuts
                        % Add a disjunction cut
                        p = disjunction(p,dual_var(jj),inner_p.F_struc(jj,:),output.Primal);
                        % Put in queuee, it will be pulled back immediately
                        list = {list{:},p};
                    else

                        p1 = p;
                        p2 = p;

                        % Add dual == 0 on p1
                        p1.K.f = p1.K.f + 1;
                        p1.F_struc = [zeros(1,size(p1.F_struc,2));p1.F_struc];
                        p1.F_struc(1,1+dual_var(jj))=1;
                        p1.lb(dual_var(jj)) = -inf;
                        p1.ub(dual_var(jj)) = inf;
                        newequality = p1.F_struc(1,:);
                        redundantinequality = findrows(p1.F_struc(p1.K.f+1:end,:),newequality);
                        if ~isempty(redundantinequality)
                            p1.F_struc(p1.K.f+redundantinequality,:)=[];
                            p1.K.l = p1.K.l-length(redundantinequality);
                        end

                        % Add slack == 0
                        p2.K.f = p2.K.f + 1;
                        newequality = inner_p.F_struc(jj,:);
                        p2.F_struc = [newequality;p2.F_struc];
                        redundantinequality = findrows(p2.F_struc(p2.K.f+1:end,:),newequality);
                        if ~isempty(redundantinequality)
                            p2.F_struc(p2.K.f+redundantinequality,:)=[];
                            p2.K.l = p2.K.l-length(redundantinequality);
                        end

                        p1.as_free(jj) = false;
                        p2.as_free(jj) = false;
                        if ~isempty(inner_p.disjoints)
                            here = find(inner_p.disjoints(:,1) == j);
                            if ~isempty(here)
                                p1.as_free(inner_p.disjoints(here,2))=false;
                                p2.as_free(inner_p.disjoints(here,2))=false;
                            else
                                here = find(inner_p.disjoints(:,2) == j);
                                if ~isempty(here)
                                    p1.as_free(inner_p.disjoints(here,1))=false;
                                    p2.as_free(inner_p.disjoints(here,1))=false;
                                end
                            end
                        end

                        p1.level = p.level+1;
                        p2.level = p.level+1;
                        list = {list{:},p1};
                        list = {list{:},p2};
                    end
                end
            end
        end
    else
        ndomcuts = ndomcuts + 1;
    end

    [list,lower] = prune(list,upper);

    gap = abs((upper-lower)/(1e-3+abs(upper)+abs(lower)));
    if isnan(gap)
        gap = inf;
    end
    if options.verbose
        fprintf(' %4.0f : %12.3E  %7.2f   %12.3E  %2.0f  %s\n',iter,full(upper),100*full(gap),full(lower),length(list),Comment)
    end
end
info.upper = upper;
info.iter = iter;
info.ninfeascuts = ninfeascuts;
info.ndomcuts = ndomcuts;

if ~isempty(xsol)
    assign(recover(all_variables(x_var)),xsol);
    assign(recover(all_variables(y_var)),zsol);
else
    sol.problem = 1;
end

function [list,lower] = prune(list,upper)

l = [];
for i = 1:length(list)
    l = [l list{i}.lower];
end
j = find(upper > l+1e-10);
list = {list{j}};
if length(list) == 0
    lower = upper;
else
    lower = min(l(j));
end

function [p,list,lower] = select(list)
l = [];
for i = 1:length(list)
    l = [l list{i}.lower];
end
[i,j] = min(l);

p = list{j};
list = {list{1:j-1},list{j+1:end}};
lower = min(l);

function p = addzero(p,i);

p.K.f = p.K.f + 1;
p.F_struc = [zeros(1,size(p.F_struc,2));p.F_struc];
p.F_struc(1,1+i)=1;

function outer_p = pad(outer_p,all_variables)

[i,loc] = find(ismember(all_variables,outer_p.used_variables));
p = outer_p;
% Set all bounds to infinite, and then place the known bounds
p.lb = -inf(length(all_variables),1);
p.lb(loc) = outer_p.lb;
p.ub = inf(length(all_variables),1);
p.ub(loc) = outer_p.ub;

% Set all variables as linear
p.variabletype = zeros(1,length(all_variables));
p.variabletype(loc) = outer_p.variabletype;

p.c = spalloc(length(all_variables),1,0);
p.c(loc) = outer_p.c;

if ~isempty(p.F_struc)
    p.F_struc = spalloc(size(p.F_struc,1),length(all_variables)+1,nnz(p.F_struc));
    p.F_struc(:,1) = outer_p.F_struc(:,1);
    p.F_struc(:,1+loc) = outer_p.F_struc(:,2:end);
end

% if ~isempty(p.binary_variables)
% end

p.Q = spalloc(length(all_variables),length(all_variables),nnz(outer_p.Q));
p.Q(loc,loc) = outer_p.Q;

outer_p = p;

function p = disjunction(p,variable,const,xstar)
neq = p.K.f+1;

x = sdpvar(length(p.c),1);
e = p.F_struc*[1;x];
Model1 = [x(variable)==0,-e(1:p.K.f)==0, e(1+p.K.f:end)>=0];
Model2 = [const*[1;x]==0,-e(1:p.K.f)==0, e(1+p.K.f:end)>=0];
Ab1 = getbase(sdpvar(Model1));
Ab2 = getbase(sdpvar(Model2));
b1 = -Ab1(:,1);
A1 =  Ab1(:,2:end);
b2 = -Ab2(:,1);
A2 = Ab2(:,2:end);

% b1c = [0;-p.F_struc(:,1)];
% b2c = [const(1);-p.F_struc(:,1)];
% A1c = [-eyev(length(p.c),variable)';p.F_struc(:,2:end)];
% A2c = [-const(2:end);p.F_struc(:,2:end)];
%norm(b1-b1c)
%norm(b2-b2c)
%norm(A1-A1c,inf)
%norm(A2-A2c,inf)

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

Objective  = alpha'*xstar-beta;
Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
%Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta == mu1'*b1, beta == mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
%Constraint = [Constraint,-10<alpha<10,sum(mu1(neq+1:end))-sum(mu1(1:neq))<10,sum(mu2(neq+1:end))-sum(mu2(1:neq))<10];
%Constraint = [Constraint,-1<alpha<1,mu1(1)+mu2(1) == 1];
Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)==1];
%Constraint = [Constraint,sum(mu1(neq+1:end))-sum(mu1(1:neq))<10,sum(mu2(neq+1:end))-sum(mu2(1:neq))<10];

solvesdp(Constraint,Objective,sdpsettings('verbose',0));
p.K.l = p.K.l + 1;
p.F_struc = [p.F_struc;-double(beta) double(alpha)'];

function p = disjunctionFAST(p,variable,const,xstar)
neq = p.K.f+1;
n = length(p.c);

b1 = [0;-p.F_struc(:,1)];
b2 = [const(1);-p.F_struc(:,1)];
A1 = [-eyev(length(p.c),variable)';p.F_struc(:,2:end)];
A2 = [-const(2:end);p.F_struc(:,2:end)];

alpha_ind = 1:length(xstar);
beta_ind = alpha_ind(end)+1;
mu1_ind = (1:length(b1))+beta_ind;
mu2_ind = (1:length(b2))+mu1_ind(end);

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

p_hull = p;
p_hull.c = zeros(mu2_ind(end),1);
p_hull.c(alpha_ind) = xstar;
p_hull.c(beta_ind) = -1;

% equalities alpha = Ai'*mui, sum(mu)==1
p_hull.K.f = length(xstar)*2+1;
p_hull.F_struc = [zeros(length(xstar),1) eye(length(xstar)) zeros(length(xstar),1) -A1' zeros(length(xstar),length(b2))];
p_hull.F_struc = [p_hull.F_struc;
    zeros(length(xstar),1) eye(length(xstar)) zeros(length(xstar),1) zeros(length(xstar),length(b2)) -A2'];
p_hull.F_struc = [p_hull.F_struc ;1 zeros(1,length(xstar)) 0 -ones(1,length(b1)+length(b2))];

% Inequalities
p_hull.F_struc = [p_hull.F_struc ;0 zeros(1,length(xstar)) -1 b1' b2'*0];
p_hull.F_struc = [p_hull.F_struc ;0 zeros(1,length(xstar)) -1 0*b1' b2'];
npmu = length(b1)-neq;
p_hull.F_struc = [p_hull.F_struc; zeros(npmu,1) zeros(npmu,length(xstar)) zeros(npmu,1) zeros(npmu,neq) eye(npmu) zeros(npmu,length(b2))];
p_hull.F_struc = [p_hull.F_struc; zeros(npmu,1) zeros(npmu,length(xstar)) zeros(npmu,1) zeros(npmu,length(b1)) zeros(npmu,neq) eye(npmu)];
p_hull.F_struc = [p_hull.F_struc; ones(length(xstar),1) -eye(length(xstar)) zeros(length(xstar),1+2*length(b1))];
p_hull.F_struc = [p_hull.F_struc; ones(length(xstar),1)  eye(length(xstar)) zeros(length(xstar),1+2*length(b1))];
p_hull.K.l = 2+npmu*2+2*length(xstar);
p_hull.lb = [];
p_hull.ub = [];
output = feval(p_hull.solver.call,p_hull);
alpha = output.Primal(alpha_ind);
beta = output.Primal(beta_ind);

% Objective  = alpha'*xstar-beta;
% Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2,beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq+1:end)>0,mu2(neq+1:end)>0];
% Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)==1];
% Constraint = [Constraint,sum(mu1)+sum(mu2)==1];
% solvesdp(Constraint,Objective,sdpsettings('verbose',0));

p.K.l = p.K.l + 1;
p.F_struc = [p.F_struc;-double(beta) double(alpha)'];

function feas = isfeasible(p,x)

feas = checkfeasiblefast(p,x,1e-8);

function p = detectdisjoint(p);

p.disjoints = [];
% for i = 1:p.K.l
%     row1 = p.F_struc(i+p.K.f,:);
%     for j = 2:1:p.K.l
%         row2 = p.F_struc(j+p.K.f,:);
%
%         if all(abs(row1)-abs(row2)==0)
%             % candidate
%             if nnz(row1 == -row2 & row1~=0)==1
%                 p.disjoints = [p.disjoints;i j];
%             end
%         end
%     end
% end
%

function FRP = fixvariables(FRP0,x_var,xi,y_var);
% Copy current model
FRP = FRP0;

%
FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
FRP.c(x_var) = [];
FRP.Q(:,x_var) = [];
FRP.Q(x_var,:) = [];
FRP.lb(x_var) = [];
FRP.ub(x_var) = [];
B = FRP.F_struc(:,1+x_var);
FRP.F_struc(:,1+x_var)=[];
FRP.F_struc(:,1) = FRP.F_struc(:,1) + B*xi;

%                         FRP.F_struc = [xi -sparse(1:length(x_var),x_var,ones(length(x_var),1),length(x_var),length(x_var)+length(y_var));FRP.F_struc];
%                         FRP.K.f = FRP.K.f + length(xi);
%                         FRP.options.verbose = 0;
%                         QQ = FRP0.Q;
%                         cc = FRP0.c;
%                         FRP.c(y_var) = FRP.c(y_var) + 2*FRP.Q(x_var,y_var)'*xi;
%                         FRP.Q(x_var,y_var)=0;
%                         FRP.Q(y_var,x_var)=0;
%                         FRP.Q(x_var,x_var)=0;

function [merged_mt,merged_vt] = mergemonoms(inner_p,outer_p);

if isequal(inner_p.used_variables,outer_p.used_variables)
    merged_mt = inner_p.monomtable;
    merged_vt = inner_p.variabletype;
else
    invar  = inner_p.used_variables;
    outvar = outer_p.used_variables;
    all_variables = unique([invar outvar]);

    [i_inner,loc_inner] = find(ismember(all_variables,inner_p.used_variables));
    [i_outer,loc_outer] = find(ismember(all_variables,outer_p.used_variables));

    merged_mt = spalloc(length(all_variables),length(all_variables),0);
    merged_vt = zeros(1,length(all_variables));

    for i = 1:length(i_inner)
        [ii,jj,kk] = find(inner_p.monomtable(i,:));
        merged_mt(loc_inner(i),loc_inner(jj)) = kk;
        merged_vt(loc_inner(i)) = inner_p.variabletype(i);
    end
    for i = 1:length(i_outer)
        [ii,jj,kk] = find(outer_p.monomtable(i,:));
        merged_mt(loc_outer(i),loc_outer(jj)) = kk;
        merged_vt(loc_outer(i)) = outer_p.variabletype(i);
    end
end

author: zarathustr

repo: LibQPEP

save_path: github-repos/MATLAB/zarathustr-LibQPEP


text:
function [ output_args ] = plotRoom2( obblist, labellist, scheme )

% scheme == 0: show objects with different color
% scheme == 1: show areas with different color
linewidth = 2;

corlist = {[1,0,0,0.3],[0,1,0,0.3],[0,1,1,0.3],[0,0,1,0.3],[1,0,1,0.3]};

floor_ind = strmatch('floor',labellist,'exact');
ceiling_ind = strmatch('ceiling',labellist,'exact');
wall_ind = strmatch('wall',labellist,'exact');
com_ind = union(floor_ind,ceiling_ind);
com_ind = union(com_ind,wall_ind);
% idx = 1:length(p_data.labellist);
% idx = setdiff(idx,com_ind);
% labellist = labellist;
% obblist = obblist;

front = [0,0,1];
up = [0,1,0];
axes = [1,0,0];

for i = 1:length(labellist)
    p = obblist(:,i);

    center = p(1:3);
    dir_1 = p(4:6);
    dir_2 = p(7:9);
    lengths = p(10:12);

    dir_1 = dir_1/norm(dir_1);
    dir_2 = dir_2/norm(dir_2);
    dir_3 = cross(dir_1,dir_2);
    dir_3 = dir_3/norm(dir_3);
%     dir = lengths(1)*dir_1+lengths(2)*dir_2+lengths(3)*dir_3;

    d1 = 0.5*lengths(1)*dir_1;
    d2 = 0.5*lengths(2)*dir_2;
    d3 = 0.5*lengths(3)*dir_3;
%     d1 = d1*front(:);
%     d2 = d2*up(:);
%     d3 = d3*axes(:);
    cornerpoints(1,:) = center-d1-d2-d3;
    cornerpoints(2,:) = center+d1-d2-d3;
    cornerpoints(3,:) = center-d1-d2+d3;
    cornerpoints(4,:) = center+d1-d2+d3;
    %cornerpoints(:,1) = 1 - cornerpoints(:,1);

    idx = find(com_ind==i);
    if(length(idx)==0)
        if(scheme==1&&isfield(p_data,'area_id'))
            aid = p_data.area_id(i);
            if(aid==0)
                cor = [0,0,0,0.3];
            else
                cor = corlist{mod(aid-1,length(corlist))+1};
            end
        else
            cor = corlist{mod(i-1,length(corlist))+1};
        end
        plot([cornerpoints(1,1),cornerpoints(2,1)],[cornerpoints(1,3),cornerpoints(2,3)],'Color',cor(1:3),'linewidth',linewidth);
        hold on
        plot([cornerpoints(2,1),cornerpoints(4,1)],[cornerpoints(2,3),cornerpoints(4,3)],'Color',cor(1:3),'linewidth',linewidth);
        plot([cornerpoints(4,1),cornerpoints(3,1)],[cornerpoints(4,3),cornerpoints(3,3)],'Color',cor(1:3),'linewidth',linewidth);
        plot([cornerpoints(3,1),cornerpoints(1,1)],[cornerpoints(3,3),cornerpoints(1,3)],'Color',cor(1:3),'linewidth',linewidth);
        id = strfind(labellist{i},'_');
        label = labellist{i};
        if(length(id)>0)
            label(id) = ' ';
        end
        min_x = min(cornerpoints(:,1));
        max_z = max(cornerpoints(:,3));
        t = text(min_x+0.01,max_z-0.02,[label,num2str(i)]);
        t.BackgroundColor = cor;
        t.FontSize = 8;
    else
        plot([cornerpoints(1,1),cornerpoints(2,1)],[cornerpoints(1,3),cornerpoints(2,3)],'k','linewidth',linewidth);
        hold on
        plot([cornerpoints(2,1),cornerpoints(4,1)],[cornerpoints(2,3),cornerpoints(4,3)],'k','linewidth',linewidth);
        plot([cornerpoints(4,1),cornerpoints(3,1)],[cornerpoints(4,3),cornerpoints(3,3)],'k','linewidth',linewidth);
        plot([cornerpoints(3,1),cornerpoints(1,1)],[cornerpoints(3,3),cornerpoints(1,3)],'k','linewidth',linewidth);
    end
end

axis equal

end

repo: GRAINS


text:
%% AMG TEST II: Different boundary conditions
%
% We consider the linear finite element discretization of Poisson equation
% on the unstructured mesh with Dirichlet or Neumann boundary conditions.

%%
load lakemesh
%% Dirichlet problem
[N,itStep,time,err] = amgtest(node,elem,1);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );

%% Neumann problem
[N,itStep,time,err] = amgtest(node,elem,0);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );

repo: Aero-matlab


text:
%%                   testPhaseLift.m

% This test file implements the Phaselift solver. The code builds
% a synthetic formulation of the Phase Retrieval problem, b = |Ax| and
% computes an estimate to x. The code finally plots a convergence curve
% and also makes a scatter plot of the true vs recovered solution.

% PAPER TITLE:
%             PhaseLift: Exact and Stable Signal Recovery from Magnitude
%             Measurements via Convex Programming.

% ARXIV LINK:
%              https://arxiv.org/abs/1109.4499

% 1) Each test script starts out by defining the length of the unknown
% signal, n and the number of measurements, m. These mesurements can be
% made complex by setting the isComplex flag to be true.
%
% 2) We then build the test problem by invoking the function
% 'buildTestProblem' which generates random gaussian measurements according
% to the user's choices in step(1). The function returns the measurement
% matrix 'A', the true signal 'xt' and the measurements 'b0'.
%
% 3) We set the options for the PR solver. For example, the maximum
% number of iterations, the tolerance value, the algorithm and initializer
% of choice. These options are controlled by setting the corresponding
% entries in the 'opts' struct.  Please see the user guide for a complete
% list of options.
%
% 4) We solve the phase retrieval problem by running the following line
% of code:
%   >>  [x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts)
% This solves the problem using the algorithm and initialization scheme
% specified by the user in the struct 'opts'.
%
% 5) Determine the optimal phase rotation so that the recovered solution
% matches the true solution as well as possible.
%
% 6) Report the relative reconstruction error. Plot residuals (a measure
% of error) against the number of iterations and plot the real part of the
% recovered signal against the real part of the original signal.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

clc;
clear;
close all;

n = 50;          % Dimension of unknown vector
m = 8 * n;        % Number of measurements
isComplex = true; % If the signal and measurements are complex

%%  Build a random test problem
fprintf('Building test problem...\n');
[A, xt, b0] = buildTestProblem(m, n, isComplex);

% Options
opts = struct;
opts.initMethod = 'custom';
opts.customx0 = randn(n,1); % random initial guess
opts.algorithm = 'PhaseLift';
opts.isComplex = isComplex;
opts.tol = 1e-6;
opts.maxIters = 10000;
opts.verbose = 2;

%% Try to recover x
fprintf('Running algorithm...\n');
[x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts);

%% Determine the optimal phase rotation so that the recovered solution
%  matches the true solution as well as possible.
alpha = (x'*xt)/(x'*x);
x = alpha * x;

%% Determine the relative reconstruction error.  If the true signal was
%  recovered, the error should be very small - on the order of the numerical
%  accuracy of the solver.
reconError = norm(xt-x)/norm(xt);
fprintf('relative recon error = %d\n', reconError);

% Plot a graph of error(definition depends on if opts.xt is provided) versus
% the number of iterations.
plotErrorConvergence(outs, opts)

% Plot a graph of the recovered signal x against the true signal xt.
plotRecoveredVSOriginal(x,xt);

author: tomgoldstein

repo: phasepack-matlab


text:
function [xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)

%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil
%Version 1.20
%Created by Stepen (zerocross_raptor@yahoo.com)
%Created 20 November 2010
%Last modified 30 November 2011
%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil generates the airfoil vertexes' coordinate of
%the given NACA Series 5 Airfoil. The math equation used to generates the
%airfoil coordinates is based on Theory of Wing Section Chapter 6 by Abbott
%and Doenhoff.
%--------------------------------------------------------------------------
%Syntax:
%[xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)
%Input argument:
%- afid (1 x 5 str) specifies NACA Series 5 Airfoil identifier.
%- dotcount (1 x 1 int) specifies the number of vertexes to be generated on
%  the airfoil's camber/mean line.
%- xmod (str) specifies the mode of airfoil vertex distribution. Enter
%  'Uniform' to create uniform vertex distribution or 'Cosine' to create
%  vertex distribution based on cosine function (More vertex on the leading
%  edge region).
%Output argument:
%- xu (i x 1 num) specifies the x axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of xu) to the trailing
%  edge (the last element of xu).
%- yu (i x 1 num) specifies the y axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of yu) to the trailing
%  edge (the last element of yu).
%- xl (i x 1 num) specifies the x axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of xl) to the trailing
%  edge (the last element of xl).
%- yl (i x 1 num) specifies the y axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of yl) to the trailing
%  edge (the last element of yl).
%- xc (i x 1 num) specifies the x axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of xc) to the trailing
%  edge (the last element of xc).
%- yc (i x 1 num) specifies the y axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of yc) to the trailing
%  edge (the last element of yc).
%--------------------------------------------------------------------------

%CodeStart-----------------------------------------------------------------
%Checking input afid
    if ~ischar(afid)
        error('Airfoil identifier must be a string!')
    end
    if numel(afid)~=5
        error('Airfoil identifier must be a 5 digit number!')
    end
    if isempty(str2double(afid))
        error('Airfoil identifier must be a 5 digit number!')
    end
    id=str2double(afid([2,3]));
    if (id~=10)&&(id~=20)&&(id~=30)&&(id~=40)&&(id~=50)
        error(['Airfoil identifier must start with X10,',...
               ' X20, X30, X40, or X50!'])
    end
%Checking input dotcount
    if numel(dotcount)~=1
        error('Number of vertex must be scalar!')
    end
    if (mod(dotcount,1~=0))||(dotcount<0)
        error('Number of vertex must be positive integer!')
    end
%Checking input xmod
    if nargin<3
        xmod='Cosine';
    end
    if (~strcmpi(xmod,'Uniform'))&&(~strcmpi(xmod,'Cosine'))
        error('Vertex distribution input must be Uniform or Cosine!')
    end
%Declaring look-up table
    mtable=[0.0580,0.1260,0.2025,0.2900,0.3910];
    ktable=[361.4000,51.6400,15.9570,6.6430,3.2300];
%Assigning identifier to equation coefficient
    id1=str2double(afid(1));
    id2=str2double(afid([2,3]));
    id3=str2double(afid([4,5]));
    cl=id1*(3/2)*(10/100);      %Design lift coefficient
    p=id2*(1/2)*(1/100);        %Maximum camber location
    t=id3*(1/100);              %Maximum thickness
    if t==0
        warning(['Zero thickness airfoil!',...
                 ' Airfoil will be just a camber line!'])
    end
    m=mtable(str2double(afid(2)));
    k=ktable(str2double(afid(2)));
%Calculating x-axis location of camber line vertexes
    panelcount=dotcount-1;
    if strcmpi(xmod,'Uniform')
        panellength=1/panelcount;
        xc=(0:panellength:1)';
    elseif strcmpi(xmod,'Cosine')
        deltadeg=90/panelcount;
        xc=1-cosd(0:deltadeg:90)';
    end
%Preallocating array for speed
    yc=zeros(dotcount,1);
    gc=zeros(dotcount,1);
    yt=zeros(dotcount,1);
    xu=zeros(dotcount,1);
    xl=zeros(dotcount,1);
    yu=zeros(dotcount,1);
    yl=zeros(dotcount,1);
%Calculating y-axis location of camber line vertexes and camber gradient
    if m~=0
        for i=1:1:dotcount
            if xc(i)<=p
                yc(i)=(k/6)*((xc(i)^3)-...
                             (3*m*(xc(i)^2))+...
                             ((m^2)*(3-m)*xc(i)));
                gc(i)=(k/6)*((2*(xc(i)^2))-...
                             (6*m*xc(i))+...
                             (((3*(m^2))-(m^3))));
            elseif xc(i)>p
                yc(i)=(k/6)*(m^3)*(1-xc(i));
                gc(i)=-1*(k/6)*(m^3);
            end
        end
    end
%Correcting camber line vertexes and camber gradient for extended series
    yc=yc*(id1/2);
    gc=gc*(id1/2);
%Converting camber gradient to camber slope and normal
    sc=atand(gc);
%Calculating thickness distribution
    for i=1:1:dotcount
        yt(i)=5*t*((0.29690*(xc(i)^0.5))-...
                   (0.12600*xc(i))-...
                   (0.35160*(xc(i)^2))+...
                   (0.28430*(xc(i)^3))-...
                   (0.10150*(xc(i)^4)));
    end
%Generating airfoil vertexes
    for i=1:1:dotcount
        xu(i)=xc(i)-yt(i)*sind(sc(i));
        yu(i)=yc(i)+yt(i)*cosd(sc(i));
        xl(i)=xc(i)+yt(i)*sind(sc(i));
        yl(i)=yc(i)-yt(i)*cosd(sc(i));
    end
%CodeEnd-------------------------------------------------------------------

end

author: Sable

repo: mcbench-benchmarks


text:
function [x, p, f]=numunique(x)
%NUMUNIQUE Numerical set unique.
% [B P F]=numunique(A) for the array A returns the same values as in A but
% with no repetitions. B will also be sorted. A can only be numerical,
% as the name of the function suggests.
%
% The second output, P, is a row of cells, containing the indices of A,
% such that, A(p{n})=B(n) is true. P has the same number of cells as the
% number of elements in B.
%
% Note that each cell of P lists all the indices of A, not only the first
% or last occurrence, which have the repetitious values. It is this nature
% of the second output makes this function different to the Mathwork's
% function UNIQUE. Sometimes we need to know all the indices of which A
% have the same values.
%
% The third optional output, F, is for the indices of the first occurrences
% of repetitious values, so that B==A(F) is true.
%
% Example, let  A = [1 1 5 6 2 3 3 9 8 6 2 4], then
%            [B P F]=numunique(A)
% results in
%            B =[1 2 3 4 5 6 8 9],
% P =
%  [1x2 double] [1x2 double] [1x2 double]  [12]  [3]  [1x2 double] [9] [8]
% and
% F =
%    1     5     6    12     3     4     9     8
%
% Check for the second element B for example, P{2}=[5 11],
% A([5 11])=[2 2], which are all equal to B(2). And of course, B-A(F)
% gives all 0's.
%
% Note that the first output can be a row or column vector depends on the
% input is a row vector or otherwise (column or matrix). However the second
% output P is always a row of cells. To force P in a row order has an
% advantage, so that you can get A sorted with a shortcut of A([P{:}]).
%
% See also UNIQUE
%
% Zhigang Xu. 02-Sep-2009. Updated on 05-Sep-2009.

isrowvec=size(x,1)==1;

if nargout<=1
	x=sort(x(:));
	n=[true; diff(x)~=0];
	x=x(n);
	if isrowvec, x=x.'; end
	return
end

[x s]=sort(x(:));
s=s.';
N=numel(x);
n=find([true; diff(x)~=0]);
x=x(n);
K=numel(x);
if isrowvec, x=x.'; end

switch(nargout)
	case{2}
		if K==1
			p{1}=s;
		else
			p{K}=s(n(K):N);
			for k=1:K-1
				p{k}=s(n(k):n(k+1)-1);
			end
		end
	case{3}
		if K==1
			p{1}=s;
			f(1)=p{1}(1);
		else
			p{K}=s(n(K):N);
			f(K)=p{K}(1);
			for k=1:K-1
				p{k}=s(n(k):n(k+1)-1);
				f(k)=p{k}(1);
			end
		end
	otherwise
		error('More than 3 outputs are not allowed!')
end
end


text:
function psi = conv(psi1,psi2,varargin)
% convolution of an SO3Kernel function with a function or a kernel on SO(3)
%
% We convolute an SO3Kernel $f$ with another SO3Kernel or an SO3Fun $g$
% by the convolution
%
% $$ (f *_L g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(q^{-1}\,R) \, dq $$
%
% which in this case is similar to the so caled right sided convolution,
% see SO3FunHarmonic/conv.
%
% The convolution of an SO3Kernel with an S2Kernel or an S2Fun $h$ is
% defined by
%
% $$ (f * h)(\xi) =  \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot h(q^{-1}\,\xi) \, dq $$.
%
%
% Syntax
%   psi = conv(psi1,psi2)
%   SO3F2 = conv(psi1,SO3F1)
%   sF2 = conv(psi1,sF1)
%   phi2 = conv(psi1,phi1)
%   psi = conv(psi1)
%
% Input
%  psi1, psi2 - @SO3Kernel
%  phi1       - @S2Kernel
%  SO3F1      - @SO3Fun
%  sF1        - @S2Fun
%
% Output
%  psi   - @SO3Kernel
%  SO3F2 - @SO3Fun
%  sF2   - @S2Fun
%  phi2  - @S2Kernel
%
% See also
% SO3FunHarmonic/conv SO3FunRBF/conv S2FunHarmonic/conv S2Kernel/conv

if nargin == 1, psi2 = psi1; end

% ------------------- convolution with a SO3Fun -------------------
% In case psi2 is a SO3Fun, convolute the SO3Fun with the kernel
% conv is commutative if kernel is included
if isa(psi2,'SO3Fun')
  psi = conv(psi2,psi1,varargin{:});
  return
end

% ------------------- convolution with a S2Fun -------------------
if isa(psi2,'S2Fun')
  sF = S2FunHarmonic(psi2);
  L = min(psi1.bandwidth,sF.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = psi1.A(l+1) * sF.fhat(l^2+1:(l+1)^2) ./ (2*l+1);
  end

  if isa(psi2,'S2FunHarmonicSym')
    warning(['There is no symmetry given for the SO3Kernel function. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the symmetry of the S2Fun.'])
  end
  psi = S2FunHarmonic(fhat);
  return
end

% ------------------- convolution with a S2Kernel -------------------
if isa(psi2,'S2Kernel')
  L = min(psi1.bandwidth,psi2.bandwidth);
  l = (0:L);
  psi = S2Kernel(psi1.A(1:L+1) .* psi2.A(1:L+1) ./ (2*l+1));
  return
end

% ------------------- convolution of SO3Kernels -------------------
if isnumeric(psi1)
  psi = conv(psi2,psi1,varargin{:});
  return
end

% extract Legendre coefficients of psi1
A1 = psi1.A(:);

% extract Legendre coefficients of psi2
if isnumeric(psi2)
  A2 = psi2(:);
else
  A2 = psi2.A(:);
  A2 = A2 ./ (2*(0:length(A2)-1)+1).';
end

% multiplication in harmonic domain
L = min(psi1.bandwidth,psi2.bandwidth);
psi = SO3Kernel(A1(1:L+1) .* A2(1:L+1));

end

author: mtex-toolbox

repo: mtex

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


text:
function Script_CECLM_cross_data()

addpath(genpath('../'));

[images, detections, labels] = Collect_menpo_imgs('G:\Datasets\menpo/');

%% loading the CE-CLM model and parameters
[patches, pdm, clmParams, early_term_params] = Load_CECLM_general();
views = [0,0,0; 0,-30,0; 0,30,0; 0,-55,0; 0,55,0; 0,0,30; 0,0,-30; 0,-90,0; 0,90,0; 0,-70,40; 0,70,-40];
views = views * pi/180;

% As early termination weights were trained on part of menpo turn them off,
% to perform a clean cross-data experiment
early_term_params.weights_scale(:) = 1;
early_term_params.weights_add(:) = 0;
early_term_params.cutoffs(:) = -0.2;

% Use the multi-hypothesis model, as bounding box tells nothing about
% orientation
multi_view = true;

%% Setup recording
experiment.params = clmParams;

num_points = numel(pdm.M)/3;

shapes_all = cell(numel(images), 1);
labels_all = cell(numel(images), 1);
lhoods = zeros(numel(images),1);
all_lmark_lhoods = zeros(num_points, numel(images));
all_views_used = zeros(numel(images),1);

% Change if you want to visualize the outputs
verbose = false;
output_img = false;

if(output_img)
    output_root = './ceclm_gen_out/';
    if(~exist(output_root, 'dir'))
        mkdir(output_root);
    end
end
if(verbose)
    f = figure;
end
%% Fitting the model to the provided images
tic
for i=1:numel(images)
    image = imread(images(i).img);
    image_orig = image;

    if(size(image,3) == 3)
        image = rgb2gray(image);
    end

    bbox = squeeze(detections(i,:));

    % The actual work get's done here
    [shape,~,~,lhood,lmark_lhood,view_used] =...
        Fitting_from_bb_multi_hyp(image, [], bbox, pdm, patches, clmParams, views, early_term_params);

    all_lmark_lhoods(:,i) = lmark_lhood;
    all_views_used(i) = view_used;

    shapes_all{i} = shape;
    labels_all{i} = labels{i};

    if(mod(i, 200)==0)
        fprintf('%d done\n', i );
    end

    lhoods(i) = lhood;

    if(output_img)
        v_points = logical(patches(1).visibilities(view_used,:))';
        DrawFaceOnImg(image_orig, shape, sprintf('%s/%s%d.jpg', output_root, 'fit', i), bbox, v_points);
    end

    if(verbose)
        v_points = logical(patches(1).visibilities(view_used,:))';
        DrawFaceOnFig(image_orig, shape, bbox, v_points);
    end

end
toc

experiment.lhoods = lhoods;
experiment.shapes = shapes_all;
experiment.labels = labels_all;
experiment.all_lmark_lhoods = all_lmark_lhoods;
experiment.all_views_used = all_views_used;

%%
output_results = 'results/results_ceclm_cross-data.mat';
save(output_results, 'experiment');

end

author: TadasBaltrusaitis

repo: OpenFace


text:
function g = indexardKernGradient(kern, x, varargin)

% INDEXARDKERNGRADIENT Gradient of INDEXARD kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% index ard based covariance function
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO indexardKernParamInit, kernGradient, indexardKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2011

% KERN

  g = zeros(1, length(kern.indices));
  covGrad = varargin{end};
  if nargin < 4
    x2 = x;
  else
    x2 = varargin{1};
  end
  for i = 1:length(kern.indices);
    g(i) = sparse(round(x)==kern.indices(i))'*covGrad*sparse(round(x2)==kern.indices(i));
  end
end

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
%%***************************************************
%% qprod:
%%
%% Input: A = [A1 A2 ... An]
%%        x = [x1; x2; ...; xn]
%% Output: [A1*x1 A2*x2 ... An*xn]
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%***************************************************

  function Ax = qprod(pblk,A,x);

  if (size(pblk,1) > 1)
     error('qprod: pblk can only have 1 row');
  end
  if issparse(x); x = full(x); end; %% for spconvert
  n = length(x);
  ii = [1:n]';
  jj = mexexpand(pblk{2},[1:length(pblk{2})]');
  X = spconvert([ii, jj, x]);
  Ax = A*X;
%%***************************************************

repo: REST


text:
function [err] = FigPrint (Opt, colopt)
%
%   mode 1:
%   [err] = FigPrint ([Opt])
%   mode 2:
%   [err] = FigPrint (h, [col])
%Purpose:
%   Prints the figure, or save to a file
%   Printing only works if you add the printer setup on your machine
%   in this file. I recommend you make your own copy of this function
%   should you choose to modify it.
%
%Input Parameters:
%   Usage mode 1:
%   Opt is an optional structure with the following optional fields
%      .Handle : Handle to the current figure to print.
%              Default is the current figure
%      .Color : Choose 'c' for color  printer
%                      'b' for bw (default)  printer
%      .Layout : Layout of the figure, choose from
%               'p' for portrait
%               'l' for landscape
%               't' for tall (print to fit)(default)
%      .GUI : flag (0/1) uses some special recommended settings to print GUIs
%             Default is 0
%      .Prompt : flag (0/1/2). Asks if you want to print the figure. Default is 0
%                 1 is command line prompt, 2 is GUI prompt
%      .PrintOpt : a string containing additional printing options such as
%                '-zbuffer -r200' for example, default is ''
%      .PaperPosition : a four element vector specifying the position
%               of the figure when printed on the page like
%               [0.25 0.25 4 6] (for [left bottom width height])
%               the units are inches
%Output options:
%    Paper:
%      .prndriver: One of MATLAB's built in drivers, like '-dpsc2',
%                  See help print
%      .prnname: Name of printer, something like '-P_128.231.212.205'
%                prndriver and prnname are set automatically
%                for recognized machines
%    TreeHugging:
%      .as: 'jpg', or 'pdf' , prints electronic instead of paper version
%      .prefix: prefix for electronic printout.
%
%    Usage mode 2:
%       h is the figure handle (h can be a vector of handles)
%       col is either 'b' for bw or 'c' for color
%
%Output Parameters:
%   err : 0 No Problem
%       : 1 Mucho Problems
%
%
%
%More Info :
%   print, printopt
%
%
%
%     Author : Ziad Saad
%     Date : Fri Jun 05 13:49:26 CDT 1998

%Define the function name for easy referencing
FuncName = 'FigPrint';

%initailize return variables
err = 1;

%Set up the default Options
OptDef.Handle = gcf;
OptDef.Color = 'b';
OptDef.Layout = 't';
OptDef.GUI = 0;
OptDef.Prompt = 0;
OptDef.PrintOpt = '';
OptDef.PaperPosition = [];
OptDef.as = [];
OptDef.prefix = 'FigPrint';
OptDef.prndriver = [];
OptDef.prnname = [];

%if nothing was passed
if (nargin == 0),
   Opt = OptDef;
else
   if (~isstruct(Opt)),
      h = Opt; Opt = []; Opt.Handle = h;
      if (nargin == 2),
         Opt.Color = colopt;
      end
   end
   if (~isfield(Opt,'Handle') | isempty (Opt.Handle)),   Opt.Handle = OptDef.Handle;   end
   if (~isfield(Opt,'Color') | isempty (Opt.Color)),   Opt.Color = OptDef.Color;   end
   if (~isfield(Opt,'Layout') | isempty (Opt.Layout)),   Opt.Layout = OptDef.Layout;   end
   if (~isfield(Opt,'GUI') | isempty (Opt.GUI)),   Opt.GUI = OptDef.GUI;   end
   if (~isfield(Opt,'Prompt') | isempty (Opt.Prompt)),   Opt.Prompt = OptDef.Prompt;   end
   if (~isfield(Opt,'PrintOpt') | isempty (Opt.PrintOpt)),   Opt.PrintOpt = OptDef.PrintOpt;   end
   if (~isfield(Opt,'PaperPosition') | isempty (Opt.PaperPosition)),   Opt.PaperPosition = []; end
   if (~isfield(Opt,'as') | isempty (Opt.as)),   Opt.as = []; end
   if (~isfield(Opt,'prefix') | isempty (Opt.prefix)),
      Opt.prefix = 'FigPrint';
   end
   if (~isfield(Opt,'prnname')),   Opt.prnname = []; end
   if (~isfield(Opt,'prndriver')),   Opt.prndriver = []; end
end

for (kk=1:1:length(Opt.Handle)),
   figh = Opt.Handle(kk);

   if (Opt.Prompt == 1),
      figure (figh);
      s = input ('Print figure (y/n) ?','s');
      if (isempty(s) | s ~= 'y'), err = 0; return;   end
   elseif (Opt.Prompt == 2),
      figure (figh);
      Butt = questdlg(sprintf('Print figure %d?', figh), 'Print Figure', 'Yes', 'No', 'No');
      switch Butt,
         case 'No',
            err = 0;
            return;
      end
   end

   %Now form the command line
      %The handle
      Hndl = sprintf('-f%g',figh);

      %The layout
      if (Opt.Layout == 'p'),
        orient(figh, 'portrait');
      elseif (Opt.Layout == 'l'),
        orient(figh, 'landscape');
      elseif (Opt.Layout == 't'),
        orient(figh, 'tall');
      else
        fprintf (2,'%s: Wrong layout specification\n\a', mfilename);
        err = 1;
        return;
      end

      %The colors
      [e,mc] = unix('hostname -s'); ilt = find(isletter(mc)); mc = mc(ilt);
      if (isempty(Opt.prnname) | isempty(Opt.prndriver)),
         switch mc,
         case 'rhodium',
            if (Opt.Color == 'b')
              prnname = '-Plp';
              prndriver = '-dps2';
            elseif (Opt.Color == 'c')
              prnname = '-Plpc';
              prndriver = '-dpsc2';
            else
              fprintf (2,'%s: Wrong printer (Color) specification\n\a', mfilename);
              err = 1;
              return;
            end
         case 'eomer',
            if (Opt.Color == 'b')
              prnname = '-P_128.231.212.205';
              prndriver = '-dps2';
            elseif (Opt.Color == 'c')
              prnname = '-PPhaser_8550DT__00_00_aa_9b_d2_6b_';
              prndriver = '-dpsc2';
            else
              fprintf (2,'%s: Wrong printer (Color) specification\n\a', mfilename);
              err = 1;
              return;
            end
         case 'iodine',
            if (Opt.Color == 'b')
              prnname = '-PHP8100DN';
              prndriver = '-dps2';
            elseif (Opt.Color == 'c')
              prnname = '-PTek';
              prndriver = '-dpsc2';
            else
              fprintf (2,'%s: Wrong printer (Color) specification\n\a', mfilename);
              err = 1;
              return;
            end
         otherwise,
            fprintf (2,'%s: Machine %s not recognized.\n\a',mfilename , mc);
            err = 1;
            return
         end
      else
         prnname = Opt.prnname;
         prndriver = Opt.prndriver;
      end
      %The GUI settings
      if (Opt.GUI),
         set(figh,'PaperPositionMode','auto');
         set(figh,'InvertHardcopy','off');
      end

      %the size settings
      if (~isempty(Opt.PaperPosition)),
         set(figh,'PaperUnits','inches');
         set(figh,'PaperPosition', Opt.PaperPosition);
      end

      if (isempty(Opt.as)),
            CmndLine = sprintf ('print %s %s %s %s',prndriver, Opt.PrintOpt, prnname, Hndl);

       else
         if (length(Opt.Handle) > 1),
            CmndLine = sprintf ('saveas(%d, ''%s'' ,''%s'')', ...
               figh, sprintf('%s.%02d.%s', Opt.prefix, kk, Opt.as), Opt.as);
         else,
            CmndLine = sprintf ('saveas(%d, ''%s'' ,''%s'')', ...
               figh, sprintf('%s.%s', Opt.prefix, Opt.as), Opt.as);
         end
       end
       fprintf (1,'%s verbose : Executing %s ...\n',FuncName,CmndLine);

       eval([CmndLine]);
   end

   err = 0;
   return;

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function N = facenorm(X,Y,Z)

% facenorm compute alternate surface normals for OpenGL surface plots
%
% N = facenorm(X,Y,Z);
%
% X,Y,Z surface coordinates as used with surf
% N     3D array with normal coordinates to be used as value for
%       'VertexNormals' property in surface plots that use OpenGL renderer
%       and flat lighting
%
% EXAMPLE:
%
% % generate some demo data
% [Y,Z,X] = cylinder([0.8 1 1 0.8],144);
% idxShift = mod(1:size(X,2),4)>1;
% X(2,idxShift) = X(2,idxShift)-0.2;
% X(3,idxShift) = X(3,idxShift)+0.2;
%
%
% % standard surf plot
% figure(1)
% surf(X,Y,Z,'EdgeAlpha',0.2,'FaceColor','c')
% axis equal
% light('Position',[0 0 1],'Style','infinite')
% view(0,60)
%
%
% % same plot with alternate surface normals
% % (giving symmetrical lighting, as it should)
% figure(10)
% surf(X,Y,Z,'EdgeAlpha',0.2,'FaceColor','c',...
% 'VertexNormals',facenorm(X,Y,Z))
% axis equal
% light('Position',[0 0 1],'Style','infinite')
% view(0,60)
%
% See also surfo

% Andres, 2009-07-09
% v1.01  with improved code efficiency as suggested by Jan Simon

% input checking left to surf et.al. ...

if numel(Z) > numel(X) || numel(Z) > numel(Y);
    [X,Y] = meshgrid(X,Y);
end

coord = cat(3,X,Y,Z);

% edge vectors
d1 = diff(coord,1,1);
d2 = diff(coord,1,2);

% normals of opposing pairs of edge vectors
% (i.e. at the opposing corners of the quadrilaterals)
n1 = cross_plain(d2(1:end-1,:,:), d1(:,1:end-1,:));
n2 = cross_plain(d2(2:end, :, :), d1(:, 2:end, :));

% add the normals to get an average normal of the face
n = n1 + n2;

% normalize
mag = sqrt(sum(n.^2,3));
mag(mag==0)=eps;
n = n./repmat(mag,[1 1 3]);

% arrange data for surf's 'VertexNormals' property
N = NaN(size(n)+[1 1 0]);
N(2:end,2:end,:) = n;

function c = cross_plain(a,b)

% cross_plain vector cross product without overhead
%
% c = cross_plain(a,b) returns the cross product of the vectors a and b.
%
% Faster direct cross product calculation without input checking.

c = cat(3, ...
    a(:, :, 2) .* b(:, :, 3) - a(:, :, 3) .* b(:, :, 2), ...
    a(:, :, 3) .* b(:, :, 1) - a(:, :, 1) .* b(:, :, 3), ...
    a(:, :, 1) .* b(:, :, 2) - a(:, :, 2) .* b(:, :, 1));


text:
function AX = normAxes(obj, AX, varargin);
% normAxes: set all axes within a figure to have the same axes.
%
%	AX = normAxes(obj, AX, [options]);
%
% AX = normAxes(obj) sets all axes that are children of
% the specified object to have the same, maximal axes. Returns
% AX, these maximal axes. obj defaults to gcf.
%
% If you only want to normalize some axes within the figure
% (E.g., for making figure papers), you can pass obj as a
% vector of handles to those axes.
%
% You can also manually specify the axis bounds
% by specifying AX as a second argument. The format
% is [xmin xmax ymin ymax].
%
% Options include:
%
%	'xonly': will normalize each subplot only along the X axis, leaving the
%	Y axis range of each subplot as it was before. In this case, if you
%	don't pass in a value for the AX argument, it will default to the max
%	range across all axes; otherwise, it will only pay attention to the
%	first two elements of AX.
%
%	'yonly': will normalize each subplot only along the Y axis, leaving the
%	X axis range of each subplot as it was before. In this case, if you
%	don't pass in a value for the AX argument, it will default to the max
%	range across all axes; otherwise, it will only pay attention to the
%	third and fourth elements of AX.
%
%	'zoom': rather than setting the axis range AX based on the maximum
%	range across all subplots, the code will set the axis range based on
%	the minimum range across subplots. This will generally cause most
%	subplots to have data outside the plotted range, but will prevent any
%	subplot from have the data compressed in a small part of the axis
%	range.
%
%
% ras 06/05.
% ras 10/09: added optional inputs, made more sophisticated (for better or
% worse :).
if ~exist('obj', 'var') | isempty(obj), obj = get(gca, 'Parent'); end

if length(obj)==1
    haxes = findobj('Type','axes','Parent',obj);
else
    haxes = obj;
end

if isempty(haxes), AX = []; return; end

%% flags for optional behaviors
maxRange = 1; % default axis range is the max across subplots
xOnly = 0;
yOnly = 0;

% parse the options
for ii = 1:length(varargin)
	switch lower(varargin{ii})
		case 'zoom', maxRange = 0;
		case 'xonly', xOnly = 1;
		case 'yonly', yOnly = 1;
		otherwise, warning('Unrecognized option: %s', varargin{ii});
	end
end

if ~exist('AX','var') | isempty(AX)
	% get a default axis range across subplots
    for i = 1:length(haxes)
        axes(haxes(i));
        a(i,:) = axis;
	end

	if maxRange==1
	    % take the maximum axis range across all subplots
		AX = [min(a(:,1)) max(a(:,2)) min(a(:,3)) max(a(:,4))];
	else
		% take the minimum axis range across all subplots.
	    AX = [max(a(:,1)) min(a(:,2)) max(a(:,3)) min(a(:,4))];
	end
end

for i = 1:length(haxes)
    axes(haxes(i));

	if xOnly==1
		% set x range only
		tmpAX = axis;
		tmpAX(1:2) = AX(1:2);
	elseif yOnly==1
		% set y range only
		tmpAX = axis;
		tmpAX(3:4) = AX(3:4);
	else
		tmpAX = AX;
	end

	axis(tmpAX);
end

return

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function PlotIt(X1, YMatrix1)
%CREATEFIGURE(X1,YMATRIX1)
%  X1:  vector of x data
%  YMATRIX1:  matrix of y data

%  Auto-generated by MATLAB on 22-May-2012 09:40:50

% Create figure
figure1 = figure('PaperSize',[20.98 29.68],'Color',[1 1 1]);

% Create axes
axes1 = axes('Parent',figure1);
hold('all');

% Create multiple lines using matrix input to plot
plot1 = plot(X1,YMatrix1,'Parent',axes1,'Color',[0 0 0]);
set(plot1(1),'MarkerSize',2,'Marker','*','DisplayName','N=4');
set(plot1(2),'MarkerSize',3,'Marker','x','DisplayName','N=5');
set(plot1(3),'LineStyle','-.','DisplayName','N=6');
set(plot1(4),'LineStyle',':','DisplayName','N=7');
set(plot1(5),'LineStyle','--','DisplayName','N=8');
set(plot1(6),'Marker','.','DisplayName','N=9');
set(plot1(7),'Marker','o');

% Create xlabel
xlabel('Strike');

% Create ylabel
ylabel('Price');

% Create title
title('\Gamma Bermuda Option COS');

% Create legend
legend(axes1,'show');


text:
% preproc_vbm_simplified - preprocessing for voxel-based morphometry
%
% Need to specify:
% * Structural images for all subjects
%__________________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Ged Ridgway
% $Id: preproc_vbm_simplified.m 6177 2014-09-16 10:44:55Z guillaume $

if exist('preproc_vbm_simplified', 'file') && ~isdeployed
    help preproc_vbm_simplified
end
matlabbatch = {};

%% Segmentation
iSeg = 1;
ngaus  = [1 1 2 3 4 2];
native = [
    1 0 0 0 0 0
    1 1 0 0 0 0];
for c = 1:6 % tissue class c
    matlabbatch{iSeg}.spm.spatial.preproc.tissue(c).tpm = {
        fullfile(spm('dir'), 'tpm', sprintf('TPM.nii,%d', c))};
    matlabbatch{iSeg}.spm.spatial.preproc.tissue(c).ngaus = ngaus(c);
    matlabbatch{iSeg}.spm.spatial.preproc.tissue(c).native = native(:, c)';
    matlabbatch{iSeg}.spm.spatial.preproc.tissue(c).warped = [0 0];
end

%%
iDar = 2;
matlabbatch{iDar}.spm.tools.dartel.warp.images{1}(1) = ...
    cfg_dep('Segment: rc1 Images', substruct('.','val', '{}',{iSeg}, ...
    '.','val', '{}',{1}, '.','val', '{}',{1}), ...
    substruct('.','tiss', '()',{1}, '.','rc', '()',{':'}));
matlabbatch{iDar}.spm.tools.dartel.warp.images{2}(1) = ...
    cfg_dep('Segment: rc2 Images', substruct('.','val', '{}',{iSeg}, ...
    '.','val', '{}',{1}, '.','val', '{}',{1}), ...
    substruct('.','tiss', '()',{2}, '.','rc', '()',{':'}));

%% Normalise segmentations to MNI space
iNS = 3;
matlabbatch{iNS}.spm.tools.dartel.mni_norm.template(1) = ...
    cfg_dep('Run Dartel (create Templates): Template (Iteration 6)', ...
    substruct('.','val', '{}',{iDar}, '.','val', '{}',{1}, '.','val', '{}',{1}, '.','val', '{}',{1}), ...
    substruct('.','template', '()',{7}));
matlabbatch{iNS}.spm.tools.dartel.mni_norm.data.subjs.flowfields(1) = ...
    cfg_dep('Run Dartel (create Templates): Flow Fields', ...
    substruct('.','val', '{}',{iDar}, '.','val', '{}',{1}, '.','val', '{}',{1}, '.','val', '{}',{1}), ...
    substruct('.','files', '()',{':'}));
matlabbatch{iNS}.spm.tools.dartel.mni_norm.data.subjs.images{1}(1) = ...
    cfg_dep('Segment: c1 Images', ...
    substruct('.','val', '{}',{iSeg}, '.','val', '{}',{1}, '.','val', '{}',{1}), ...
    substruct('.','tiss', '()',{1}, '.','c', '()',{':'}));
matlabbatch{iNS}.spm.tools.dartel.mni_norm.preserve = 1;
matlabbatch{iNS}.spm.tools.dartel.mni_norm.fwhm = [8 8 8];

%% If run as script, open matlabbatch GUI
% NB mfilename is cfg_load_jobs if loaded from matlabbatch GUI
if strcmp(mfilename, 'preproc_vbm_simplified')
    spm_jobman('interactive', matlabbatch)
end

author: spm


text:
function test_ft_prepare_layout

% WALLTIME 00:10:00
% MEM 4gb
% DEPENDENCY ft_prepare_layout ft_plot_layout ft_plot_sens

%%
% Most of the test cases require visual inspection of the results. Nevertheless, it is useful to run them in a test batch.
% The test cases that require manual interaction should not run in batch mode.
interactive = false;

% this corresponds to the ftp directory
cd(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/layout'));

%%
% make it from a figure

if interactive
  cfg = [];
  cfg.image = 'easycap_m10_equidistant61chan.png';
  cfg.bw = 'yes';

  layout = ft_prepare_layout(cfg);
  figure; ft_plot_layout(layout)
end

%%
% simple projection of EEG electrodes
% channel 35 is at Fpz
% the nose is along +Y

clear all
close all

elec = ft_read_sens('easycap-M10.txt');

figure
ft_plot_sens(elec, 'label', 'label')
ft_plot_axes(elec, 'fontcolor', 'm');

cfg = [];
cfg.elec = elec;

cfg.skipscale = 'yes';
cfg.skipcomnt = 'yes';
cfg.rotate = 0; % 0 is appropriate for this set of electrodes

layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'convex';
cfg.mask    = 'convex';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'circle';
cfg.mask    = 'convex';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'convex';
cfg.mask    = 'circle';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

%%
% simple projection of CT151 gradiometers
% the nose is along +X

clear all
close all

load ctf151.mat % the mat files contains the variable "sens"

figure
ft_plot_sens(sens, 'label', 'label', 'chantype', 'meggrad')
ft_plot_axes(sens, 'fontcolor', 'm');

cfg = [];
cfg.grad = sens;
cfg.channel = 'MEG';

cfg.projection = 'stereographic';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.projection)

cfg.projection = 'orthographic';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.projection)

cfg.projection = 'polar';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.projection)

cfg.projection = 'gnomic'; % this works technically, but does not result in a nice layout
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.projection)

%%
% simple projection of CT151 gradiometers
% the nose is along +X

clear all
close all

load ctf151.mat % the mat files contains the variable "sens"

figure
ft_plot_sens(sens, 'label', 'label', 'chantype', 'meggrad')
ft_plot_axes(sens, 'fontcolor', 'm');

cfg = [];
cfg.grad = sens;
cfg.channel = 'MEG';

cfg.skipscale = 'yes';
cfg.skipcomnt = 'yes';

layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'convex';
cfg.mask    = 'convex';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'circle';
cfg.mask    = 'convex';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

cfg.outline = 'convex';
cfg.mask    = 'circle';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout)

%%
% simple projection of CT151 gradiometers
% the nose is along +X

clear all
close all

load ctf151.mat % the mat files contains the variable "sens"

figure
ft_plot_sens(sens, 'label', 'label', 'chantype', 'meggrad')
ft_plot_axes(sens, 'fontcolor', 'm');

cfg = [];
cfg.grad = sens;
cfg.channel = 'MEG';
cfg.projection = 'orthographic';

cfg.viewpoint = 'superior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

cfg.viewpoint = 'inferior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

cfg.viewpoint = 'anterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

cfg.viewpoint = 'posterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

cfg.channel = 'ML*';
cfg.viewpoint = 'left';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

cfg.channel = 'MR*';
cfg.viewpoint = 'right';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint);

%%
% WORKS if elec/grad and headshape contain the same coordsys
% WORKS for most common coordsys options

clear all
close all

load ctf151.mat % the mat files contains the variable "sens"
headshape = ft_read_headshape('Subject01.shape');
headshape.coordsys = 'ctf';

figure
ft_plot_sens(sens, 'label', 'label', 'chantype', 'meggrad')
ft_plot_axes(sens, 'fontcolor', 'm');
ft_plot_mesh(headshape)

cfg = [];
cfg.grad = sens;
cfg.channel = 'MEG';
cfg.headshape = headshape;
cfg.projection = 'orthographic';

cfg.viewpoint = 'superior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'inferior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'left';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'right';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'posterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'anterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

%%
% WORKS if elec/grad and mri contain the same coordsys AND mri either has brain field or can be segmented on the fly (other segmentations can be achieved via ft_prepare_mesh and cfg.headshape)
% WORKS for most common coordsys options

clear all
close all

load ctf151.mat % the mat files contains the variable "sens"
load segmentedmri

figure
ft_plot_ortho(segmentedmri.white, 'transform', segmentedmri.transform, 'style', 'intersect', 'unit', 'mm')
ft_plot_sens(sens, 'label', 'label', 'chantype', 'meggrad', 'unit', 'mm', 'edgecolor', 'y', 'fontcolor', 'g')
ft_plot_axes([], 'unit', 'mm', 'fontcolor', 'm');

cfg = [];
cfg.grad = sens;
cfg.channel = 'MEG';
cfg.mri = segmentedmri;
cfg.projection = 'orthographic';

cfg.viewpoint = 'superior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'inferior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'posterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.viewpoint = 'anterior';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.channel = 'ML*';
cfg.viewpoint = 'left';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.channel = 'MR*';
cfg.viewpoint = 'right';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

%%
% ok, here we actually start looking at what all the changes were about

lh = load('SubjectUCI29_cortex_lh');
rh = load('SubjectUCI29_cortex_rh');
elec = ft_read_sens('SubjectUCI29_elec_tal_f.mat');

% shift it a bit, otherwise it seems too high (!?)
elec.chanpos(:,3) = elec.chanpos(:,3) - 5;
elec.elecpos(:,3) = elec.elecpos(:,3) - 5;

% note that SubjectUCI29_elec_tal_fr is better, since projected onto the
% cortical sheet, but it lacks the depth electrodes

figure
ft_plot_sens(elec, 'label', 'label')
ft_plot_mesh(lh.mesh);
ft_plot_mesh(rh.mesh);
camlight

cfg = [];
cfg.elec = elec;
cfg.projection = 'orthographic';

cfg.headshape = lh.mesh;
cfg.channel = 'LP*';
cfg.viewpoint = 'left';
cfg.mask = 'convex';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

cfg.headshape(1) = lh.mesh;
cfg.headshape(2) = rh.mesh;
cfg.channel = {'LA*', 'LH*', 'RO*', 'RT*'};
cfg.viewpoint = 'inferior';
cfg.mask = 'none';
layout = ft_prepare_layout(cfg);
figure; ft_plot_layout(layout); title(cfg.viewpoint)

%%
% test the non-topographic layouts

clear all
close all

elec = ft_read_sens('easycap-M10.txt');

style = {'ordered', 'vertical', 'horizontal', 'butterfly', 'circular'};

for i=1:numel(style)
  cfg = [];
  cfg.channel = elec.label;
  cfg.layout = style{i};
  layout = ft_prepare_layout(cfg);
  figure; ft_plot_layout(layout); title(style{i})
end

%% test the ordered/horizontal/vertical layouts for iEEG

clear all
close all

nchan = 10;
ntime = 1000;
data = [];
for i=1:nchan
  data.label{i} = num2str(i);
end
data.avg = rand(nchan, ntime);
data.time = (1:ntime)/ntime;

direction = {'TB', 'BT'};
for i=1:numel(direction)
  cfg = [];
  cfg.channel = data.label;
  cfg.layout = 'vertical';
  cfg.direction = direction{i};
  layout = ft_prepare_layout(cfg);
  figure; ft_plot_layout(layout); title(direction{i})
end

direction = {'LR', 'RL'};
for i=1:numel(direction)
  cfg = [];
  cfg.channel = data.label;
  cfg.layout = 'horizontal';
  cfg.direction = direction{i};
  layout = ft_prepare_layout(cfg);
  figure; ft_plot_layout(layout); title(direction{i})
end

%%

nchan = 24; % 4x6 or 6x4 or 3x8 or 8x3
ntime = 1000;
data = [];
for i=1:nchan
  data.label{i} = num2str(i);
end
data.avg = rand(nchan, ntime);
data.time = (1:ntime)/ntime;

direction = {'LRTB' 'RLTB' 'LRBT' 'RLBT' 'TBLR' 'TBRL' 'BTLR' 'BTRL'};
for i=1:numel(direction)
  cfg = [];
  cfg.channel = data.label;
  cfg.skipscale = 'no';
  cfg.skipcomnt = 'no';
  cfg.layout = 'ordered';
  cfg.direction = direction{i};
  cfg.rows = 3;
  layout = ft_prepare_layout(cfg);
  figure; ft_plot_layout(layout); title(direction{i})
end


text:
function res = grs2rgb(img, map)

%%Convert grayscale images to RGB using specified colormap.
%	IMG is the grayscale image. Must be specified as a name of the image
%	including the directory, or the matrix.
%	MAP is the M-by-3 matrix of colors.
%
%	RES = GRS2RGB(IMG) produces the RGB image RES from the grayscale image IMG
%	using the colormap HOT with 64 colors.
%
%	RES = GRS2RGB(IMG,MAP) produces the RGB image RES from the grayscale image
%	IMG using the colormap matrix MAP. MAP must contain 3 columns for Red,
%	Green, and Blue components.
%
%	Example 1:
%	open 'image.tif';
%	res = grs2rgb(image);
%
%	Example 2:
%	cmap = colormap(summer);
% 	res = grs2rgb('image.tif',cmap);
%
% 	See also COLORMAP, HOT
%
%	Written by
%	Valeriy R. Korostyshevskiy, PhD
%	Georgetown University Medical Center
%	Washington, D.C.
%	December 2006
%
% 	vrk@georgetown.edu

% Check the arguments
if nargin<1
	error('grs2rgb:missingImage','Specify the name or the matrix of the image');
end;

if ~exist('map','var') || isempty(map)
	map = hot(64);
end;

[l,w] = size(map);

if w~=3
	error('grs2rgb:wrongColormap','Colormap matrix must contain 3 columns');
end;

if ischar(img)
	a = imread(img);
elseif isnumeric(img)
	a = img;
else
	error('grs2rgb:wrongImageFormat','Image format: must be name or matrix');
end;

% Calculate the indices of the colormap matrix
a = double(a);
a(a==0) = 1; % Needed to produce nonzero index of the colormap matrix
ci = ceil(l*a/max(a(:)));

% Colors in the new image
[il,iw] = size(a);
r = zeros(il,iw);
g = zeros(il,iw);
b = zeros(il,iw);
r(:) = map(ci,1);
g(:) = map(ci,2);
b(:) = map(ci,3);

% New image
res = zeros(il,iw,3);
res(:,:,1) = r;
res(:,:,2) = g;
res(:,:,3) = b;

repo: FCNT


text:
% Test: Mesh subdivision using the Loop scheme.
%
% Author: Jesus Mena

% Example: Box
vertices = [10 10 10; -10 10 10; 10 -10 10; -10 -10 10; 10 10 -10; -10 10 -10; 10 -10 -10; -10 -10 -10]';
faces = [1 2 3; 4 3 2; 1 3 5; 7 5 3; 1 5 2; 6 2 5; 8 6 7; 5 7 6; 8 7 4; 3 4 7; 8 4 6; 2 6 4]';

figure(1);
subplot(1,4,1);
plotMesh(vertices, faces);
for i=2:4
 subplot(1,4,i);
 [vertices, faces] = loopSubdivision(vertices, faces);
 plotMesh(vertices, faces);
end

% Example: Tetrahedron
vertices = [10 10 10; -100 10 -10; -100 -10 10; 10 -10 -10]';
faces = [1 2 3; 1 3 4; 1 4 2; 4 3 2]';

figure(2);
subplot(1,4,1);
plotMesh(vertices, faces);
for i=2:4
 subplot(1,4,i);
 [vertices, faces] = loopSubdivision(vertices, faces);
 plotMesh(vertices, faces);
end

% Example: Cylinder
vertices = [0 -5 0; 0 5 0; 10 -5 0; 9.65 -5 2.58; 8.66 -5 5; 7.07 -5 7.07; 5 -5 8.66; 2.58 -5 9.65; 0 -5 10; -2.58 -5 9.65; -5 -5 8.66; -7.07 -5 7.07; -8.66 -5 5; -9.65 -5 2.58; -10 -5 0; -9.65 -5 -2.58; -8.66 -5 -5; -7.07 -5 -7.07; -5 -5 -8.66; -2.58 -5 -9.65; -0 -5 -10; 2.58 -5 -9.65; 5 -5 -8.66; 7.07 -5 -7.07; 8.66 -5 -5; 9.65 -5 -2.58; 10 5 0 ; 9.65 5 2.58; 8.66 5 5; 7.07 5 7.07; 5 5 8.66; 2.58 5 9.65; 0 5 10; -2.58 5 9.65; -5 5 8.66; -7.07 5 7.07; -8.66 5 5; -9.65 5 2.58; -10 5 0; -9.65 5 -2.58; -8.66 5 -5; -7.07 5 -7.07; -5 5 -8.66; -2.58 5 -9.65; -0 5 -10; 2.58 5 -9.65; 5 5 -8.66; 7.07 5 -7.07; 8.66 5 -5; 9.65 5 -2.58]';
faces = [1 3 4; 1 4 5; 1 5 6; 1 6 7; 1 7 8; 1 8 9; 1 9 10; 1 10 11; 1 11 12; 1 12 13; 1 13 14; 1 14 15; 1 15 16; 1 16 17; 1 17 18; 1 18 19; 1 19 20; 1 20 21; 1 21 22; 1 22 23; 1 23 24; 1 24 25; 1 25 26; 1 26 3; 2 28 27; 2 29 28; 2 30 29; 2 31 30; 2 32 31; 2 33 32; 2 34 33; 2 35 34; 2 36 35; 2 37 36; 2 38 37; 2 39 38; 2 40 39; 2 41 40; 2 42 41; 2 43 42; 2 44 43; 2 45 44; 2 46 45; 2 47 46; 2 48 47; 2 49 48; 2 50 49; 2 27 50; 3 27 28; 3 28 4; 4 28 29; 4 29 5; 5 29 30; 5 30 6; 6 30 31; 6 31 7; 7 31 32; 7 32 8; 8 32 33; 8 33 9; 9 33 34; 9 34 10; 10 34 35; 10 35 11; 11 35 36; 11 36 12; 12 36 37; 12 37 13; 13 37 38; 13 38 14; 14 38 39; 14 39 15; 15 39 40; 15 40 16; 16 40 41; 16 41 17; 17 41 42; 17 42 18; 18 42 43; 18 43 19; 19 43 44; 19 44 20; 20 44 45; 20 45 21; 21 45 46; 21 46 22; 22 46 47; 22 47 23; 23 47 48; 23 48 24; 24 48 49; 24 49 25; 25 49 50; 25 50 26; 26 50 27; 26 27 3]';

figure(3);
subplot(1,4,1);
plotMesh(vertices, faces);
for i=2:4
 subplot(1,4,i);
 [vertices, faces] = loopSubdivision(vertices, faces);
 plotMesh(vertices, faces);
end

% Example: Grid
vertices = [-4 -4 0; -2 -4 0; 0 -4 0; 2 -4 0; 4 -4 0; -4 -2 0; -2 -2 0; 0 -2 0; 2 -2 0; 4 -2 0; -4 0 0; -2 0 0; 0 0 0; 2 0 0; 4 0 0; -4 2 0; -2 2 0; 0 2 0; 2 2 0; 4 2 0; -4 4 0; -2 4 0; 0 4 0; 2 4 0; 4 4 0]';
faces = [7 2 1; 1 6 7; 8 3 2; 2 7 8; 9 4 3; 3 8 9; 10 5 4; 4 9 10; 12 7 6; 6 11 12; 13 8 7; 7 12 13; 14 9 8; 8 13 14; 15 10 9; 9 14 15; 17 12 11; 11 16 17; 18 13 12; 12 17 18; 19 14 13; 13 18 19; 20 15 14; 14 19 20; 22 17 16; 16 21 22; 23 18 17; 17 22 23; 24 19 18; 18 23 24; 25 20 19; 19 24 25]';

figure(4);
subplot(1,4,1);
plotMesh(vertices, faces);
for i=2:4
 subplot(1,4,i);
 [vertices, faces] = loopSubdivision(vertices, faces);
 plotMesh(vertices, faces);
end


text:
fclose all;
clear;

%Time configuration
TIME.interval=[10 2500];    %start/stop
TIME.dt=1/400;  %sampling time (required only to seek faster. dt is actually computed from the file)
TIME.salign=25;   %static alignment duration for the beginning
TIME.zuptInterval=[40 70];    %Time intervals to apply zupt
TIME.zuptDt=0.5;  %minimum time between 2 zupt updates
TIME.maxCovStep=0.1; %maximum covariance propagation step

%Observation params
OBS.odoR=0.2^2;  %(m/s)^2
OBS.useNonHolo=1;   %0:Don't use, 1:use
OBS.nonHoloR=0.1^2; %(m/s)^2
OBS.gpsR=0.1^2'; %(m)^2
OBS.zuptR=0.1^2;
OBS.LA=[0.2;0;0];

INDIR='/home/yigiter/Desktop/Dropbox/INS/SAGE/mfiles/data/';
OUTDIR='/home/yigiter/Desktop/Dropbox/INS/SAGE/mfiles/data/output/';

IMUTYPE=2;

%open files
fimu=fopen([INDIR 'imudata.bin'],'r');
imudata=fread(fimu,7,'double');
sr_a=round((TIME.interval(1)-imudata(1))/TIME.dt);
if sr_a>0
    fseek(fimu, sr_a*7*8, 0);
end

fodo=fopen([INDIR 'odo.txt'],'rt');
fgps=fopen([INDIR 'gps.txt'],'rt');

ffilres=fopen([OUTDIR 'navresult.bin'],'w');
fupdres=fopen([OUTDIR 'updresult.bin'],'w');
fpreres=fopen([OUTDIR 'preresult.bin'],'w');
fnavres=fopen([OUTDIR 'prenavresult.bin'],'w');

%Initial PVA
iniLlh=[39.88864*pi/180;32.78002*pi/180;1112];
Llh=iniLlh;
Vn=[0;0;0];
qbn=[1;0;0;0]; %Unknown:To be determined during coarse alignment

%Load IMU error definitions
IMU_ERRDEF=imu_err_defs_v000(IMUTYPE); %needed just to initialize P. The rest is handled within sys model

%Initial Covariance matrix
nst=21;
P=zeros(nst);
[Rn, Re, ~, sL, cL, WIE_E]=geoparam_v000(Llh);
Rappx=sqrt((Rn+Llh(3))*(Re+Llh(3)));
P(1:2,1:2)=eye(2)*(5/Rappx)^2; %position errors in meter
P(3,3)=5^2; %position errors in meter
P(4:6,4:6)=eye(3)*0.05^2; %vel error in m/s
P(7:9,7:9)=eye(3); %attitude errors in radian (will be determined later during coarse alignment)
P(10:12,10:12)=eye(3)*IMU_ERRDEF.acc_bias_var;
P(13:15,13:15)=eye(3)*IMU_ERRDEF.gyro_bias_var;
P(16:18,16:18)=eye(3)*IMU_ERRDEF.acc_scale_var;
P(19:21,19:21)=eye(3)*IMU_ERRDEF.gyro_scale_var;

%%%%%Static Alignment
%%Step I:Coarse align
coarse_dur=TIME.salign;
%Compute the mean of imu data during the coarse alignment period
datbuf=fread(fimu, [7 round(coarse_dur/TIME.dt)], 'double');
meandat=mean(datbuf(2:end,:),2)/TIME.dt;
preimutime=datbuf(1,end);   %the last imu time
clear datbuf;

%%%Compute the alignment between the geodetic and the body frame
z_axis=-meandat(4:6)/norm(meandat(4:6));
ngyro=meandat(1:3)/norm(meandat(1:3));
vr_a=cross(z_axis,ngyro);
y_axis=vr_a/norm(vr_a);
x_axis=cross(y_axis,z_axis);
Cnb=[x_axis y_axis z_axis];
qbn=dcm2quat_v000(Cnb');

%%Step II:Determine the attitude covariance
%%Note I will ignore the initial cross correlation between the imu states
%%and the initial attitude. I will use convervative correlation values and
%%expect the initial ZUPT to take care of cross correlations.
P(7,7)=(IMU_ERRDEF.acc_vrw/10+IMU_ERRDEF.acc_bias_var)/9.81^2; %assume only a 1 sec average.
P(8,8)=(IMU_ERRDEF.acc_vrw/10+IMU_ERRDEF.acc_bias_var)/9.81^2;
P(9,9)=(IMU_ERRDEF.gyro_arw/10+IMU_ERRDEF.gyro_bias_var)/(7.2e-5*cos(Llh(1)))^2;
P(9,9)=P(9,9)+(3*pi/180)^2; %forget about the flicker and use a random value for the initial bias error.

%%Step III:Self calibrate based on zero-motion
%TODO Later

%%%%%Discard all observation data before the current time
ododata=fscanf(fodo,'%f%f',[1,2]);
while (ododata(1)<=preimutime)
    ododata=fscanf(fodo,'%f%f',[1,2]);
end
gpsdata=fscanf(fgps,'%f%f',[1,7]);
while (gpsdata(1)<=preimutime)
    gpsdata=fscanf(fgps,'%f%f',[1,7]);
end

%%%%%Start the main INS
%prepare for the loop
initime=preimutime;
covupt_time=preimutime;  %the time that we last updated the covariance
zupt_time=preimutime;   %the time that we last applied a zupt
imuaccum=zeros(6,1);
imuerrors=zeros(12,1);
gpsctr=0;

%I will accumulate 4 imu samples before processing.
imudata=fread(fimu, [7,4], 'double'); %imudata must be in increment form (not rate). If the file contains rate data, correct it here.
curimutime=imudata(1,end);

while (~feof(fimu))
    %Write result to the files
    vr_a=posdiff_v000(Llh(1:2)', Llh(3), iniLlh);
    vr_c=quat2dcm_v000(qbn);
    fwrite(ffilres,[preimutime;vr_a';Vn;dcm2euler_v000(vr_c)*180/pi;diag(P)],'double');

    if (imudata(1)-initime)>60
        disp(['Process Time:' num2str(imudata(1)/60)]);
        initime=imudata(1);
    end

    %downsample the data by accumulating it. (here I will downsample by a
    %factor of 4)
    angleinc=sum(imudata(2:4,:),2);
    velinc=sum(imudata(5:7,:),2);
    dt=curimutime-preimutime;

    %Calibrate the imu
    angleinc=angleinc-(imuerrors(4:6)*dt+diag(angleinc)*imuerrors(10:12)/1000);
    velinc=velinc-(imuerrors(1:3)*dt+diag(velinc)*imuerrors(7:9)/1000);

    %imu data accumulator for the covariance update
    imuaccum=imuaccum+[angleinc;velinc];

    %run strapdown with angle and velocity increments
    [qbn, Vn, Llh]=strapdown_ned_quat_v000(qbn, Vn, Llh, velinc, angleinc, dt);

    %Check if we are going to update.
    %Hardcode the ZUPT intervals here.
    cov_upd=0;
    zupt_upd=0;
    odo_upd=0;
    gps_upd=0;
    if (curimutime-covupt_time)>=TIME.maxCovStep
        cov_upd=1;
    elseif (curimutime>ododata(1))
        cov_upd=1;
        odo_upd=1;
    elseif (curimutime>gpsdata(1))
        cov_upd=1;
        gps_upd=1;
    elseif curimutime-zupt_time>=TIME.zuptDt
        if (curimutime>TIME.zuptInterval(1,1) && curimutime<TIME.zuptInterval(1,2))
            cov_upd=1;
            zupt_upd=1;
        elseif (curimutime>TIME.zuptInterval(2,1) && curimutime<TIME.zuptInterval(2,2))
            cov_upd=1;
            zupt_upd=1;
        end
    end

    %Update the covariance
    if (cov_upd)
        %propagate the covariance
        covdt=curimutime-covupt_time;
        %[STM, Qd]=sys_ned_dcm_v001(Llh, Vn, qbn, imuaccum(4:6)/covdt, imuaccum(1:3)/covdt, covdt);
        [STM, Qd]=sys_llh_phipsi_v000(Llh, Vn, qbn, imuaccum(4:6)/covdt, imuaccum(1:3)/covdt, covdt, 1, IMUTYPE);
        P=STM*P*STM'+Qd;

        %Propagate the imu error states
        imuerrors=STM(10:end,10:end)*imuerrors;

        covupt_time=curimutime;
        imuaccum=zeros(6,1);

        %%Smoother Stuff
        %Record the predicted solutions
        fwrite(fnavres,[curimutime;Llh;Vn;qbn;imuerrors],'double');
        fwrite(fpreres,[curimutime;mat2vec_v000(P);STM(:);mat2vec_v000(Qd)],'double');  %I do not need Q for smoothing. But I am going to use it for disturbance estimation.
    end

    %%If there is any observation to process
    if (gps_upd || odo_upd || zupt_upd)
        %Smoother stuff
        STM_upd=eye(21);
        smo_dx_inp=0;
        smo_dP_inp=0;

        %Apply zupt
        if (zupt_upd)
            inno=Vn;
            H=[zeros(3) eye(3) zeros(3,15)];
            R=eye(3)*OBS.zuptR;

            %Kalman
            klmn_scrpt;

            zupt_time=curimutime;
        end

        %%Odometer updates
        if (odo_upd)  %we have an odometer data
            Cnb=quat2dcm_v000(qbn)';
            if (OBS.useNonHolo)
                inno=[ododata(2);0;0]-Cnb*Vn;
                H=[zeros(3) -Cnb Cnb*skew(Vn) zeros(3,12)];
                R=diag([OBS.odoR, OBS.nonHoloR, OBS.nonHoloR]);
            else
                inno=ododata(2)-Cnb(1,:)*Vn;
                H=[zeros(1,3) -Cnb(1,:) Cnb(1,:)*skew(Vn) zeros(1,12)];
                R=OBS.odoR;
            end

            %Kalman
            klmn_scrpt;

            %Read the next odometer data
            ododata=fscanf(fodo,'%f%f',[1,2]);
            if (isempty(ododata))
                ododata(1)=inf;
            end
        end

        %%GPS updates
        if (gps_upd)
            vr_a=posdiff_v000(Llh(1:2)', Llh(3), [gpsdata(2:3)*pi/180 gpsdata(4)]);
            disp(vr_a');

            lever=diag([1/Rappx, 1/Rappx,1])*quatrot_v000(qbn,OBS.LA,0); %Thx Jianzhu
            inno=Llh-[gpsdata(2:3)*pi/180 gpsdata(4)]'-lever;
            H=[eye(3) zeros(3,18)]; %Completely ignore the effect of Cbn errors on level-arm compensation. If you want, use H(:,7:9) = skew(OBS.LA);
            R=diag([1/Rappx^2, 1/Rappx^2,1])*OBS.gpsR;

            %Kalman
            klmn_scrpt;

            %Read the new gps data
            gpsdata=fscanf(fgps,'%f%f',[1,7]);
            if (isempty(gpsdata))
                gpsdata(1)=inf;
            end

            gpsctr=gpsctr+1;
        end

        %%Smoother stuff
        mx_a=mat2vec_v000(smo_dP_inp);
        mx_b=STM_upd(:);
        fwrite(fupdres,[curimutime;smo_dx_inp;mx_a;mx_b],'double');

        if (gpsctr==2)
            break;
        end
    end

    %Read the next imu data
    preimutime=curimutime;
    imudata=fread(fimu, [7,4], 'double');
    if (isempty(imudata))
        break;
    end
    curimutime=imudata(1,end);
end

%Write the latest filtered result
vr_a=posdiff_v000(Llh(1:2)', Llh(3), iniLlh);
vr_c=quat2dcm_v000(qbn);
fwrite(ffilres,[preimutime;vr_a';Vn;dcm2euler_v000(vr_c)*180/pi],'double');

fclose all;

repo: ekfmonoslam


text:
function modes = gtmlmode(net, data)
%GTMLMODE Mode responsibility for data in a GTM.
%
%	Description
%	 MODES = GTMLMODE(NET, DATA) takes a GTM structure NET, and computes
%	the modes of the responsibility  distributions for each data point in
%	DATA.  These will always lie at one of the latent space sample points
%	NET.X.
%
%	See also
%	GTM, GTMPOST, GTMLMEAN
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check for consistency
errstring = consist(net, 'gtm', data);
if ~isempty(errstring)
  error(errstring);
end

R = gtmpost(net, data);
% Mode is maximum responsibility
[max_resp, max_index] = max(R, [], 2);
modes = net.X(max_index, :);

author: bayesnet

repo: bnt

save_path: github-repos/MATLAB/bayesnet-bnt


text:
 classdef FGaussDiscontinuousFunction < handle
    % nDimf * nGaus * nElem
    properties (Constant, Access = public)
        fType = 'GAUSSPOINTS'
    end

    properties (Access = public)
        ndimf
        fValues
        quadrature
    end

    properties (Access = private)
        mesh
    end

    properties (Access = private)
    end

    methods (Access = public)

        function obj = FGaussDiscontinuousFunction(cParams)
            obj.init(cParams)
        end

        function fxV = evaluate(obj, xV)
            assert(isequal(xV, obj.quadrature.posgp), 'Gauss points do not match')
            fxV = obj.fValues;
        end

        function dNdx  = computeCartesianDerivatives(obj, quad)
            assert(isequal(quad,obj.quadrature), 'Quadrature does not match');
            nElem = size(obj.mesh.connec,1);
            nNode = obj.mesh.interpolation.nnode;
            nDime = obj.mesh.interpolation.ndime;
            nGaus = quad.ngaus;
            invJ  = obj.mesh.computeInverseJacobian(quad,obj.mesh.interpolation);
            dShapeDx  = zeros(nDime,nNode,nElem,nGaus);
            for igaus = 1:nGaus
                dShapes = obj.mesh.interpolation.deriv(:,:,igaus);
                for jDime = 1:nDime
                    invJ_JI   = invJ(:,jDime,:,igaus);
                    dShape_KJ = dShapes(jDime,:);
                    dSDx_KI   = bsxfun(@times, invJ_JI,dShape_KJ);
                    dShapeDx(:,:,:,igaus) = dShapeDx(:,:,:,igaus) + dSDx_KI;
                end
            end
            dNdx = dShapeDx;
        end

        function applyVoigtNotation(obj)
            switch obj.ndimf
                case 4
                    obj.applyVoigt2D()
                case 9
                    obj.applyVoigt3D()
            end
        end

        function plot(obj)
            s.mesh = obj.mesh;
            proj = Projector_toP1(s);
            p1fun = proj.project(obj);
            p1fun.plot();
        end

        function print(obj, s)
            s.mesh = obj.mesh;
            s.fun  = {obj};
            p = FunctionPrinter(s);
            p.print();
        end

        function [res, pformat] = getDataToPrint(obj)
            nElem = size(obj.fValues, 3);
            nGaus = obj.quadrature.ngaus;
            s.nDimf   = obj.ndimf;
            s.nData   = nElem*nGaus;
            s.nGroup  = nElem;
            s.fValues = obj.getFormattedFValues();
            fps = FunctionPrintingSettings(s);
            [res, pformat] = fps.getDataToPrint();
        end

        function dofConnec = computeDofConnectivity(obj)
            % This assumes that FGaussDiscFun comes from a P1Fun...
            conne  = obj.mesh.connec;
            nDimf  = obj.ndimf;
            nNode  = size(conne, 2);
            nDofsE = nNode*nDimf;
            dofsElem  = zeros(nDofsE,size(conne,1));
            for iNode = 1:nNode
                for iUnkn = 1:nDimf
                    idofElem   = nDimf*(iNode - 1) + iUnkn;
                    globalNode = conne(:,iNode);
                    idofGlobal = nDimf*(globalNode - 1) + iUnkn;
                    dofsElem(idofElem,:) = idofGlobal;
                end
            end
            dofConnec = dofsElem;
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.fValues    = cParams.fValues;
            obj.quadrature = cParams.quadrature;
            obj.ndimf      = size(cParams.fValues,1);
            obj.mesh       = cParams.mesh;
        end

        function applyVoigt2D(obj)
            nGaus = obj.quadrature.ngaus;
            nElem = size(obj.fValues,3);
            fV(1,:,:) = obj.fValues(1,:,:); % xx
            fV(2,:,:) = obj.fValues(4,:,:); % yy
            fV(3,:,:) = obj.fValues(2,:,:) + obj.fValues(3,:,:); % xy
            fV = reshape(fV, [3 nGaus nElem]);
            obj.fValues = fV;
            obj.ndimf = 3;
        end

        function applyVoigt3D(obj)
            nGaus = obj.quadrature.ngaus;
            nElem = size(obj.fValues,3);
            fV(1,:,:) = obj.fValues(1,:,:); % xx
            fV(2,:,:) = obj.fValues(5,:,:); % yy
            fV(3,:,:) = obj.fValues(9,:,:); % zz
            fV(4,:,:) = obj.fValues(2,:,:) + obj.fValues(4,:,:); % xy
            fV(5,:,:) = obj.fValues(3,:,:) + obj.fValues(7,:,:); % xz
            fV(6,:,:) = obj.fValues(6,:,:) + obj.fValues(8,:,:); % yz
            fV = reshape(fV, [6 nGaus nElem]);
            obj.fValues = fV;
            obj.ndimf = 6;
        end

        % Printing
        function fM = getFormattedFValues(obj)
            fV = obj.fValues;
            nGaus = obj.quadrature.ngaus;
            nComp = obj.ndimf;
            nElem = size(obj.fValues, 3);
            fM  = zeros(nGaus*nElem,nComp);
            for iStre = 1:nComp
                for iGaus = 1:nGaus
                    rows = linspace(iGaus,(nElem - 1)*nGaus + iGaus,nElem);
                    fM(rows,iStre) = fV(iStre,iGaus,:);
                end
            end
        end

    end

end

author: SwanLab

repo: Swan

save_path: github-repos/MATLAB/SwanLab-Swan


text:
function gb=gabor_fn(bw,gamma,psi,lambda,theta)
% bw    = bandwidth, (1)
% gamma = aspect ratio, (0.5)
% psi   = phase shift, (0)
% lambda= wave length, (>=2)
% theta = angle in rad, [0 pi)

sigma = lambda/pi*sqrt(log(2)/2)*(2^bw+1)/(2^bw-1);
sigma_x = sigma;
sigma_y = sigma/gamma;

sz=fix(8*max(sigma_y,sigma_x));
if mod(sz,2)==0, sz=sz+1;end

% alternatively, use a fixed size
% sz = 60;

[x y]=meshgrid(-fix(sz/2):fix(sz/2),fix(sz/2):-1:fix(-sz/2));
% x (right +)
% y (up +)

% Rotation
x_theta=x*cos(theta)+y*sin(theta);
y_theta=-x*sin(theta)+y*cos(theta);

gb=exp(-0.5*(x_theta.^2/sigma_x^2+y_theta.^2/sigma_y^2)).*cos(2*pi/lambda*x_theta+psi);
imshow(gb/2+0.5);