%{
 * Copyright (C) 2013-2020, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function cost = cost4Points(theta_x, theta_y, theta_z, T, X, Y, intrinsic)

%         function cost = cost4Points(app, xi, X, Y, intrinsic)
%             H = expm(hat(app, xi'));
%             R = H(1:3, 1:3);
%             T = H(1:3, 4);
    H = eye(4);
    H(1:3,1:3) = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    H(1:3,4) = T';
    P = intrinsic * [eye(3) zeros(3,1)] * H;
    cost = verifyCornerAccuracy(X, Y, P);
end


function [ a, R, T, T3D, params, error, shapeOrtho ] = fit_PDM_ortho_proj_to_2D( M, E, V, shape2D, f, cx, cy)
%FITPDMTO2DSHAPE Summary of this function goes here
%   Detailed explanation goes here

    params = zeros(size(E));

    hidden = false;

    % if some of the points are unavailable modify M, V, and shape2D (can
    % later infer the actual shape from this)
    if(sum(shape2D(:)==0) > 0)

        hidden = true;
        % which indices to remove
        inds_to_rem = shape2D(:,1) == 0 | shape2D(:,2) == 0;

        shape2D = shape2D(~inds_to_rem,:);

        inds_to_rem = repmat(inds_to_rem, 3, 1);

        M_old = M;
        V_old = V;

        M = M(~inds_to_rem);
        V = V(~inds_to_rem,:);

    end

    num_points = numel(M) / 3;

    m = reshape(M, num_points, 3)';
    width_model = max(m(1,:)) - min(m(1,:));
    height_model = max(m(2,:)) - min(m(2,:));

    bounding_box = [min(shape2D(:,1)), min(shape2D(:,2)),...
                    max(shape2D(:,1)), max(shape2D(:,2))];

    a = (((bounding_box(3) - bounding_box(1)) / width_model) + ((bounding_box(4) - bounding_box(2))/ height_model)) / 2;

    tx = (bounding_box(3) + bounding_box(1))/2;
    ty = (bounding_box(4) + bounding_box(2))/2;

    % correct it so that the bounding box is just around the minimum
    % and maximum point in the initialised face
    tx = tx - a*(min(m(1,:)) + max(m(1,:)))/2;
    ty = ty - a*(min(m(2,:)) + max(m(2,:)))/2;

    R = eye(3);
    T = [tx; ty];

    currShape = getShapeOrtho(M, V, params, R, T, a);

    currError = getRMSerror(currShape, shape2D);

    reg_rigid = zeros(6,1);
    regFactor = 20;
    regularisations = [reg_rigid; regFactor ./ E]; % the above version, however, does not perform as well
    regularisations = diag(regularisations)*diag(regularisations);

    red_in_a_row = 0;

    for i=1:1000

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

        % Now find the current residual error
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error_res = shape2D - currShape;

        eul = Rot2Euler(R);

        p_global = [a; eul'; T];

        % get the Jacobians
        J = CalcJacobian(M, V, params, p_global);

        % RLMS style update
        p_delta = (J'*J + regularisations) \ (J'*error_res(:) - regularisations*[p_global;params]);

        [params, p_global] = CalcReferenceUpdate(p_delta, params, p_global);

        a = p_global(1);
        R = Euler2Rot(p_global(2:4));
        T = p_global(5:6);

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error = getRMSerror(currShape, shape2D);

        if(0.999 * currError < error)
            red_in_a_row = red_in_a_row + 1;
            if(red_in_a_row == 5)
                break;
            end
        end

        currError = error;

    end

    if(hidden)
        shapeOrtho = getShapeOrtho(M_old, V_old, params, R, T, a);
    else
        shapeOrtho = currShape;
    end
    if(nargin == 7)

        Zavg = f / a;
        Xavg = (T(1) - cx) / a;
        Yavg = (T(2) - cy) / a;

        T3D = [Xavg;Yavg;Zavg];
    else
        T3D = [0;0;0];
    end

end

function [shape2D] = getShapeOrtho(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    shape3D = getShape3D(M, V, p);
    shape2D = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape2D] = getShapeOrthoFull(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    T = [T; 0];
    shape3D = getShape3D(M, V, p);
    shape2D = a * R*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape3D] = getShape3D(M, V, params)

    shape3D = M + V * params;
    shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

end

function [error] = getRMSerror(shape2Dv1, shape2Dv2)

    error = sqrt(mean(reshape(shape2Dv1 - shape2Dv2, numel(shape2Dv1), 1).^2));

end

% This calculates the combined rigid with non-rigid Jacobian
function J = CalcJacobian(M, V, p, p_global)

    n = size(M, 1)/3;

    non_rigid_modes = size(V,2);

    J = zeros(n*2, 6 + non_rigid_modes);

    % now the layour is
    % ---------- Rigid part -------------------|----Non rigid part--------|
    % dx_1/ds, dx_1/dr1, ... dx_1/dtx, dx_1/dty dx_1/dp_1 ... dx_1/dp_m
    % dx_2/ds, dx_2/dr1, ... dx_2/dtx, dx_2/dty dx_2/dp_1 ... dx_2/dp_m
    % ...
    % dx_n/ds, dx_n/dr1, ... dx_n/dtx, dx_n/dty dx_n/dp_1 ... dx_n/dp_m
    % dy_1/ds, dy_1/dr1, ... dy_1/dtx, dy_1/dty dy_1/dp_1 ... dy_1/dp_m
    % ...
    % dy_n/ds, dy_n/dr1, ... dy_n/dtx, dy_n/dty dy_n/dp_1 ... dy_n/dp_m

    % getting the rigid part
    J(:,1:6) = CalcRigidJacobian(M, V, p, p_global);

    % constructing the non-rigid part
    R = Euler2Rot(p_global(2:4));
    s = p_global(1);

    % 'rotate' and 'scale' the principal components

    % First reshape to 3D
    V_X = V(1:n,:);
    V_Y = V(n+1:2*n,:);
    V_Z = V(2*n+1:end,:);

    J_x_non_rigid = s*(R(1,1)*V_X + R(1,2)*V_Y + R(1,3)*V_Z);
    J_y_non_rigid = s*(R(2,1)*V_X + R(2,2)*V_Y + R(2,3)*V_Z);

    J(1:n, 7:end) = J_x_non_rigid;
    J(n+1:end, 7:end) = J_y_non_rigid;

end

function J = CalcRigidJacobian(M, V, p, p_global)

 	n = size(M, 1)/3;

	% Get the current 3D shape (not affected by global transform, as this
	% is how the Jacobian was derived (for derivation please see
	% ../derivations/orthoJacobian
	shape3D = GetShape3D(M, V, p);

	% Get the rotation matrix corresponding to current global orientation
	R = Euler2Rot(p_global(2:4));
	s = p_global(1);

    % Rigid Jacobian is laid out as follows
    % dx_1/ds, dx_1/dr1, dx_1/dr2, dx_1/dr3, dx_1/dtx, dx_1/dty
    % dx_2/ds, dx_2/dr1, dx_2/dr2, dx_2/dr3, dx_2/dtx, dx_2/dty
    % ...
    % dx_n/ds, dx_n/dr1, dx_n/dr2, dx_n/dr3, dx_n/dtx, dx_n/dty
    % dy_1/ds, dy_1/dr1, dy_1/dr2, dy_1/dr3, dy_1/dtx, dy_1/dty
    % ...
    % dy_n/ds, dy_n/dr1, dy_n/dr2, dy_n/dr3, dy_n/dtx, dy_n/dty

    J = zeros(n*2, 6);

    % dx/ds = X * r11  + Y * r12 + Z * r13
    % dx/dr1 =  s*(r13 * Y - r12 * Z)
    % dx/dr2 = -s*(r13 * X - r11 * Z)
    % dx/dr3 =  s*(r12 * X - r11 * Y)
    % dx/dtx = 1
    % dx/dty = 0

    % dy/ds = X * r21  + Y * r22 + Z * r23
    % dy/dr1 =  s * (r23 * Y - r22 * Z)
    % dy/dr2 = -s * (r23 * X - r21 * Z)
    % dy/dr3 =  s * (r22 * X - r21 * Y)
    % dy/dtx = 0
    % dy/dty = 1

    % set the Jacobian for x's

    % with respect to scaling factor
    J(1:n,1) = shape3D * R(1,:)';

    % with respect to angular rotation around x, y, and z axes

    % Change of x with respect to change in axis angle rotation
	dxdR = [      0,  R(1,3), -R(1,2);
            -R(1,3),       0,  R(1,1);
             R(1,2), -R(1,1),      0];

    J(1:n,2:4) = s*(dxdR * shape3D')';

    % with respect to translation
    J(1:n,5) = 1;
    J(1:n,6) = 0;

    % set the Jacobian for y's

    % with respect to scaling factor
    J(n+1:end,1) = shape3D * R(2,:)';

     % with respect to angular rotation around x, y, and z axes

    % Change of y with respect to change in axis angle rotation
    dydR = [      0,  R(2,3), -R(2,2);
            -R(2,3),       0,  R(2,1);
             R(2,2), -R(2,1),      0];

     J(n+1:end,2:4) = s*(dydR * shape3D')';

     % with respect to translation
     J(n+1:end,5) = 0;
     J(n+1:end,6) = 1;

end

% This updates the parameters based on the updates from the RLMS
function [non_rigid, rigid] = CalcReferenceUpdate(params_delta, current_non_rigid, current_global)

    rigid = zeros(6, 1);
    % Same goes for scaling and translation parameters
    rigid(1) = current_global(1) + params_delta(1);
    rigid(5) = current_global(5) + params_delta(5);
    rigid(6) = current_global(6) + params_delta(6);

    % for rotation however, we want to make sure that the rotation matrix
    % approximation we have
	% R' = [1, -wz, wy
	%       wz, 1, -wx
	%       -wy, wx, 1]
    % is a legal rotation matrix, and then we combine it with current
    % rotation (through matrix multiplication) to acquire the new rotation

	R = Euler2Rot(current_global(2:4));

    wx = params_delta(2);
    wy = params_delta(3);
    wz = params_delta(4);

    R_delta = [1, -wz, wy;
               wz, 1, -wx;
               -wy, wx, 1];

	% Make sure R_delta is orthonormal
	R_delta = OrthonormaliseRotation(R_delta);

    % Combine rotations
	R_final = R * R_delta;

	% Extract euler angle
	euler = Rot2Euler(R_final);

	rigid(2:4) = euler;

    if(length(params_delta) > 6)
        % non-rigid parameters can just be added together
        non_rigid = params_delta(7:end) +  current_non_rigid;
    else
        non_rigid = current_non_rigid;
    end

end

function R_ortho = OrthonormaliseRotation(R)

    % U * V' is basically what we want, as it's guaranteed to be
    % orthonormal
    [U, ~, V] = svd(R);

    % We also want to make sure no reflection happened

    % get the orthogonal matrix from the initial rotation matrix
    X = U*V';

    % This makes sure that the handedness is preserved and no reflection happened
    % by making sure the determinant is 1 and not -1
    W = eye(3);
    W(3,3) = det(X);
    R_ortho = U*W*V';
end


%%*****************************************************************
%% schurmat_lblk: compute A*D*A'
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,dd);

   global idxdenAl

   iter = par.iter;
   n = sum(blk{p,2});

   if (iter==1)
      idxdenAl{p} = checkdense(At{p}');
   end
   ddsch = dd{p};
   if ~isempty(idxdenAl{p});
      idxden = idxdenAl{p};
      len = length(idxden);
      Ad = At{p}(idxden,:)' *spdiags(sqrt(ddsch(idxden)),0,len,len);
      UU = [UU, Ad];
      if isempty(EE)
         count = 0;
      else
         count = max(max(EE(:,1)),max(EE(:,2)));
      end
      tmp = count + [1:len]';
      EE = [EE; [tmp, tmp, -ones(len,1)] ];
      ddsch(idxden) = zeros(len,1);
   end
   schurtmp = At{p}' *spdiags(ddsch,0,n,n) *At{p};
   schur = schur + schurtmp;
%%*******************************************************************


function s = y_to_s_eg_civil ( y )

%*****************************************************************************80
%
%% Y_TO_S_EG_CIVIL writes an Egyptian Civil year into a string.
%
%  Format:
%
%    EN YearNumber
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Discussion:
%
%    "EN" stands for the Era of Nabonassar, a Babylonian king who
%    acceded in 747 BC, used by the astronomer Ptolemy to assign
%    an artificial starting year for the Egyptian calendar.
%
%  Modified:
%
%    28 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, string S, a representation of the year.
%
  s = sprintf ( 'EN %d', y );

  return
end


function dec = sll_decodemaxcorr(data,condid,varargin)

%preliminary code, undocumented; please do not distribute
%data is 3D (variables, timepoints,observations)
%condid is a cell of strings denoting the condition id for each observation
%
%temporalgen output: dsvm.d: (test time x train time x conditions)
%multiclass output: dsvm.d: (actual condition x predicted condition x conditions)
%
% example:
%  d = sll_decodemaxcorr(data,condid,'numpermutation',10,'verbose',2);
%
% Author: Dimitrios Pantazis

%% parse inputs

numpermutation  = sll_inputparser(varargin,'numpermutation',100, @(x) isscalar(x) && x>0 && x == round(x) );
kfold           = sll_inputparser(varargin,'kfold',5, @(x) isscalar(x) && x>=2 && x == round(x));
method          = sll_inputparser(varargin,'method','pairwise',{'pairwise','temporalgen','multiclass'});
whiten          = sll_inputparser(varargin,'whiten',true);
verbose         = sll_inputparser(varargin,'verbose',false);

%% check inputs and convert string labels to numbers

[condlabel,condidval,conditiongen,data2,condlabel2,condidval2] = sll_checkdatacondid(data,condid,varargin{:});

%% initialize variables

ncond = length(condlabel); %number of conditions
rng('shuffle'); %seed the random number generator based on the current time
d = 0; %store decoding results
d2 = 0; %store decoding results (if cross-condition generalized)

%% compute cross-validated Euclidean distance

for p = 1:numpermutation

    %% verbose permutation

    if verbose & ~rem(p,verbose)
        disp(['Permutation: ' num2str(p) ' out of ' num2str(numpermutation)]);
    end

    %% assign data to k folds

    foldid = sll_createfolds(condidval,kfold);
    for c = 1:ncond
        traindata{c} = mean(data(:,:,condidval==c & foldid<kfold),3);
        testdata{c} = mean(data(:,:,condidval==c & foldid==kfold),3);
        if conditiongen
            n = nnz(condidval==c & foldid==kfold); %we need that many elements from data2
            cndx = find(condidval2==c); %available elements in condition c
            testdata2{c} = mean(data2(:,:,cndx(randperm(length(cndx),n))),3);
        end
    end

    %% multivariate noise normalization (whiten data)

    if whiten
        %compute whitening matrix using only training data
        down = max(round(size(data,2)/100),1); %downsample the data to make computation fast
        W = sll_whitenmatrix(data(:,1:down:end,foldid<kfold),condidval(foldid<kfold));
        %apply whitening matrix
        for c = 1:ncond
            traindata{c} = W*traindata{c}; %multiply data with whitener
            testdata{c} = W*testdata{c}; %multiply data with whitener
            if conditiongen
                testdata2{c} = W*testdata2{c};
            end
        end
    end

    %% decode using max correlation
    d = d + decodemaxcorr(traindata,testdata,method);
    if conditiongen
        d2 = d2 + decodemaxcorr(traindata,testdata2,method);
    end

end

%% normalize output

scale = 100/numpermutation/2;
if strcmp(method,'multiclass')
    scale = scale*2;
end
d = d*scale;
if conditiongen
    d2 = d2*scale;
end

%% parse output

dec.d                       = d;
dec.condlabel               = condlabel;
dec.numpermutation          = numpermutation;
dec.kfold                   = kfold;
dec.whiten                  = whiten;
dec.method                  = method;
if conditiongen
    dec.d2                  = d2;
    dec.condlabel2          = condlabel2;
end

%% local function; computes decoding with maxcorr
function d = decodemaxcorr(traindata,testdata,method)

%initial variables
ncond = length(traindata);
ntimes = size(traindata{1},2);

% remove mean and divide by standard deviation (to speedup computation of Pearson corr)
for c = 1:ncond
    traindata{c} = bsxfun(@minus,traindata{c},mean(traindata{c},1));  % Remove mean
    sx = sqrt(sum(abs(traindata{c}).^2, 1)); % sqrt first to avoid under/overflow; (1/(n-1) doesn't matter, renormalizing anyway)
    traindata{c} = bsxfun(@rdivide,traindata{c},sx);
    testdata{c} = bsxfun(@minus,testdata{c},mean(testdata{c},1));  % Remove mean
    sx = sqrt(sum(abs(testdata{c}).^2, 1)); % sqrt first to avoid under/overflow
    testdata{c} = bsxfun(@rdivide,testdata{c},sx);
end

switch method

    case 'pairwise'
        %pairwise decoding
        d = zeros(ntimes,ncond*(ncond-1)/2,'single');
        ndxmat = squareform(1:ncond*(ncond-1)/2); %index used to map 2D to squareform
        for i = 1:ncond
            coefii = sum(traindata{i} .* testdata{i}); %compute correlation coefficient
            for j = [1:i-1 i+1:ncond] %for all j ~= i
                coefij = sum(traindata{i} .* testdata{j});
                correct = coefii - coefij > 0; %difference of correlation coef
                d(:,ndxmat(i,j)) = d(:,ndxmat(i,j)) + correct';
            end
        end

    case 'temporalgen'

        %pairwise decode with temporal generalization
        d = zeros(ntimes,ntimes,ncond*(ncond-1)/2,'single');
        ndxmat = squareform(1:ncond*(ncond-1)/2); %index used to map 2D to squareform
        for i = 1:ncond
            coefii = traindata{i}' * testdata{i}; %compute correlation coefficient
            for j = [1:i-1 i+1:ncond] %for all j ~= i
                coefij = traindata{i}' * testdata{j};
                correct = coefii - coefij > 0; %difference of correlation coef
                d(:,:,ndxmat(i,j)) = d(:,:,ndxmat(i,j)) + single(correct)';
            end
        end

    case 'multiclass'

        %pairwise decoding
        %compute correlation coefficient
        coef = zeros(ntimes,ncond,ncond,'single'); %coef is time x train x test
        for i = 1:ncond
            for j = 1:ncond
                coef(:,i,j) = sum(traindata{i} .* testdata{j});
            end
        end

        %assign 1 to maximum correlation category
        d = zeros(ntimes,ncond,ncond,'single');
        [x,I] = max(coef,[],2); %operate on train dimension
        for t = 1:ntimes
            for c = 1:ncond
                d(t,I(t,1,c),c) = d(t,I(t,1,c),c) + 1;
            end
        end

end


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;


function [ nb, ni, np, ns, pr_bud, pr_bud_angular, r, seed ] = ...
  parameter_initialize ( )

%*****************************************************************************80
%
%% PARAMETER_INITIALIZE sets the initial values of parameters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer NB, the number of boundary points.
%
%    Output, integer NI, the number of interior points.
%
%    Output, integer NP, the total number of points.
%
%    Output, integer NS, the number of sample points.
%
%    Output, real PR_BUD, the probability that a boundary point will bud.
%
%    Output, real PR_BUD_ANGULAR, the probability that a boundary point bud
%    will be constrained to the boundary.
%
%    Output, real R, the radius of the circle.
%
%    Output, integer SEED, a seed for the random number generator.
%
  nb = 30;
  ni = 60;
  np = ni + nb;
  ns = 10000;

  pr_bud = 0.05;
  pr_bud_angular = 0.30;

  r = 1.0;
%
%  Call RNG to initialize the random number generator with a random seed,
%  and record that value in SEED.
%
  rng_struct = rng ( 'shuffle' );
  seed = rng_struct.Seed;

  return
end


% DISPLAY - display an EEG data class underlying structure
%
% Author: Arnaud Delorme, SCCN, INC, UCSD, Nov. 2008

% Copyright (C) 2008 Arnaud Delorme, SCCN, INC, UCSD
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function display(obj);

    tmpMMO = memmapfile(obj.dataFile, 'writable', obj.writable, 'format', { 'single' obj.dimensions 'x' });
    if obj.transposed, disp('Warning: data does not display properly for memory mapped file which have been transposed'); end
    disp(tmpMMO.data.x);


function [merged_timestamps, merged_samples, merged_fs] = ...
    merge_sensor_data(timestamps, samples, offsets, fs)

    num_devices = length(samples);

    shift = max(offsets) - offsets;

    % trim recordings according to offsets
    % figure; hold all;
    for i = 1:num_devices
        if ~isempty(timestamps)
            trimmed_timestamps{i} = timestamps{i}(shift(i)+1:end);
        end

        trimmed_samples{i} = samples{i}(shift(i)+1:end);
        % plot(trimmed_samples{i});
    end
    % title('Trimmed signals');

    if ~isempty(timestamps)
        merged_timestamps = interleave_vectors(trimmed_timestamps);
    else
        merged_timestamps = [];
    end
    merged_samples = interleave_vectors(trimmed_samples);
    merged_fs = fs * num_devices;
    % [~, gyro_merged] = interpolate_samples(merged_timestamps, gyro_merged);
end


function plotTree(tree, varNames)
%PLOTTREE   Plot a syntax tree (as stored in a TREEVAR object).
%   Calling sequence:
%      TREEVAR.PLOTTREE(TREE, VARNAMES)
%   where the input is:
%      TREE:     A MATLAB struct, describing the syntax tree of a mathematical
%                expression.
%      VARNAMES: An optional cellstring, whose elements are the name of the
%                variables that appear in a problem.
%
%   Usually, this method is called from within the TREEVAR plot() method.
%
%   Example:
%      % First, define a TREEVAR and carry out some operations:
%      u = treeVar();
%      v = cos(u);
%      w = sin(diff(u));
%      t = v + w;
%      % The following are equivalent:
%      plot(t)
%      treeVar.plotTree(t.tree)
%
% See also TREEVAR.PLOT.

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

% Number of variables involved in the problem
numVars = length(tree.ID);

% Default variable names if none are passed:
if ( nargin < 2 )
    varNames = cell(1, numVars + 1);
    varNames{1} = 't';
    if ( numVars == 1 )
        % Simpler output for scalar case
        varNames{2} = 'u';
    else
        for varCounter = 1:numVars
            varNames{varCounter + 1} = sprintf('u%i', varCounter);
        end
    end
end

% Starting values for plotting
startx = .5;
deltax = .25;

% Layout the tree. The returned tree will store information about where
% it's supposed to be plotted.
tree = layoutNodes(tree, startx, deltax, tree.height + 1, tree.height + 1);

% Create a figure, and plot the first node
plot(tree.x, tree.y, '.','markersize', 25);
hold on

% Maximum differential orders that appear in the tree
maxDiffOrder = tree.diffOrder;

% Start the recursive calls to the actual plotting method:
plotTreePlot(tree, maxDiffOrder, varNames);

% Some final massaging of the figure:
xlim([0, 1])
ylim([0, 1])
hold off
set(gca, 'xtick', [])
set(gca, 'ytick', [])
title('Function evaluation tree')
end

function tree = layoutNodes(tree, treex, deltax, currHeight, maxHeight)
%LAYOUTNODES   Return coordinates for where to plot nodes of syntax trees.

if ( ~isstruct(tree) )
    % In case of scalars or CHEBFUNS appearing in the expression, the
    % corresponding nodes will not be syntax trees. Need to treat this case
    % separately.

    % Store the value of the current "tree":
    treeVal = tree;

    % Create a dummy tree, needed for storing the information required when the
    % tree is plotted:
    tree = [];
    tree.numArgs = 0;
    if ( isnumeric(treeVal) )
        tree.method = num2str(treeVal, '%2.2f');
    else
        tree.method = 'chebfun';
    end

    % Store coordinates where the node should be plotted:
    tree.y = currHeight/(maxHeight+1);
    tree.x = treex;

    % Scalars and CHEBFUNS have zero diffOrders.
    tree.diffOrder = 0;
    return
end

% Store coordinates where the current node should be plotted:
tree.y = currHeight/(maxHeight+1);
tree.x = treex;

% Lay out nodes recursively
switch tree.numArgs
    case 0
        % Do nothing
    case 1
        % Current tree has one child:
        tree.center = layoutNodes(tree.center, treex, deltax, ...
            currHeight - 1, maxHeight);
    case 2
        % Current tree has two childs:
        tree.left  = layoutNodes(tree.left, treex - deltax, ...
            deltax/2, currHeight - 1, maxHeight);
        tree.right  = layoutNodes(tree.right, treex + deltax, ...
            deltax/2, currHeight - 1, maxHeight);
end

end

function plotTreePlot(tree, maxDiffOrder, varNames)
%PLOTREEPLOT   The actual plotting of the syntax tree.

% Nice modern Matlab colours for lines and nodes:
lineColor = [0 0.447 0.741];
depVarColor = [0.85 0.325 0.098];
indVarColor = [0.929 0.694 0.125];

% Plot, depending on the number of arguments that the method has.
switch tree.numArgs
    case 0
        % Plot the constructor leaves in different colours.
        if ( strcmp(tree.method, 'constr') )
            if ( ~any(tree.ID) )
                % Independent variable
                col = indVarColor;
            else
                col = depVarColor;
            end
            plot(tree.x, tree.y, '.','markersize', 25, 'color', col);
        end

    case 1
        % Plot syntax tree for univariate methods:
        plot(tree.center.x, tree.center.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.center.x], [tree.y tree.center.y], 'color',lineColor)
        plotTreePlot(tree.center, maxDiffOrder, varNames)

    case 2
        % Plot syntax tree for bivariate methods. Start with left sub-tree.
        plot(tree.left.x, tree.left.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.left.x], [tree.y tree.left.y], 'color',lineColor)
        plotTreePlot(tree.left, maxDiffOrder, varNames)

        % Plot right sub-tree.
        plot(tree.right.x, tree.right.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.right.x], [tree.y tree.right.y], 'color',lineColor)
        plotTreePlot(tree.right, maxDiffOrder, varNames)
end

% Add the method name to the plot:
if ( strcmp(tree.method, 'constr') )
    % If we're at a constructor leaf, change the text slightly:
    varID = find(tree.ID == 1);
    if ( isempty(varID) )
        % Independent variable
        varString = varNames{1};
    else
        varString = varNames{varID + 1};
    end

    text(tree.x + 0.02, tree.y - 0.01, varString, 'Interpreter', 'none', ...
        'fontsize',14)
else
    text(tree.x + 0.02, tree.y - 0.01, tree.method, 'Interpreter', 'none', ...
        'fontsize',14)
end

end


classdef LevelSetSmoothRectangleInclusion < ...
        LevelSetAbstractRectangleInclusion & ...
        PnormDescriptor

    methods (Access = public)

        function obj = LevelSetSmoothRectangleInclusion(cParams)
            obj.m1 = cParams.widthH;
            obj.m2 = cParams.widthV;
            obj.pnorm  = cParams.pnorm;
            obj.compute(cParams);
        end

    end

end


function P = rosa_lineextract(P, RADIUS, bUpdateConnectivity)
%
% we RECOVER CONNECTIVITY after each edge collapse, which is slow, but the
% result sampling is more uniform.
% bUpdateConnectivity: 0 for RECOVER CONNECTIVITY after each edge collapse, which is slow, but the
% result sampling is more uniform.
% bUpdateConnectivity: 1 for update CONNECTIVITY after each edge collapse, which is faster, and the
% result sampling is almost as same as that of the original method (bUpdateConnectivity=0).
%
% @author: Andrea Tagliasacchi
% reformed by jjcao
% @reform-data:     2009-9-3   using 1-ring to construct the connectivity
% graph
% @reform-data:     2010-8-20  decompose the original function into several
% small functions.
SHOW_RESULTS = true;

options.collapse_order = 1;
if (bUpdateConnectivity)
    [P.spls,P.corresp] = farthest_sampling_by_sphere(P.cpts, P.sample_radius);
    P.spls_adj = connect_by_inherit_neigh(P.cpts, P.spls, P.corresp, P.rings);
    [P.spls, P.spls_adj,P.corresp] = edge_collapse_update(P.cpts, P.spls, P.corresp, P.rings, P.spls_adj, options);
else
    [P.spls,P.corresp] = farthest_sampling_by_sphere(P.cpts, P.sample_radius);
    P.spls_adj = connect_by_inherit_neigh(P.cpts, P.spls, P.corresp, P.rings);
    [P.spls, P.spls_adj,P.corresp] = edge_collapse(P.cpts, P.spls, P.corresp, P.rings, P.spls_adj, options);
end

%%
if (SHOW_RESULTS)
    figure; movegui('northeast');set(gcf,'color','white');hold on;
    plot3( P.spls(:,1), P.spls(:,2), P.spls(:,3), '.r', 'markersize', 5);
    axis off; axis equal;set(gcf,'Renderer','OpenGL');
    plot_connectivity(P.spls, P.spls_adj);
    view3d zoom;
end


sim('pend');

a = logsout.angle;
f = logsout.force;

figure(3)
plot(a.time, a.data*180/pi)
xlabel('time')
ylabel('angle [degree]')
grid on;

figure(4)
plot(f.time, f.data)
xlabel('time')
ylabel('force [N]')
grid on;


function [pyra, im_pyra] = deep_pyramid(im, cnn_model)
% [pyra, im_pyra] = deep_pyramid(im, cnn_model, cache_opts)
%
% im: a color image
% cnn_model: a handle to the model loaded into caffe

% AUTORIGHTS
% ---------------------------------------------------------
% Copyright (c) 2014, Ross Girshick
%
% This file is part of the DeepPyramid code and is available
% under the terms of the Simplified BSD License provided in
% LICENSE. Please retain this notice and LICENSE if you use
% this file (or any portion of it) in your project.
% ---------------------------------------------------------

[pyra, im_pyra] = feat_pyramid(im, cnn_model);

imsize = size(im);
pyra.imsize = imsize(1:2);
pyra.num_levels = cnn_model.pyra.num_levels;
pyra.stride = cnn_model.pyra.stride;

pyra.valid_levels = true(pyra.num_levels, 1);
pyra.padx = 0;
pyra.pady = 0;

% ------------------------------------------------------------------------
function [pyra, im_pyra] = feat_pyramid(im, cnn_model)
% ------------------------------------------------------------------------
% Compute a feature pyramid with caffe

if cnn_model.init_key ~= caffe('get_init_key')
  error('You probably need to load the cnn model into caffe.');
end

% we're assuming the input height and width are the same
assert(cnn_model.pyra.dimx == cnn_model.pyra.dimy);
% compute output width and height
sz_w = (cnn_model.pyra.dimx - 1) / cnn_model.pyra.stride + 1;
sz_h = sz_w;
sz_l = cnn_model.pyra.num_levels;
sz_c = cnn_model.pyra.num_channels;

%th = tic;
[batch, scales, level_sizes] = image_pyramid(im, cnn_model);
%fprintf('prep: %.3fs\n', toc(th));

%th = tic;
feat = caffe('forward', {batch});
% standard song and dance of swapping width and height between caffe and matlab
feat = permute(reshape(feat{1}, [sz_w sz_h sz_c sz_l]), [2 1 3 4]);
%fprintf('fwd: %.3fs\n', toc(th));

pyra.feat = feat;
pyra.scales = scales;
pyra.level_sizes = level_sizes;

im_pyra = batch;

% ------------------------------------------------------------------------
function [batch, scales, level_sizes] = image_pyramid(im, cnn_model)
% ------------------------------------------------------------------------
% Construct an image pyramid that's ready for feeding directly into caffe
% forward

batch_width = cnn_model.pyra.dimx;
batch_height = cnn_model.pyra.dimy;
num_levels = cnn_model.pyra.num_levels;
stride = cnn_model.pyra.stride;

im = single(im);
% Convert to BGR
im = im(:,:,[3 2 1]);
% Subtract mean (mean of the image mean--one mean per channel)
im = bsxfun(@minus, im, cnn_model.mu);

% scale = base scale relative to input image
im_sz = size(im);
if im_sz(1) > im_sz(2)
  height = batch_height;
  width = NaN;
  scale = height / im_sz(1);
else
  height = NaN;
  width = batch_width;
  scale = width / im_sz(2);
end
im_orig = im;

batch = zeros(batch_width, batch_height, 3, num_levels, 'single');
alpha = cnn_model.pyra.scale_factor;
scales = scale*(alpha.^-(0:num_levels-1))';
level_sizes = zeros(num_levels, 2);
for i = 0:num_levels-1
  if i == 0
    im = imresize(im_orig, [height width], 'bilinear');
  else
    im = imresize(im_orig, scales(i+1), 'bilinear');
  end
  im_sz = size(im);
  im_sz = im_sz(1:2);
  level_sizes(i+1, :) = ceil((im_sz - 1) / stride + 1);
  % Make width the fastest dimension (for caffe)
  im = permute(im, [2 1 3]);
  batch(1:im_sz(2), 1:im_sz(1), :, i+1) = im;
end


function [ f,resL2,qualMeasOut] = OS_AwPCSD(proj,geo,angles,maxiter,varargin)
%OS_AwPCSD solves the reconstruction problem using adaptive-weighted
%projection-controlled steepest descent method
%
%   OS_AwPCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   OS_AwPCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros(default)

%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%   'delta'        Defines Parameter to control the amount of smoothing
%                  for pixels at the edges. A large 'delta' is not able to
%                  differentiate image gradients at different pixels. A
%                  small 'delta' give low weights to almost every pixels,
%                  making the algorithm inefficient in removing noise or
%                  straking artifacts. Default is -0.00055
%   'QualMeas'     Asks the algorithm for a set of quality measurement
%                  parameters. Input should contain a cell array of desired
%                  quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                  These will be computed in each iteration.
%   'BlockSize':   Sets the projection block size used simultaneously. If
%                  BlockSize = 1 OS-SART becomes SART and if  BlockSize = length(alpha)
%                  then OS-SART becomes SIRT. Default is 20.
% 'OrderStrategy'  Chooses the subset ordering strategy. Options are
%                  'ordered' :uses them in the input order, but divided
%                  'random'  : orders them randomply
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the ones used.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------
%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,delta,blocksize,OrderStrategy,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);
resL2=zeros(1,niter);
if nargout>1
    computeL2=true;
else
    computeL2=false;
end

[alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);
%
% angles=cell2mat(alphablocks);
% index_angles=cell2mat(orig_index);

if measurequality
    qualMeasOut=zeros(length(QualMeasOpts),maxiter);
end

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W

% Projection weight, W
W=computeW(geo,angles,gpuids);

% Back-Projection weigth, V
V=computeV(geo,angles,alphablocks,orig_index,'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated','gpuids',gpuids);
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:length(alphablocks)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end

            f=f+beta* bsxfun(@times,1./sum(V(:,:,jj),3),Atb(W(:,:,jj).*(proj(:,:,orig_index{jj})-Ax(f,geo,alphablocks{:,jj},'gpuids',gpuids)),geo,alphablocks{:,jj},'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end
    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;

    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');

    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize AwTV
    f=minimizeAwTV(f0,step,ng,delta,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end
    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        resL2(ii)=im3Dnorm(proj-Ax(f,geo,angles,'gpuids',gpuids),'L2');                       % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  iter~=1 && resL2(ii)>resL2(ii-1)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(iter)]);
            end
            return;
        end

    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('OS_AwPCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,delta,block_size,OrderStrategy,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','delta','blocksize','orderstrategy','qualmeas','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:OS_AwPCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:OS_AwPCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE:Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % Lambda
            %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
            % Lambda reduction
            %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
            % Initial image
            %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid init')
                end
            end
            % Number of iterations of TV
            %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
            %  Maximum L2 error to have a "good image"
            %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles),'L2')*0.2; %heuristic
            else
                epsilon=val;
            end
            %  =========================================================================
            %  Parameter to control the amount of smoothing for pixels at the
            %  edges
            %  =========================================================================
        case 'delta'
            if default
                delta=-0.005;
            else
                delta=val;
            end
            %  =========================================================================
            %  Block size for OS-SART
            %  =========================================================================
        case 'blocksize'
            if default
                block_size=20;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwASD_POCS:InvalidInput','Invalid BlockSize')
                end
                block_size=val;
            end
            %  Order strategy
            %  =========================================================================
        case 'orderstrategy'
            if default
                OrderStrategy='random';
            else
                OrderStrategy=val;
            end
            %  =========================================================================
            %  Image Quality Measure
            %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
            %  Non negative
            %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
            %  GPU Ids
            %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:OS_AwPCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in OS_AwPCSD()']);

    end
end

end


function [Q,Z,decomp,Den] = findsos(P,flag,options)
% FINDMATRIXSOS --- Find a sum of squares decomposition of a given matrix polynomial.
%
% [Q,Z,decomp,Den] = findsos(P,flag,options)
%
% P is a symmetric polynomial matrix.
%
% FLAG is an optional argument which, if set to 'rational', returns a
% sum of squares decomposition with rational coefficients.
%
% OPTIONS is an optional argument which specifies the solver and the
% solver-specific parameters on its fields as
%   options.solver
%   options.params
% the default value for options.solver is 'SeDuMi'. The solver parameters
% are fields of options.params as, for instance, options.params.tol = 1e-9.
%
% A positive semidefinite Q and a symbolic monomial vector Z will be
% computed such that
%
%    (Ir kron Z)' * Q * (Ir kron Z) = P(x)
%
% If P is not a sum of squares, the function will return empty Q and Z.
%
% If P is a polynomial with integer coefficients and is represented as a
% symbolic object, then [Q,Z] = findsos(P,'rational') will compute a rational
% matrix Q such that Z'*Q*Z = P.

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%

% 12/27/01 - SP
% 03/27/02 - SP

if nargin == 2
    if ~strcmp(flag,'rational')
        options=flag;
        flag = 'abcdefgh';
    else
        %options.solver='sedumi';
        options.solver='sdpt3';
        flag = 'abcdefgh';
    end
elseif nargin==1
    %options.solver='sedumi';
    options.solver='sdpt3';
end

if isa(P,'sym')

    dimp = size(P);
    if dimp(1)~=dimp(2) ;
        disp(['The polynomial matrix is not square, it cannot be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    P = expand(P);
    vars = findsym(P);
    vars = sym(['[',vars,']']);

    nvars = size(vars,2) ;

    for var = 1:nvars;
        for j = 1:dimp(1)
            if rem(double(feval(symengine,'degree',P(j,j))),2) ;
                disp(['Degree in ' char(vars(var)) ' is not even for the diagonal elements. The polynomial matrix cannot be a sum of squares']);
                Q = [];
                Z = [];
                return;
            end;
        end;
    end;

    if isequal(P,P.')~=1;
        disp(['The polynomial matrix is not symmetric, it can not be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    prog = sosprogram(vars);
    prog = sosineq(prog,P);
    [prog,info] = sossolve(prog,options);

    if strcmp(options.solver,'SDPNAL')
        disp('findsos function currently not supported for SDPNAL solver');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    elseif (info.dinf==1)||(info.pinf==1)
        disp('No sum of squares decomposition is found.');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    else
        Q = reshape(prog.solinfo.RRx,sqrt(length(prog.solinfo.RRx)),sqrt(length(prog.solinfo.RRx)));
        if isa(P,'sym');
            Z = mysympower(vars,prog.extravar.Z{1});
        else
            pvar Z
            coefftemp = speye(size(prog.extravar.Z{1},1));
            Z = polynomial(coefftemp,prog.extravar.Z{1},prog.vartable,[size(prog.extravar.Z{1},1),1]);
        end;
    end;

    if nargin > 1 & strcmp(flag,'rational')

        A = (full(prog.expr.At{1})') ;
        B = (full(prog.expr.b{1})) ;
        % Round x0
        N = 1 ; % This is the tolerance in the rounding
        xx = prog.solinfo.x;

        kmax = 10 ;
        pd = 0 ;
        while kmax ~= 0 ;
            kmax = kmax - 1 ;
            x0 = round(N*xx);
            [Qflat,NN] = proj3(x0,A,B,N);
            n = sqrt(length(Qflat));
            Qr = reshape(Qflat,n,n);
            % Qr should be PSD (should really check symbolically)
            if min(eig(Qr/NN))>-1e-14 ; kmax=0 ; pd = 1 ; end
            % Increase N, and try again
            N = 2*N ;
        end

        % Experimental, no good error checking yet, so we check that
        % expand(NN*P - Z.'*Qr*Z) is zero!

        if (expand(NN*P-Z.'*Qr*Z) ~= 0) | (pd == 0);
            Qr=[];Z=[];NN=[];
            disp('Could not compute a rational SOS!');
        end

        if nargout == 4
            Q = Qr;
            Den = NN;
        else
            if isa(P,'sym')
                Q = sym(Qr/NN);
            else
                Q = Qr/NN;
                disp('To obtain an exact rational solution, run the function with three output arguments.');
            end;
        end;

    end;

    L = sqrtm(double(Q));
    decomp   = L*(kron(eye(dimp(1)),Z));

else

    % PJS -- Handle polynomial variables
    % Most of this code simply mimics the symbolic case and hence the
    % overlap can be reduced.

    dimp = size(P);
    if dimp(1)~=dimp(2) ;
        disp(['The polynomial matrix is not square, it cannot be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    nvars = P.nvar;
    vars = polynomial(zeros(nvars,1));
    for j=1:nvars
        vars(j) = pvar(P.varname{j});
    end

    for var = 1:nvars;
        for j = 1:dimp(1)
            Pjj = P(j,j);
            maxdeg = max(Pjj.degmat(:,var));
            if rem(maxdeg,2)
                disp(['Degree in ' char(vars(var)) ' is not even for the diagonal elements. The polynomial matrix cannot be a sum of squares']);
                Q = [];
                Z = [];
                return;
            end;
        end;
    end;

    tmp = isequal(P,P.');
    if ~all(tmp(:))
        disp(['The polynomial matrix is not symmetric, it can not be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    prog = sosprogram(vars);
    prog = sosineq(prog,P);
    [prog,info] = sossolve(prog);

    if 'solver' == 'SDPNAL'
        disp('findsos function currently not supported for SDPNAL solver');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    elseif (info.dinf==1)||(info.pinf==1)
        disp('No sum of squares decomposition is found.');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    else
        Q = reshape(prog.solinfo.RRx,sqrt(length(prog.solinfo.RRx)),sqrt(length(prog.solinfo.RRx)));
        if isa(P,'sym');
            Z = mysympower(vars,prog.extravar.Z{1});
        else
            pvar Z
            coefftemp = speye(size(prog.extravar.Z{1},1));
            Z = polynomial(coefftemp,prog.extravar.Z{1},prog.vartable,[size(prog.extravar.Z{1},1),1]);
        end;
    end;

    if nargin > 1 & strcmp(flag,'rational')

        A = (full(prog.expr.At{1})') ;
        B = (full(prog.expr.b{1})) ;
        % Round x0
        N = 1 ; % This is the tolerance in the rounding
        xx = prog.solinfo.x;

        kmax = 10 ;
        pd = 0 ;
        while kmax ~= 0 ;
            kmax = kmax - 1 ;
            x0 = round(N*xx);
            [Qflat,NN] = proj3(x0,A,B,N);
            n = sqrt(length(Qflat));
            Qr = reshape(Qflat,n,n);
            % Qr should be PSD (should really check symbolically)
            if min(eig(Qr/NN))>-1e-14 ; kmax=0 ; pd = 1 ; end
            % Increase N, and try again
            N = 2*N ;
        end

        % Experimental, no good error checking yet, so we check that
        % expand(NN*P - Z.'*Qr*Z) is zero!

        if (expand(NN*P-Z.'*Qr*Z) ~= 0) | (pd == 0);
            Qr=[];Z=[];NN=[];
            disp('Could not compute a rational SOS!');
        end

        if nargout == 4
            Q = Qr;
            Den = NN;
        else
            if isa(P,'sym')
                Q = sym(Qr/NN);
            else
                Q = Qr/NN;
                disp('To obtain an exact rational solution, run the function with three output arguments.');
            end;
        end;

    end;

    L = sqrtm(double(Q));
    decomp   = L*(kron(eye(dimp(1)),Z));

    % PJS -- Comment out original code in this section
%     varname = P.var;
%     vars = [];
%     for i = 1:size(varname,1)
%         pvar(varname{i});
%         vars = [vars eval(varname{i})];
%     end;
%
%     deg = mod(max(P.degmat,[],1),2);
%     if sum(deg)~=0
%         i = find(deg~=0);
%         disp(['Degree in ' varname{i(1)} ' is not even. The polynomial cannot be a sum of squares']);
%         Q = [];
%         Z = [];
%         return;
%     end;

end;


function headmodel = ft_headmodel_openmeeg(bnd, varargin)

% FT_HEADMODEL_OPENMEEG creates a volume conduction model of the
% head using the boundary element method (BEM). This function takes
% as input the triangulated surfaces that describe the boundaries and
% returns as output a volume conduction model which can be used to
% compute leadfields.
%
% This function implements
%   Gramfort et al. OpenMEEG: opensource software for quasistatic
%   bioelectromagnetics. Biomedical engineering online (2010) vol. 9 (1) pp. 45
%   http://www.biomedical-engineering-online.com/content/9/1/45
%   doi:10.1186/1475-925X-9-45
% and
%   Kybic et al. Generalized head models for MEG/EEG: boundary element method
%   beyond nested volumes. Phys. Med. Biol. (2006) vol. 51 pp. 1333-1346
%   doi:10.1088/0031-9155/51/5/021
%
% This link with FieldTrip is derived from the OpenMEEG project
% with contributions from Daniel Wong and Sarang Dalal, and uses external
% command-line executables. See http://openmeeg.github.io/
%
% Use as
%   headmodel = ft_headmodel_openmeeg(bnd, ...)
%
% Optional input arguments should be specified in key-value pairs and can
% include
%   conductivity     = vector, conductivity of each compartment
%   tissue           = tissue labels for each compartment
%
% See also FT_PREPARE_VOL_SENS, FT_COMPUTE_LEADFIELD, FT_SYSMAT_OPENMEEG,
%          FT_SENSINTERP_OPENMEEG

%$Id$

ft_hastoolbox('openmeeg', 1);  % add to path (if not yet on path)
openmeeg_license;              % show the license (only once)
prefix = om_checkombin;        % check the installation of the binaries
if(~ispc) % if Linux/Mac, set number of threads
    omp_num_threads = feature('numCores');
    prefix = ['export OMP_NUM_THREADS=' num2str(omp_num_threads) ' && ' prefix];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the first part is largely shared with the dipoli and bemcp implementation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% get the optional arguments
conductivity    = ft_getopt(varargin, 'conductivity');
tissue          = ft_getopt(varargin, 'tissue');

% copy the boundaries from the mesh into the volume conduction model
if isfield(bnd, 'bnd')
    bnd = bnd.bnd;
end

% rename pnt into pos
bnd = fixpos(bnd);

% start with an empty volume conductor
headmodel = [];

% determine the number of compartments
numboundaries = length(bnd);

% determine the desired nesting of the compartments
order = surface_nesting(bnd, 'outsidefirst');

% rearrange boundaries and conductivities
if numel(bnd)>1
    fprintf('reordering the boundaries to: ');
    fprintf('%d ', order);
    fprintf('\n');
    % update the order of the compartments
    bnd = bnd(order);
end

if isempty(conductivity)
    ft_warning('No conductivity is declared, using default values\n')
    if numboundaries == 1
        conductivity = 1;
    elseif numboundaries == 3
        % skin/skull/brain
        conductivity = [0.33 0.0042 0.33]
    elseif numboundaries == 4
        conductivity = [0.33 0.0042 1 0.33]
    else
        ft_error(['Conductivity values are required for ' num2str(numboundaries) ' shells'])
    end
    headmodel.cond = conductivity;
else
    if numel(conductivity)~=numboundaries
        ft_error('a conductivity value should be specified for each compartment');
    end
    % update the order of the compartments
    headmodel.cond = conductivity(order);
end

% assign default tissue labels if none provided
if(isempty(tissue))
    switch(numboundaries)
        case 3
            tissue = {'scalp','skull','brain'};
        case 4
            tissue = {'scalp','skull','CSF','brain'};
        otherwise
            tissue = strcat({'domain'},num2str((1:numboundaries)'));
    end
else
    tissue = tissue(order);
end
headmodel.tissue = tissue;

headmodel.skin_surface = 1;
headmodel.source = numboundaries;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% this uses an implementation that was contributed by INRIA Odyssee Team
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

workdir = fullfile(tempdir,['ft_om_' datestr(now,'ddmmyyHHMMSSFFF')]);
mkdir(workdir);

try
    % Write the triangulations to file, named after tissue type.
    % OpenMEEG v2.3 and up internally adjusts the convention for surface
    % normals, but OpenMEEG v2.2 expects surface normals to point inwards;
    % this checks and corrects if needed
    bndfile = fullfile(workdir,strcat(tissue,'.tri'));
    for ii=1:length(bnd)
        ok = checknormals(bnd(ii));
        if ~ok
            % Flip faces for openmeeg convention (inwards normals)
            bndtmp = bnd(ii);
            bndtmp.tri = fliplr(bnd(ii).tri);

            ok = checknormals(bndtmp);
            if(ok)
                fprintf('flipping normals for OpenMEEG''s convention\n')
                bnd(ii).tri = bndtmp.tri;
            else
                % if still not ok after flip, throw a warning
                ft_warning('neither orientation of surface normals passes check... leaving as is.')
            end
            clear bndtmp
        end

        om_save_tri(bndfile{ii}, bnd(ii).pos, bnd(ii).tri);
    end

    % retain surfaces in headmodel structure (after possible normal flip)
    headmodel.bnd = bnd;

    condfile  = fullfile(workdir,'om.cond');
    geomfile  = fullfile(workdir,'om.geom');
    hmfile    = fullfile(workdir,'hm.bin');
    hminvfile = fullfile(workdir,'hminv.bin');

    % write conductivity and mesh files
    bndlabel = {};
    for i=1:length(bnd)
        [dum,bndlabel{i}] = fileparts(bndfile{i});
    end

    om_write_geom(geomfile,bndfile,bndlabel);
    om_write_cond(condfile,headmodel.cond,bndlabel);

    om_status = system([prefix 'om_assemble -HM ' geomfile ' ' condfile ' ' hmfile]);
    if(om_status ~= 0) % status = 0 if successful
        ft_error('Aborting OpenMEEG pipeline due to above error.');
    end

    headmodel.mat = inv(om_load_sym(hmfile,'binary'));

    rmdir(workdir,'s'); % remove workdir with intermediate files
catch
    disp(lasterr);
    rmdir(workdir,'s'); % remove workdir with intermediate files
    ft_error('an error occurred while running OpenMEEG');
end

% remember the type of volume conduction model
headmodel.type = 'openmeeg';

function ok = checknormals(bnd)
points = bnd.pos;
faces = bnd.tri;

% FIXME: this method is rigorous only for star shaped surfaces
ok = 0;
% translate to the center
org = mean(points,1);
points(:,1) = points(:,1) - org(1);
points(:,2) = points(:,2) - org(2);
points(:,3) = points(:,3) - org(3);

w = sum(solid_angle(points, faces));

if w<0 && (abs(w)-4*pi)<1000*eps
    ok = 0;
    disp('Your surface normals are outwards oriented...')
elseif w>0 && (abs(w)-4*pi)<1000*eps
    ok = 1;
    %   ft_warning('your normals are inwards oriented')
else
    ft_error('your surface probably is irregular\n')
    ok = 0;
end


function [mXSVolcanoes] = volcano_Transform2CrossSection(mVolcanoes, fWidth, fLength, fLat1, fLon1, fLat2, fLon2)

vSel = (mVolcanoes(:,3) == 1);
mType1 = mVolcanoes(vSel,:);

vSel = (mVolcanoes(:,3) == 2);
mType2 = mVolcanoes(vSel,:);

[vType1X, vType1Y] = lc_xsec2(mType1(:,2)', mType1(:,1)', mType1(:,1)*0, fWidth, fLength, fLat1, fLon1, fLat2, fLon2);

mType1 = [vType1X' vType1Y];
mType1(:,3) = 1;

[vType2X, vType2Y] = lc_xsec2(mType2(:,2)', mType2(:,1)', mType2(:,1)*0, fWidth, fLength, fLat1, fLon1, fLat2, fLon2);

mType2 = [vType2X' vType2Y];
mType2(:,3) = 2;

mXSVolcanoes = [mType1; mType2];


function [TB] = B2TB(B)
% Convert computery things from bytes to terabytes.
% Chad A. Greene 2012
TB = B*2^-40 ;


function [train_sample1, train_sample2, label1, label2] = gen_train_sample_xqda(ID_cam_train, train_feat)

label = ID_cam_train(:, 1);
cam = ID_cam_train(:, 2);   % camera label of all training data

uni_label = unique(label);
ind1 = [];
ind2 = [];
label1 = [];
label2 = [];
for n = 1:length(uni_label)
    pos = find(label == uni_label(n));
    perm = randperm(length(pos));
    tmp1 = pos(perm(1:floor(length(pos)/2)));
    tmp2 = pos(perm(floor(length(pos)/2)+1:floor(length(pos)/2)*2));
    cam1 = cam(tmp1);
    cam2 = cam(tmp2);
    pos2 = find(cam1~=cam2);
    tmp1 = tmp1(pos2);
    tmp2 = tmp2(pos2);
    ind1 = [ind1; tmp1];
    ind2 = [ind2; tmp2];
    label1 = [label1; repmat(uni_label(n), [length(tmp1), 1])];
    label2 = [label2; repmat(uni_label(n), [length(tmp2), 1])];
end
train_sample1 = train_feat(:, ind1)';
train_sample2 = train_feat(:, ind2)';


function a = r82row_permute ( n, a, p )

%*****************************************************************************80
%
%% R82ROW_PERMUTE permutes an R82ROW in place.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%    This routine permutes an array of real "objects", but the same
%    logic can be used to permute an array of objects of any arithmetic
%    type, or an array of objects of any complexity.  The only temporary
%    storage required is enough to store a single object.  The number
%    of data movements made is N + the number of cycles of order 2 or more,
%    which is never more than N + N/2.
%
%  Example:
%
%    Input:
%
%      N = 5
%      P = (   2,    4,    5,    1,    3 )
%      A = ( 1.0,  2.0,  3.0,  4.0,  5.0 )
%          (11.0, 22.0, 33.0, 44.0, 55.0 )
%
%    Output:
%
%      A    = (  2.0,  4.0,  5.0,  1.0,  3.0 )
%             ( 22.0, 44.0, 55.0, 11.0, 33.0 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of objects.
%
%    Input, real A(2,N), the array to be permuted.
%
%    Input, integer P(N), the permutation.  P(I) = J means
%    that the I-th element of the output array should be the J-th
%    element of the input array.  P must be a legal permutation
%    of the integers from 1 to N, otherwise the algorithm will
%    fail catastrophically.
%
%    Output, real A(2,N), the permuted array.
%

%
%  Search for the next element of the permutation that has not been used.
%
  for istart = 1 : n

    if ( p(istart) < 0 )

      continue

    elseif ( p(istart) == istart )

      p(istart) = - p(istart);
      continue

    else

      a_temp(1:2) = a(1:2,istart);
      iget = istart;
%
%  Copy the new value into the vacated entry.
%
      while ( 1 )

        iput = iget;
        iget = p(iget);

        p(iput) = - p(iput);

        if ( iget < 1 || n < iget )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'R82ROW_PERMUTE - Fatal error!\n' );
          fprintf ( 1, '  A permutation index is out of range.\n' );
          fprintf ( 1, '  P(%d) = %d\n', iput, iget );
          error ( 'R82ROW_PERMUTE - Fatal error!' );
        end

        if ( iget == istart )
          a(1:2,iput) = a_temp(1:2)';
          break
        end

        a(1:2,iput) = a(1:2,iget);

      end

    end

  end
%
%  Restore the signs of the entries.
%
% p(1:n) = -p(1:n);

  return
end


function fullBconv()
    global config mem;
    current_layer = config.misc.current_layer;
    mem.grads{current_layer+1} = mem.deltas{current_layer+1} * mem.activations{current_layer}';
    mem.grads{current_layer+1+config.layer_num} = sum(mem.deltas{current_layer+1}, 2);
end


function [sF,sDCM] = VBA_spm_split_dcm(DCM,nsplits)
% split DCM inversion using time series partition
% function [sF,sDCM] = spm_split_dcm(DCM,nsplits)
% IN:
%   - DCM: the original DCM structure
%   - nsplits: the number of splits in the partition
% OUT:
%   - sF: log-evidences for each split in the partition
%   - sDCM: cell array of DCM structures for each split

try; nsplits; catch; nsplits=4; end

% 1- split DCM structure
[sDCM] = split_DCM(DCM,nsplits);

% 2- invert split-models
sF = zeros(nsplits,1);
for i=1:nsplits
    try
        % fprintf(1,'confound removal...')
        nq = size(sDCM{i}.Y.X0,2);
        sDCM{i}.Y.X0 = sDCM{i}.Y.X0(ceil(nq./nsplits),:);
        sDCM{i}.Y.X0 = ones(sDCM{i}.v,1);
        % sDCM{i}.delays =0.*sDCM{i}.delays;
        [sDCM{i}] = VBA_spm_dcm_estimate(sDCM{i});
        sF(i) = sDCM{i}.F;
        fprintf(1,' OK.\n')
    catch
        sF(i) = NaN;
    end
end

function [splitDCM] = split_DCM(DCM,nsplits)

if nsplits==1
    splitDCM{1} = DCM;
    return
end

splitDCM = cell(nsplits,1);

nt = DCM.v;
subnt = floor(nt./nsplits);

ntu = size(DCM.U.u,1);
subntu = floor(ntu./nsplits);

for i=1:nsplits
    splitDCM{i} = DCM;
    in = subnt*(i-1)+1:subnt*i;
    splitDCM{i}.v = subnt;
    splitDCM{i}.Y.y = splitDCM{i}.Y.y(in,:);
    splitDCM{i}.Y.X0 = splitDCM{i}.Y.X0(in,:);
    nq = length(DCM.Y.Q);
    %     splitDCM{i}.Y.Q = eye(subnt);
    for j=1:nq
        tmp = zeros(nq*subnt,1);
        tmp(subnt*(j-1)+1:subnt*j) = 1;
        splitDCM{i}.Y.Q{j} = diag(tmp);
    end
    inu = subntu*(i-1)+1:subntu*i;
    splitDCM{i}.U.u = splitDCM{i}.U.u(inu,:);
end


function g = linardKernGradient(kern, x, varargin)

% LINARDKERNGRADIENT Gradient of LINARD kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% automatic relevance determination linear
% 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 linardKernParamInit, kernGradient, linardKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2009

% KERN

  g = zeros(1, size(x, 2)+1);
  if nargin < 4
    [k, sk] = linardKernCompute(kern, x);
  else
    [k, sk] = linardKernCompute(kern, x, varargin{1});
  end
  g(1) = sum(sum(varargin{end}.*sk));
  if nargin < 4
    for i = 1:size(x, 2)
      g(1+i) =  x(:, i)'*varargin{end}*x(:, i)*kern.variance;
    end
  else
    for i = 1:size(x, 2)
      g(1+i) =  x(:, i)'*varargin{end}*varargin{1}(:, i)*kern.variance;
    end
  end
end


function hb_file_write ( output_unit, title, key, totcrd, ptrcrd, indcrd, ...
  valcrd, rhscrd, mxtype, nrow, ncol, nnzero, neltvl, ptrfmt, indfmt, ...
  valfmt, rhsfmt, rhstyp, nrhs, nrhsix, colptr, rowind, values, ...
  rhsval, rhsptr, rhsind, rhsvec, guess, exact )

%*****************************************************************************80
%
%% HB_FILE_WRITE writes an HB file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992.
%
%  Parameters:
%
%    Input, integer OUTPUT_UNIT, the unit to which data is written.
%
%    Input, character ( len = 72 ) TITLE, a title for the matrix.
%
%    Input, character ( len = 8 ) KEY, an identifier for the matrix.
%
%    Input, integer TOTCRD, the total number of lines of data.
%
%    Input, integer PTRCRD, the number of input lines for pointers.
%
%    Input, integer INDCRD, the number of input lines for row indices.
%
%    Input, integer VALCRD, the number of input lines for numerical values.
%
%    Input, integer RHSCRD, the number of input lines for right hand sides.
%
%    Input, character ( len = 3 ) MXTYPE, the matrix type.
%    First character is R for Real, C for complex, P for pattern only.
%    Second character is S for symmetric, U for unsymmetric, H for
%      Hermitian, Z for skew symmetric, R for rectangular.
%    Third character is A for assembled and E for unassembled
%      finite element matrices.
%
%    Input, integer NROW, the number of rows or variables.
%
%    Input, integer NCOL, the number of columns or elements.
%
%    Input, integer NNZERO.  In the case of assembled sparse matrices,
%    this is the number of nonzeroes.  In the case of unassembled finite
%    element matrices, in which the right hand side vectors are also
%    stored as unassembled finite element vectors, this is the total
%    number of entries in a single unassembled right hand side vector.
%
%    Input, integer NELTVL, the number of finite element matrix entries,
%    set to 0 in the case of assembled matrices.
%
%    Input, character ( len = 16 ) PTRFMT, the format for reading pointers.
%
%    Input, character ( len = 16 ) INDFMT, the format for reading indices.
%
%    Input, character ( len = 20 ) VALFMT, the format for reading values.
%
%    Input, character ( len = 20 ) RHSFMT, the format for reading values
%    of the right hand side.
%
%    Input, character ( len = 3 ) RHSTYP, the right hand side type.
%    First character is F for full storage or M for same as matrix.
%    Second character is G if starting "guess" vectors are supplied.
%    Third character is X if exact solution vectors are supplied.
%    Ignored if NRHS = 0.
%
%    Input, integer NRHS, the number of right hand sides.
%
%    Input, integer NRHSIX, the number of row indices (set to 0
%    in the case of unassembled matrices.)  Ignored if NRHS = 0.
%
%    Input, integer COLPTR(NCOL+1), COLPTR(I) points to the location of
%    the first entry of column I in the sparse matrix structure.
%
%    Input, integer ROWIND(NNZERO) or ROWIND(NELTVL), the row index of
%    each item.
%
%    Input, real VALUES(NNZERO) or VALUES(NELTVL), the nonzero values
%    of the matrix.
%
%    If RHSTYP(1:1) == 'F':
%
%      Input, real RHSVAL(NROW,NRHS), contains NRHS dense right hand
%      side vectors.
%
%      Input, integer RHSPTR(*), is not used.
%
%      Input, integer RHSIND(*), is not used.
%
%      Input, real RHSVEC(*), is not used.
%
%    If RHSTYP(1:1) = 'M' and MXTYPE(3:3) = 'A':
%
%      Input, real RHSVAL(NROW,NRHS), is not used.
%
%      Input, integer RHSPTR(NRHS+1), RHSPTR(I) points to the location of
%      the first entry of right hand side I in the sparse right hand
%      side vector.
%
%      Input, integer RHSIND(NRHSIX), indicates, for each entry of
%      RHSVAL, the corresponding row index.
%
%      Input, real RHSVEC(NRHSIX), contains the value of the right hand
%      side entries.
%
%    If RHSTYP(1:1) = 'M' and MXTYPE(3:3) = 'E':
%
%      Input, real RHSVAL(NNZERO,NRHS), contains NRHS unassembled
%      finite element vector right hand sides.
%
%      Input, integer RHSPTR(*), is not used.
%
%      Input, integer RHSIND(*), is not used.
%
%      Input, real RHSVEC(*), is not used.
%
%    Input, real GUESS(NROW,NRHS), the starting guess vectors.
%
%    Input, real EXACT(NROW,NRHS), the exact solution vectors.
%

%
%  Write the header block.
%
  hb_header_write ( output_unit, title, key, totcrd, ptrcrd, indcrd, ...
    valcrd, rhscrd, mxtype, nrow, ncol, nnzero, neltvl, ptrfmt, indfmt, ...
    valfmt, rhsfmt, rhstyp, nrhs, nrhsix );
%
%  Write the matrix structure.
%
  hb_structure_write ( output_unit, ncol, mxtype, nnzero, neltvl, ...
    ptrfmt, indfmt, colptr, rowind );
%
%  Write the matrix values.
%
  hb_values_write ( output_unit, valcrd, mxtype, nnzero, neltvl, ...
    valfmt, values );
%
%  Write the right hand sides.
%
  hb_rhs_write ( output_unit, nrow, nnzero, nrhs, nrhsix, ...
    rhscrd, ptrfmt, indfmt, rhsfmt, mxtype, rhstyp, rhsval, ...
    rhsind, rhsptr, rhsvec );
%
%  Write the starting guesses.
%
  hb_guess_write ( output_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp, ...
    guess );
%
%  Write the exact solutions.
%
  hb_exact_write ( output_unit, nrow, nrhs, rhscrd, rhsfmt, rhstyp, ...
    exact );

  return
end


function varargout = pivots(varargin)
%PIVOTS   Pivot values of a CHEBFUN2.
%
%   PIVOTS(F) returns the pivots used during the construction by the GE
%   algorithm.
%
%   PIVOTS(F, 'normalize'), returns the normalised pivot values. These numbers
%   are scaled so that the columns and rows have unit 2-norm.

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

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

end


input_image_path = 'image1.jpg';

im = double(imread(input_image_path));

[masked_image] = separate(im,1);

imshow(uint8(masked_image.*im))


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

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

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


function [mtype, mkind, skind] = RBtype (A)                                 %#ok
%RBTYPE determine the Rutherford/Boeing type of a sparse matrix
% Usage:
%   [mtype mkind skind] = RBtype (A)
%
% A must be a sparse matrix.  RBtype determines the Rutherford/Boeing type of A.
% Very little memory is used (just size(A,2) integer workspace), so this can
% succeed where a test such as nnz(A-A')==0 will fail.
%
%       mkind:  R: (0), A is real, and not binary
%               P: (1), A is binary (all values or 0 or 1)
%               C: (2), A is complex
%               I: (3), A is integer
%
%       skind:  R: (-1), A is rectangular
%               U: (0), A is unsymmetric (not S, H, or Z)
%               S: (1), A is symmetric (nnz(A-A.') is 0)
%               H: (2), A is Hermitian (nnz(A-A') is 0)
%               Z: (3), A is skew symmetric (nnz(A+A.') is 0)
%
% mtype is a 3-character string, where mtype(1) is the mkind
% ('R', 'P', or 'C').  mtype(2) is the skind ('R', 'U', 'S', 'H', or 'Z'),
% and mtype(3) is 'A'.
%
% Example:
%   load west0479
%   A = west0479 ;
%   RBtype (A)
%   RBtype (spones (A))
%   RBtype (2*spones (A))
%   C = A+A' ;
%   RBtype (C)
%
% See also RBread, RBwrite.

% Copyright 2007, Timothy A. Davis, University of Florida

error ('RBtype mexFunction not found') ;


function imgOut = normalizeFromAnything(img)
%
%       imgOut = normalizeFromAnything(img)
%
%
%       input:
%         - img: an image
%
%       output:
%         - imgOut: an image with value in [0,1]
%
%     Copyright (C) 2015  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "Video Matching"
% 	  by Peter Sand and Seth Teller, MIT Computer Science and Aritficial
% 	  Intelligence Laboratory, Technical Report, November 11 2004.
%

imgOut = [];

if(isa(img, 'uint8'))
    imgOut = single(img) / 255.0;
end

if(isa(img, 'uint16'))
    imgOut = single(img) / 65535.0;
end

if(isa(img, 'double') | isa(img, 'single'))
    max_val = max(img(:));
    if(max_val > 1.0)
        imgOut = img / max_val;
    end
end

if(isempty(imgOut))
    imgOut = img;
end

end