text:
function f = spm_fx_mountaincar(x,v,a,P)
% state equations for mountain car problem
% FORMAT f = spm_fx_mountaincar(x,v,P)
% FORMAT f = spm_fx_mountaincar(x,v,a,P)
% FORMAT f = spm_fx_mountaincar(x,v,P,M)
% x    - [x, x']
% v    - exogenous force
% a    - action
%
% P.a  - 0th order coefficients of force
% P.b  - 1st order coefficients of force
% P.c  - 2nd order coefficients of force
% P.d  - action coefficient
%
% M    - model structure
%
% f    - flow dx/dt
%
% see:
% Gaussian Processes in Reinforcement Learning
% Carl Edward Rasmussen and Malte Kuss
% Max Planck Institute for Biological Cybernetics
% Spemannstraße 38, 72076 T¨ubingen, Germany
% {carl,malte.kuss}@tuebingen.mpg.de
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_mountaincar.m 7679 2019-10-24 15:54:07Z spm $

% determine controlled forces (a)
%==========================================================================

% spm_fx_mountaincar(x,v,P) - recognition model (no action)
%--------------------------------------------------------------------------
global eta
if isempty(eta), eta = 8; end
if nargin == 3

    P    = a;
    a    = 0;

elseif all(isfield(P,{'f','g'}))

    % spm_fx_mountaincar(x,v,P,M) - generative model (no action)
    %----------------------------------------------------------------------
    P.f;
    P.g;
    M    = P;
    P    = a;
    a    = 0;

end

% default parameters
%--------------------------------------------------------------------------
if isempty(P)
    P.a  = 0;
    P.b  = [0 0];
    P.c  = [0 0 0 0];
    P.d  = 1;
end

% acceleration = force:
%--------------------------------------------------------------------------
x     = x(1:2);
a     = tanh((P.d*a + P.a + P.b*x + P.c*kron(x,x))/2);

% f(x)
%--------------------------------------------------------------------------
dt    = 1/4;
if x(1) < 0                                  % gravity
    dHdx = 2*x(1) + 1;
else
    xx   = x(1)^2;
    dHdx = (5*xx + 1).^(-3/2) + (x(1)/2).^4;
end
f     = [x(2); a + v - dHdx - x(2)/eta]*dt;

return

% NOTES: Plots for figure
%--------------------------------------------------------------------------
dx    = 1/64;
x     = linspace(-2,2,1/dx);
xx    = x.^2;
dHdx  = (x < 0).*(2*x + 1);
dHdx  = (x > 0).*((5*xx + 1).^(-3/2) + (x/2).^4) + dHdx;
H     = cumsum(dHdx)*dx;
H     = H - min(H);

subplot(2,2,1)
plot(x,H,x.^0,H,':')
xlabel('position','FontSize',12)
ylabel('height','FontSize',12)
title('mountain car problem','FontSize',16)
axis square

subplot(2,2,2)
plot(x,-dHdx,x,x*0,'-.',x,-x.^0,'--')
xlabel('position','FontSize',12)
ylabel('force','FontSize',12)
title('forces','FontSize',16)
axis square

author: spm


text:
function Avect   = sparsevec(blk,Acell)
%% Implement vec of a cell of sparse matrices
%% This is much faster than calling vec(Acell{i})
%% The generated Avect is typically used for CDCS

n           = blk{1,2};
m           = length(Acell);
nsq         = n^2;

ssi         = [];
ssj         = [];
ssdata      = [];
% fprintf('vec a cell of sparse matrices, progress ...')
for i = 1:m
    % if rem(i,10000) == 1
    %     fprintf('%d/%d ',i,m);
    % end
    Ai              = Acell{i};
    [ii,jj,val]     = find(Ai);

    si              = n * (jj-1) + ii;
    sj              = ones(length(si),1) * i;
    sdata           = val;

    ssi             = [ssi;si(:)];
    ssj             = [ssj;sj(:)];
    ssdata          = [ssdata;sdata(:)];
end
Avect               = sparse(ssi,ssj,ssdata,nsq,m);
% fprintf('Done.\n')
end

author: MIT-SPARK

repo: CertifiablyRobustPerception


text:
function varargout = drawParabola(varargin)
%DRAWPARABOLA Draw a parabola on the current axis
%
%   drawParabola(PARABOLA);
%   Draws a vertical parabola, defined by its vertex and its parameter.
%   Such a parabola admits a vertical axis of symetry.
%
%   The algebraic equation of parabola is given by:
%      (Y - YV) = A * (X - VX)^2
%   Where XV and YV are vertex coordinates and A is parabola parameter.
%
%   A parametric equation of parabola is given by:
%      x(t) = t + VX;
%      y(t) = A * t^2 + VY;
%
%   PARABOLA can also be defined by [XV YV A THETA], with theta being the
%   angle of rotation of the parabola (in degrees and Counter-Clockwise).
%
%   drawParabola(PARABOLA, T);
%   Specifies which range of 't' are used for drawing parabola. If T is an
%   array with only two values, the first and the last values are used as
%   interval bounds, and several values are distributed within this
%   interval.
%
%   drawParabola(..., NAME, VALUE);
%   Can specify one or several graphical options using parameter name-value
%   pairs.
%
%   drawParabola(AX, ...);
%   Specifies handle of the axis to draw on.
%
%   H = drawParabola(...);
%   Returns an handle to the created graphical object.
%
%
%   Example:
%     figure(1); clf; hold on;
%     axis equal; axis([0 100 0 100])
%     % draw parabola with default parameterization bounds
%     drawParabola([50 50 .2 30]);
%     % draw parabola with more specific bounds and drawing style
%     drawParabola([50 50 .2 30], [-3 3], 'color', 'r', 'linewidth', 2);
%
%
%   See Also:
%   drawCircle, drawEllipse
%

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 02/06/2006.
%

%   HISTORY
%   2010-11-17 rewrite, change parametrisation, update doc
%   2011-03-30 use degrees for angle
%   2011-10-11 add management of axes handle

% Extract parabola
if nargin < 1
    error('geom2d:drawParabola:IllegalArgument', ...
        'Please specify parabola representation');
end

% extract handle of axis to draw on
if isAxisHandle(varargin{1})
    ax = varargin{1};
    varargin(1) = [];
else
    ax = gca;
end

% input parabola is given as a packed array
parabola = varargin{1};
varargin(1) = [];
x0 = parabola(:,1);
y0 = parabola(:,2);
a  = parabola(:,3);

% check if parabola orientation is specified
if size(parabola, 2) > 3
    theta = parabola(:, 4);
else
    theta = zeros(length(a), 1);
end

% extract parametrisation bounds
bounds = [-100 100];
if ~isempty(varargin)
    var = varargin{1};
    if isnumeric(var)
        bounds = var;
        varargin(1) = [];
    end
end

% create parametrisation array
if length(bounds) > 2
    t = bounds;
else
    t = linspace(bounds(1), bounds(end), 100);
end

% create handle array (in the case of several parabola)
h = zeros(size(x0));

% draw each parabola
for i = 1:length(x0)
    % compute transformation
    trans = ...
        createTranslation(x0(i), y0(i)) * ...
        createRotation(deg2rad(theta(i))) * ...
        createScaling(1, a);

	% compute points on the parabola
    [xt, yt] = transformPoint(t(:), t(:).^2, trans);

    % draw it
    h(i) = plot(ax, xt, yt, varargin{:});
end

% process output arguments
if nargout > 0
    varargout = {h};
end

author: rpng

repo: lips

save_path: github-repos/MATLAB/rpng-lips


text:
%
%       HDR Toolbox demo build radiance map:
%	   1) Read a stack of LDR images
%	   2) Align the stack
%	   3) Read exposure values from the exif
%	   4) Estimate the Camera Response Function (CRF)
%	   5) Build the radiance map using the stack and stack_exposure
%	   6) Save the radiance map in .hdr format
%	   7) Show the tone mapped version of the radiance map
%
%       Author: Francesco Banterle
%       Copyright 2013-15 (c)
%

clear all;

name_folder = 'demos/stack_alignment';
format = 'jpg';

disp('1) Read a stack of LDR images');
[stack, norm_value] = ReadLDRStack(name_folder, format, 1);

disp('2) Align the stack');
stackOut = WardAlignment(stack, 1);

disp('3) Read exposure values from the exif');
stack_exposure = ReadLDRStackInfo(name_folder, format);

disp('4) Estimate the Camera Response Function (CRF)');
[lin_fun, ~] = DebevecCRF(stackOut, stack_exposure);
h = figure(1);
set(h, 'Name', 'The Camera Response Function (CRF)');
plot(0:255, lin_fun(:,1), 'r', 0:255, lin_fun(:,2),'g', 0:255, lin_fun(:,3), 'b');

disp('5) Build the radiance map using the stack and stack_exposure');
[imgHDR, lin_fun] = BuildHDR(stackOut, stack_exposure, 'LUT', lin_fun, 'Deb97', 'log', 1);

disp('6) Save the radiance map in the .hdr format');
hdrimwrite(imgHDR, 'demos/output/stack_alignment_hdr_ward_alignment.hdr');

disp('7) Show the tone mapped version of the radiance map');
h = figure(2);
set(h, 'Name', 'Tone mapped built HDR Image from stack_alignment');
imgTMO = GammaTMO(ReinhardTMO(imgHDR), 2.2, 0, 1);
imwrite(imgTMO, 'demos/output/stack_alignment_hdr_ward_alignment_tmo.png');

disp('Note that the image needs to be cropped due to alignment');

author: banterle


text:
function desired_state = desiredState(traj_obj, t)
%desiredState computes desired position (x,y,z), yaw and its derivatives
% INPUT
%

tau_vec = traj_obj.tau_vec';
P = traj_obj.P;
path = traj_obj.path;

% Check dimensions
if size(path,2) ~= size(P,2)
    path = path';
end

cumsum_tau_vec = cumsum(tau_vec);
ts = [0; cumsum_tau_vec(:)];
D = size(P,2);

% Declare variables
pos = zeros(1,D);
vel = zeros(1,D);
acc = zeros(1,D);
jerk = zeros(1,D);
snap = zeros(1,D);

% If time is out of range, return the end point of the path
if t < 0
    error('time has to be greater than zero.')
elseif t >= sum(tau_vec)
    pos = path(end,:);
else
    k = find(ts<=t);
    k = k(end);
    for m = 1:D
        tau = t-ts(k);
        pos(m) = [tau^9,tau^8,tau^7,tau^6,tau^5,tau^4,tau^3,tau^2,tau,1] * P(10*k:-1:10*(k-1)+1,m);
        vel(m) = [9*tau^8,8*tau^7,7*tau^6,6*tau^5,5*tau^4,4*tau^3,3*tau^2,2*tau,1,0] * P(10*k:-1:10*(k-1)+1,m);
        acc(m) = [72*tau^7,56*tau^6,42*tau^5,30*tau^4,20*tau^3,12*tau^2,6*tau,2,0,0] * P(10*k:-1:10*(k-1)+1,m);
        jerk(m) = [504*tau^6,336*tau^5,210*tau^4,120*tau^3,60*tau^2,24*tau,6,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
        snap(m) = [3024*tau^5,1680*tau^4,840*tau^3,360*tau^2,120*tau,24,0,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
%         pos(m) = polyval(P(10*k:-1:10*(k-1)+1,m),t-ts(k));
%         vel(m) = polyval(polyder(P(10*k:-1:10*(k-1)+1,m)),t-ts(k));
%         acc(m) = polyval(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))),t-ts(k));
%         jerk(m) = polyval(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m)))),t-ts(k));
%         snap(m) = polyval(polyder(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))))),t-ts(k));
    end
end

% Desired yaw and its derivatives are all zero.
yaw = 0;
yawdot = 0;
yawddot = 0;

desired_state.pos = pos(:);
desired_state.vel = vel(:);
desired_state.acc = acc(:);
desired_state.jerk = jerk(:);
desired_state.snap = snap(:);
desired_state.yaw = yaw;
desired_state.yawdot = yawdot;
desired_state.yawddot = yawddot;

author: yorgoon

repo: minimum-snap-geometric-control


text:
function rectangle_neighbor = bezier_surface_neighbors ( rectangle_num, ...
  rectangle_node )

%*****************************************************************************80
%
%% BEZIER_SURFACE_NEIGHBORS determines Bezier rectangle neighbors.
%
%  Discussion:
%
%    A (bicubic) Bezier surface is constructed of patches.  Each
%    patch is an (X,Y,Z) image of the unit rectangle in (U,V) space.
%    Two patches may be said to be "neighbors" if their images share
%    a side.
%
%    It is perfectly possible for each Bezier patch to have NO neighbors.
%    It is perfectly possible for each Bezier patch to have many neighbors
%    sharing a single side.
%
%    However, in the most common case, and the one handled here, we
%    may assume that the (X,Y,Z) patches fit together in a way similar
%    to the way quilt patches form a surface, so that most rectangles
%    have four neighbors, one on each side, while boundary rectangles
%    might have fewer neighbors.
%
%    This routine creates a data structure recording the neighbor information.
%
%    The primary amount of work occurs in sorting a list of
%    4 * RECTANGLE_NUM data items representing the sides of each patch.
%
%    The nodes of a single rectangle are assumed to be numbered as follows:
%
%    V
%    |   13-14-15-16
%    |    |  |  |  |
%    |    9-10-11-12
%    |    |  |  |  |
%    |    5--6--7--8
%    |    |  |  |  |
%    |    1--2--3--4
%    |
%    +--------------->U
%
%    We assume that a neighbor patch will agree in its (X,Y,Z) values
%    for all four nodes along a single side (although the nodes might
%    be listed in reverse order.)
%
%    We assume that there is at most one neighbor patch on each side.
%    (However, even in a simple case like the Utah teapot, this is not
%    true, because at the top and bottom, many patches have one side
%    that degenerates to a single point, and several such patches
%    meet at that point!)
%
%    We choose to number the sides of each patch as follows:
%
%
%    V      SIDE 3
%    |
%    |   13-14-15-16
%    | S  |  |  |  |
%    | I  9-10-11-12
%    | D  |  |  |  | SIDE 2
%    | E  5--6--7--8
%    |    |  |  |  |
%    | 4  1--2--3--4
%    |       SIDE 1
%    +--------------->U
%
%    And these indices for the sides correspond to the first index
%    in the RECTANGLE_NEIGHBOR array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer RECTANGLE_NUM, the number of rectangles.
%
%    Input, integer RECTANGLE_NODE(16,RECTANGLE_NUM), the nodes that make up
%    each rectangle.
%
%    Output, integer RECTANGLE_NEIGHBOR(4,RECTANGLE_NUM), the indices of
%    the rectanges that are direct neighbors of a given rectangle.
%    RECTANGLE_NEIGHBOR(1,I) is the index of the rectangle which touches
%    side 1, defined by nodes 1 and 4, and so on.  RECTANGLE_NEIGHBOR(1,I)
%    is negative if there is no neighbor on that side.
%

%
%  Step 1.
%  From the list of vertices for rectangle T,
%  construct records that describe the four side segments, by listing
%  the first and last nodes of each segment.
%  To make matching easier later, we sort each pair of nodes.
%
  for rectangle = 1 : rectangle_num

    i1 = rectangle_node(1,rectangle);
    i2 = rectangle_node(4,rectangle);
    i3 = rectangle_node(13,rectangle);
    i4 = rectangle_node(16,rectangle);

    if ( i1 < i2 )
      row(4*(rectangle-1)+1,1:4) = [ i1, i2, 1, rectangle ];
    else
      row(4*(rectangle-1)+1,1:4) = [ i2, i1, 1, rectangle ];
    end

    if ( i2 < i4 )
      row(4*(rectangle-1)+2,1:4) = [ i2, i4, 2, rectangle ];
    else
      row(4*(rectangle-1)+2,1:4) = [ i4, i2, 2, rectangle ];
    end

    if ( i4 < i3 )
      row(4*(rectangle-1)+3,1:4) = [ i4, i3, 3, rectangle ];
    else
      row(4*(rectangle-1)+3,1:4) = [ i3, i4, 3, rectangle ];
    end

    if ( i3 < i1 )
      row(4*(rectangle-1)+4,1:4) = [ i3, i1, 4, rectangle ];
    else
      row(4*(rectangle-1)+4,1:4) = [ i1, i3, 4, rectangle ];
    end

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on columns 1 and 2; the routine we call here
%  sorts on columns 1 through 3 but that won't hurt us.
%
%  What we need is to find cases where two rectangles share an edge.
%  Say they share an edge defined by the nodes I and J.  Then there are
%  two rows of ROW that start out ( I, J, ?, ? ).  By sorting ROW,
%  we make sure that these two rows occur consecutively.  That will
%  make it easy to notice that the rectangles are neighbors.
%
  row = i4row_sort_a ( 4*rectangle_num, 4, row );
%
%  Step 3. Neighboring rectangles show up as consecutive rows with
%  identical first two entries.  Whenever you spot this happening,
%  make the appropriate entries in RECTANGLE_NEIGHBOR.
%
  rectangle_neighbor(1:4,1:rectangle_num) = -1;

  irow = 1;

  while ( 1 )

    if ( 4 * rectangle_num <= irow )
      break
    end

    if ( row(irow,1) ~= row(irow+1,1) | row(irow,2) ~= row(irow+1,2) )
      irow = irow + 1;
      continue
    end

    side1 = row(irow,3);
    rectangle1 = row(irow,4);
    side2 = row(irow+1,3);
    rectangle2 = row(irow+1,4);

    rectangle_neighbor(side1,rectangle1) = rectangle2;
    rectangle_neighbor(side2,rectangle2) = rectangle1;

    irow = irow + 2;

  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
classdef house

methods(Static)

function [v, beta] = gen(x)
    % Generates the householder reflection vector for a given vector x
    % P = I - beta * v * v' is the householder projection matrix
    % P is an orthogonal projector
    % P *x changes x in such a way that only 1st entry is non-zero
    % GVL4: algorithm 5.1.1
    m = length(x);
    if m == 1
        v = 0; beta = 0; return;
    end
    sigma = x(2:m)' * x(2:m);
    v = [1; x(2:m)];
    if sigma == 0  && x(1) >= 0
        beta = 0;
    elseif sigma == 0 && x(1) < 0
        beta = -2;
    else
        mu = sqrt(x(1)^2 + sigma);
        if x(1) <  0
            v(1) = x(1) - mu;
        else
            v(1) = -sigma /(x(1) + mu);
        end % if
        beta = 2 * v(1)^2 / (sigma + v(1)^2);
        v = v / v(1);
    end % if
end % func

function B = premul(A, v, beta)
    % Efficiently computes B = PA  = (I - beta v v') A
    B = A - (beta * v) *(v' * A);
end % func

function B = postmul(A, v, beta)
    % Efficiently computes B = AP  = A (I - beta v v')
    B = A - (A*v) * (beta * v)';
end % func

function beta = beta_from_v(v)
    beta = 2/(v'*v);
end% function

function v = from_essential_v(essential)
    v = [zeros(j-1,1); 1; essential];
end% function

function [W, Y] = wy(QF)
    % Let Q be stored in the factored form Q = Q_1 * ... * Q_r
    % where Q_i = I - beta * v * v'
    % This algorithm computes W and Y such that
    % Q = I_r - W * Y';
    % GVL4: algorithm 5.1.2
    [m,r] = size(QF);
    for j=1:r
        % extract j-th v from the factors array
        % The essential part of v
        essential = QF(j+1:m,j);
        % Insert additional zeros and one to complete v
        v = [zeros(j-1,1); 1; essential];
        % Compute beta from v [it is not stored in factors array]
        beta = 2/(v'*v);
        if j==1
            % Initialize W and Y
            Y = v;
            W = beta*v;
        else
            % Update W and Y
            z = beta*(v - W*(Y(j:m,:)'*v(j:m)));
            Y = [Y v];
            W = [W z];
        end
    end
end% function

function B = wy_premul(A, W, Y)
    % Efficiently computes B = (I - WY') A
    B = A - W*(Y'*A);
end % func

function B = wy_postmul(A, W, Y)
    % Efficiently computes B = A (I - WY')
    B = A - (A*W)*Y';
end % func

function [QF, R] = qr(A)
    % Performs A = QR factorization
    % using Householder reflections
    % GVL4: Algorithm 5.2.1
    import spx.la.house;
    [m,n] = size(A);
    for j=1:min(n,m-1)
        % Compute the jth Householder matrix Q{j}...
        [v,beta] = house.gen(A(j:m,j));
        % Update... A = Q_j A
        A(j:m,j:n) = house.premul(A(j:m,j:n), v, beta);
        % Save the essential part of householder vector...
        A(j+1:m,j) = v(2:m-j+1);
    end
    % extract the R component
    R = triu(A(1:n,1:n));
    % extract the Q component in factored form
    QF = tril(A,-1);
end% function

function [QF, R, P] = qr_pivot(A)
    % Householder QR with column pivoting
    % Achieves factorization AP = QR
    % GVL4 : algorithm 5.4.1
    import spx.la.house;
    [m,n] = size(A);
    % Space for keeping squared norms of columns
    c = zeros(1,n);
    % Compute squared norms of each column
    for j=1:n
        c(j) = A(:,j)'*A(:,j);
    end
    % space for the permutation matrix
    P = 1:n;
    % index of column being processed
    r = 0;
    % Find the index of maximum norm column
    [tau,k] = max(c);
    % The tau > 0 check ensures that we process only till
    % rank of A
    while tau>1e-10 && r<min(m,n)
        r = r+1;
        % Column interchange between current column and max norm column
        A(:,[r k]) = A(:,[k r]);
        % Corresponding column interchange in the permutation matrix
        P([r k]) = P([k r]);
        % Corresponding interchange in the squared norm vector
        c([r k]) = c([k r]);
        % Compute the r-th Householder vector
        [v,beta] = house.gen(A(r:m,r));
        % Update by premultiplication A = Q_r A
        A(r:m,r:n) = A(r:m,r:n) - (beta*v)*(v'*A(r:m,r:n));
        % Save the essential part of householder vector.
        A(r+1:m,r) = v(2:m-r+1);
        if r<min(m,n)
            % update the squared norms
            c(r+1:n) = c(r+1:n)- A(r,r+1:n).^2;
            [tau k] = max(c(r+1:n));
            k = k+r;
        end
    end
    % extract the R component
    R = triu(A(1:n,1:n));
    % extract the Q component in factored form below sub-diagonal
    QF = tril(A,-1);
end % function

function Q = q_back_accum_thin(QF)
    % Explicit formation of an orthogonal matrix from its factored form
    %   representation. Uses backward accumulation.
    % GVL4: Section 5.2.2
    % Produces a "thin", m x n Q
    % with orthonormal columns, the first n columns of Q_{1}...Q_{n}.
    [m,n] = size(QF);
    % start with an empty matrix
    Q = [];
    % iterate over columns backwards
    for j=n:-1:1
        % extract the essential part of the j-th householder vector
        essential = QF(j+1:m,j);
        % expand it
        v = [1;essential];
        % Compute beta from the vector
        beta = 2/(v'*v);
        % Expand the Q matrix with zeros on top right and bottom left
        k = m-j;
        Q = [1 zeros(1,n-j);
             zeros(m-j,1) Q];
        % check if the essential part was non-zero
        if norm(essential)>0
            % premultiply Q with the householder projector
            Q = Q - (beta*v)*(v'*Q);
        end
    end
end% function

function Q = q_back_accum_full(QF)
    % Explicit formation of an orthogonal matrix from its factored form
    %   representation. Uses backward accumulation.
    % QF is m x n and
    % Produces an m x m Q that is the product of of Q_{1}...Q_{n}.
    % GVL4: Section 5.2.2
    [m,n] = size(QF);
    % We start with a matrix of size (m-n) x (m -n)
    % If m <= n, then we start with a matrix of size 1 x 1
    Q = eye(max(m-n,1),max(m-n,1));
    % We iterative over columns backwards
    for j=min(n,m-1):-1:1
        % Q_fact(j+1:m,j) is the essential part of the
        % j-th Householder matrix Q_{j}.
        % Get the essential part of householder vector in this column
        essential = QF(j+1:m,j);
        % Extend the vector with 1
        v = [1;essential];
        % Compute beta from the householder vector
        beta = 2/(v'*v);
        % Expand Q with zeros
        k = m-j;
        Q = [1 zeros(1,k);
             zeros(k,1) Q];
        % check for zero changes.
        if norm(essential)>0
            % premultiply with Householder projector
            Q = Q - (beta*v)*(v'*Q);
        end
   end
end % function

function [UF, B, VF] = bidiag(A)
    % Performs bidiagonalization of A = U B V'.
    % U and V are stored in factored forms.
    % GVL4: algorithm 5.4.2
    import spx.la.house;
    [m,n] = size(A);
    % Iterate over columns of A from first to last column.
    % If A is tall, then we can process all columns.
    % If A is square, then we will process m -1 columns
    % If A is wide, we can process all columns.
    for j=1:min(n,m-1)
        % zero out the j-th column
        % Compute the jth Householder vector  for U_j
        [u,beta] = house.gen(A(j:m,j));
        % Update... A = U_j A
        A(j:m,j:n) = A(j:m,j:n) - (beta*u)*(u'*A(j:m,j:n));
        % Save the essential part of householder vector...
        A(j+1:m,j) = u(2:m-j+1);
        if j+2<=n
            % zero out the j-th row now
            % Compute the j-th Householder vector for V_j
            [v,beta] = house.gen(A(j,j+1:n)');
            % Update... A = A V_j
            A(j:m,j+1:n) = A(j:m,j+1:n) - (A(j:m,j+1:n)*v)*(beta*v)';
            % Save the essential Householder vector in j-th row
            A(j,j+2:n) = v(2:n-j)';
        end
    end
    % Extract the diagonal and first super-diagonal
    B = tril(triu(A),1);
    % Extract the subdiagonal lower triangular part
    UF = tril(A,-1);
    % Space for V factors
    VF = zeros(n,n);
    % Extract the factors row by row
    for j=1:n-2
        % j-th row householder vector
        VF(j+2:n,j+1) = A(j,j+2:n)';
    end
end % function

end % methods

end % classdef

author: indigits

repo: sparse-plex


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

% Creates a Matlab array d0 for the absorbtion probability in the SABR
% model. It uses a csv file which can be downloaded via P. Dousts page
% mentioned in his RISK article "No-Arbitrage SABR"

n1 = length(sabrVol);
n2 = length(sabrbeta);
n3 = length(sabrrho);
n4 = length(sabrnu);
n5 = length(sabrtau);

d0 = zeros(n1,n2,n3,n4,n5);
xindex = 1;
for j2=1:n2
    for j4 = 1:n4
        for j3=1:n3
            for j1=1:n1
                for j5 = 1:n5
                    value = data(xindex,4 + j5);
                    d0(j1,j2,j3,j4,j5) =  value;
                end
                xindex = xindex+1;
            end
        end
    end
end

d0 = d0/2500000;

author: Sable

repo: mcbench-benchmarks


text:
function model = ZF_for_Faster_RCNN_VOC2007(model)

model.mean_image                                = fullfile(pwd, 'models', 'pre_trained_models', 'ZF', 'mean_image');
model.pre_trained_net_file                      = fullfile(pwd, 'models', 'pre_trained_models', 'ZF', 'ZF.caffemodel');
% Stride in input image pixels at the last conv layer
model.feat_stride                               = 16;

%% stage 1 rpn, inited from pre-trained network
model.stage1_rpn.solver_def_file                = fullfile(pwd, 'models', 'rpn_prototxts', 'ZF', 'solver_60k80k.prototxt');
model.stage1_rpn.test_net_def_file              = fullfile(pwd, 'models', 'rpn_prototxts', 'ZF', 'test.prototxt');
model.stage1_rpn.init_net_file                  = model.pre_trained_net_file;

% rpn test setting
model.stage1_rpn.nms.per_nms_topN              	= -1;
model.stage1_rpn.nms.nms_overlap_thres        	= 0.7;
model.stage1_rpn.nms.after_nms_topN           	= 2000;

%% stage 1 fast rcnn, inited from pre-trained network
model.stage1_fast_rcnn.solver_def_file          = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF', 'solver_30k40k.prototxt');
model.stage1_fast_rcnn.test_net_def_file        = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF', 'test.prototxt');
model.stage1_fast_rcnn.init_net_file            = model.pre_trained_net_file;

%% stage 2 rpn, only finetune fc layers
model.stage2_rpn.solver_def_file                = fullfile(pwd, 'models', 'rpn_prototxts', 'ZF_fc6', 'solver_60k80k.prototxt');
model.stage2_rpn.test_net_def_file              = fullfile(pwd, 'models', 'rpn_prototxts', 'ZF_fc6', 'test.prototxt');

% rpn test setting
model.stage2_rpn.nms.per_nms_topN             	= -1;
model.stage2_rpn.nms.nms_overlap_thres       	= 0.7;
model.stage2_rpn.nms.after_nms_topN           	= 2000;

%% stage 2 fast rcnn, only finetune fc layers
model.stage2_fast_rcnn.solver_def_file          = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF_fc6', 'solver_30k40k.prototxt');
model.stage2_fast_rcnn.test_net_def_file        = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF_fc6', 'test.prototxt');

%% final test
model.final_test.nms.per_nms_topN            	= 6000; % to speed up nms
model.final_test.nms.nms_overlap_thres       	= 0.7;
model.final_test.nms.after_nms_topN          	= 300;
end

author: ShaoqingRen


text:
% Bundles submission data for sRGB denoising

% submission_folder Folder where denoised images reside

% Output is written to <submission_folder>/bundled/. Please submit
% the content of this folder.
%
% Author: Tobias Plötz, TU Darmstadt (tobias.ploetz@visinf.tu-darmstadt.de)
%
% This file is part of the implementation as described in the CVPR 2017 paper:
% Tobias Plötz and Stefan Roth, Benchmarking Denoising Algorithms with Real Photographs.
% Please see the file LICENSE.txt for the license governing this code.

function bundle_submission_srgb( submission_folder, para)

if nargin < 2
    out_folder = fullfile(submission_folder, 'bundled/');
else
    out_folder = [submission_folder, 'bundled' num2str(para) '/'];
end
mkdir(out_folder);

israw = false;
eval_version = '1.0';

for i=1:50
    Idenoised = cell(1,20);
    parfor b=1:20
        filename = sprintf('%04d_%02d.mat', i,b);
        s = load(fullfile(submission_folder, filename));

        Idenoised{b} = s.Idenoised_crop;
    end
    filename = sprintf('%04d.mat', i);
    save(fullfile(out_folder, filename), 'Idenoised', 'israw', 'eval_version', '-v7.3');
end
end

author: csjunxu


text:
function play_movie(movs,labels,min_mov,max_mov)
    % play list (cell) of movies with labels, press key to stop playing
    % inputs:
    % movs: cell of movies
    % labels: cell of titles for movies
    % min_mov,max_mov: min and max of movie for setting clims on movies

    if ~iscell(movs)
        movs = {movs};
    end
    if ~exist('labels','var')
        labels = cell(size(movs));
    end
    if ~exist('min_mov','var') || ~exist('max_mov','var')
        nml = min(1e7,numel(movs{1}));
        min_mov = quantile(movs{1}(1:nml),0.001);
        max_mov = quantile(movs{1}(1:nml),1-0.001);
    end

    dialogBox = uicontrol('Style', 'PushButton', 'String', 'stop','Callback', 'delete(gcbf)');

    num_movs = numel(movs);
    len_movs = size(movs{1},3);

    t = 0;
    while (ishandle(dialogBox)) && t<len_movs
        t = t+1;
        for idx_mov = 1:num_movs
            subplot(1,num_movs,idx_mov); imagesc(movs{idx_mov}(:,:,t),[min_mov,max_mov]);
            axis square;
            axis off;
            colormap('gray');
            title([labels{idx_mov}, ', frame ',num2str(t)]);
        end
        if t == 1
            set(gcf,'Position',[100,100,1.2*length(movs)*size(movs{1},2),1.2*size(movs{1},1)]);
        end
        pause(0.001);
    end
    close()

author: flatironinstitute

repo: CaImAn-MATLAB


text:
function [um] = km2um(km)
% Convert length from kilometers to microns (aka micrometers).
% Chad A. Greene 2012
um = km*1000000000;


text:
function [kPa] = mmHg2kPa(mmHg)
% Convert pressure from millimeters of mercury at 0 degrees C to kilopascals
% Chad Greene 2012
kPa = mmHg*.133322;


text:
function [broadcast_obs,precise_obs]=createObs(TOW,satOrbits)

precise_obs = 0;

index=find(satOrbits(1).TOW==TOW);
PRNlist=[];

for ii=1:32
    if satOrbits(ii).C1(index)~=0 && satOrbits(ii).L1(index)~=0 ...
            &&satOrbits(ii).L2(index)~=0 &&satOrbits(ii).P2(index)~=0
        PRNlist=[PRNlist,ii];
    end
end

broadcast_obs.col.XS=1;
broadcast_obs.col.YS=2;
broadcast_obs.col.ZS=3;
broadcast_obs.col.CorrP=4;
broadcast_obs.col.TOW=5;
broadcast_obs.col.PRN=6;

broadcast_obs.data=zeros(length(PRNlist),5);

for ii=1:length(PRNlist)
    broadcast_obs.data(ii,broadcast_obs.col.XS)=satOrbits(PRNlist(ii)).XS(index);
    broadcast_obs.data(ii,broadcast_obs.col.YS)=satOrbits(PRNlist(ii)).YS(index);
    broadcast_obs.data(ii,broadcast_obs.col.ZS)=satOrbits(PRNlist(ii)).ZS(index);
    broadcast_obs.data(ii,broadcast_obs.col.CorrP)=satOrbits(PRNlist(ii)).CorrP1(index);
    broadcast_obs.data(ii,broadcast_obs.col.PRN)=satOrbits(PRNlist(ii)).PRN;
    broadcast_obs.data(ii,broadcast_obs.col.TOW)=TOW;
end

author: yandld


text:
% Figure 8.11: Approximate linear discrimination via support vector classifier
% Section 8.6.1, Boyd & Vandenberghe "Convex Optimization"
% Original by Lieven Vandenberghe
% Adapted for CVX by Joelle Skaf - 10/16/05
% (a figure is generated)
%
% The goal is to find a function f(x) = a'*x - b that classifies the non-
% separable points {x_1,...,x_N} and {y_1,...,y_M} by doing a trade-off
% between the number of misclassifications and the width of the separating
% slab. a and b can be obtained by solving the following problem:
%           minimize    ||a||_2 + gamma*(1'*u + 1'*v)
%               s.t.    a'*x_i - b >= 1 - u_i        for i = 1,...,N
%                       a'*y_i - b <= -(1 - v_i)     for i = 1,...,M
%                       u >= 0 and v >= 0
% where gamma gives the relative weight of the number of misclassified
% points compared to the width of the slab.

% data generation
n = 2;
randn('state',2);
N = 50; M = 50;
Y = [1.5+0.9*randn(1,0.6*N), 1.5+0.7*randn(1,0.4*N);
     2*(randn(1,0.6*N)+1), 2*(randn(1,0.4*N)-1)];
X = [-1.5+0.9*randn(1,0.6*M),  -1.5+0.7*randn(1,0.4*M);
      2*(randn(1,0.6*M)-1), 2*(randn(1,0.4*M)+1)];
T = [-1 1; 1 1];
Y = T*Y;  X = T*X;
g = 0.1;            % gamma

% Solution via CVX
cvx_begin
    variables a(n) b(1) u(N) v(M)
    minimize (norm(a) + g*(ones(1,N)*u + ones(1,M)*v))
    X'*a - b >= 1 - u; %#ok
    Y'*a - b <= -(1 - v); %#ok
    u >= 0; %#ok
    v >= 0; %#ok
cvx_end

% Displaying results
linewidth = 0.5;  % for the squares and circles
t_min = min([X(1,:),Y(1,:)]);
t_max = max([X(1,:),Y(1,:)]);
tt = linspace(t_min-1,t_max+1,100);
p = -a(1)*tt/a(2) + b/a(2);
p1 = -a(1)*tt/a(2) + (b+1)/a(2);
p2 = -a(1)*tt/a(2) + (b-1)/a(2);

graph = plot(X(1,:),X(2,:), 'o', Y(1,:), Y(2,:), 'o');
set(graph(1),'LineWidth',linewidth);
set(graph(2),'LineWidth',linewidth);
set(graph(2),'MarkerFaceColor',[0 0.5 0]);
hold on;
plot(tt,p, '-r', tt,p1, '--r', tt,p2, '--r');
axis equal
title('Approximate linear discrimination via support vector classifier');
% print -deps svc-discr2.eps

author: yu-jiang

repo: radpbook

save_path: github-repos/MATLAB/yu-jiang-radpbook


text:
classdef LIRCMOP13 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP with large infeasible regions

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Huang, Y. Fang, Y. You, J. Mo, C. Wei, and E.
% Goodman, An improved epsilon constraint-handling method in MOEA/D for
% CMOPs with large infeasible regions, Soft Computing, 2019, 23:
% 12491-12510.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            [popsize,variable_length] = size(X);
            sum1 = zeros(popsize,1);
            for j = 3 : variable_length
                sum1 = sum1+10*(X(:,j)-0.5).^2;
            end
            PopObj(:,1) = (1.7057+sum1).*cos(0.5*pi*X(:,1)).*cos(0.5*pi*X(:,2));
            PopObj(:,2) = (1.7057+sum1).*cos(0.5*pi*X(:,1)).*sin(0.5*pi*X(:,2));
            PopObj(:,3) = (1.7057+sum1).*sin(0.5*pi*X(:,1));
            gx          =  PopObj(:,1).^2+PopObj(:,2).^2+PopObj(:,3).^2;
            PopCon(:,1) = (gx-9).*(4-gx);
            PopCon(:,2) = (gx-3.61).*(3.24-gx);
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = 1.7057*R./repmat(sqrt(sum(R.^2,2)),1,3);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            a = linspace(0,pi/2,10)';
            R = {sin(a)*cos(a')*1.7057,sin(a)*sin(a')*1.7057,cos(a)*ones(size(a'))*1.7057};
        end
    end
end

author: BIMK

repo: PlatEMO

save_path: github-repos/MATLAB/BIMK-PlatEMO


text:
function factors = kernFactors(kern, factorType)

% KERNFACTORS Extract factors associated with transformed
% optimisation space. "factorType" is one of 'atox', 'xtoa',
% or 'gradfact', as follows.
%
% 'atox': transform unrestricted input parameters to a suitably
% restricted output range.
%
% 'xtoa': transform restricted parameters back to the unrestricted
% space where e.g. gradient-based parameter optimization is done.
%
% 'gradfact': These factors are derivatives of the
% form dx/da, where "x" is the transformed parameter (for example
% a parameter that is restricted to be positive by a suitable
% transformation) and "a" is the untransformed parameter, which
% usually can freely take any real values.
%
% COPYRIGHT : unknown original copyright, possibly Neil Lawrence
%
% COPYRIGHT : Jaakko Peltonen, 2011

% KERN

factors.index = [];
factors.val = [];
if ~isempty(kern.transforms)
  fhandle = str2func([kern.type 'KernExtractParam']);
  params = fhandle(kern);

  % Process each transformation used with this kernel. Each
  % transformation may affect several parameters.
  for i = 1:length(kern.transforms)
    % Get the parameter indices involved in the i:th transformation
    index = kern.transforms(i).index;
    factors.index = [factors.index index];

    % Get the handle of the transformation function of the i:th transformation
    fhandle = str2func([kern.transforms(i).type 'Transform']);

    % If the transformation has been provided with specific
    % settings (such as a custom output range), use the settings,
    % otherwise transform without settings
    if isfield(kern.transforms(i),'transformsettings') && ~isempty(kern.transforms(i).transformsettings)
      factors.val = [factors.val  ...
		     fhandle(params(index), factorType, kern.transforms(i).transformsettings)];
    else
      factors.val = [factors.val  ...
		     fhandle(params(index), factorType)];
    end

  end
end

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
function commentAllTryCatch(comment)
% SYNTAX:
%    commentAllTryCatch(comment)
% EXAMPLE:
%    commentAllTryCatch(true)
%
% DESCRIPTION:
%    comment or uncomment all  try    cathc for debugging purpouse
%
% INPUT:
%   comment : if true comment if false decomment
%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

if nargin < 1
    comment = true;
end

% find all the m files in goGPS directory

base_dir = '.';
filter = '*.m';
if ~exist(base_dir, 'dir')
    if iscell(base_dir)
        list = base_dir;
    else
        list = {base_dir};
    end
else
    [~, list] = dos(['find ' base_dir ' -name \' filter]);
    if ~isempty(list)
        list = textscan(list,'%s','Delimiter','\n','whitespace','');
        list = list{1};
    end
end
if comment
    tic
    for i = 1 : length(list)
        file_name = list{i};
        if isempty(strfind(file_name,'thirdParty')) & isempty(strfind(file_name,'commentAllTryCatch'))
        fid = fopen(file_name, 'r');
        txt = fread(fid,'*char')';
        fclose(fid);
        %occurencies = regexp(txt, expression, 'once');
        %clean_txt = regexprep(txt, expression, replace);
        occurencies = regexp(txt,' try');

        clean_txt = regexprep(txt, ' try', '%AUTCOMMS try');
        clean_txt = regexprep(clean_txt, '\ttry', '%AUTCOMMT try');

        occurencies = strfind(clean_txt, 'catch');
        clean_txt = strrep(clean_txt, 'catch', 'if false %AUTCOMM');

        if not(isempty(clean_txt)) && ~isempty(occurencies)
            fprintf('Opening file %3d/%3d: %s', i, length(list), file_name);
            fid = fopen(file_name, 'w');
            fwrite(fid, clean_txt);
            fclose(fid);
            fprintf(' -> changed\n');
        end
        end
    end
    toc;
else
    tic
    for i = 1 : length(list)
        file_name = list{i};
                if isempty(strfind(file_name,'thirdParty')) & isempty(strfind(file_name,'commentAllTryCatch'))

        fid = fopen(file_name, 'r');
        txt = fread(fid,'*char')';
        fclose(fid);
        %occurencies = regexp(txt, expression, 'once');
        %clean_txt = regexprep(txt, expression, replace);
        occurencies = strfind(txt, '%AUTCOMMS try');
        clean_txt = strrep(txt,'%AUTCOMMS try', ' try');
        clean_txt = strrep(clean_txt,'%AUTCOMMT try', '\ttry');

        occurencies = strfind(clean_txt,  'if false %AUTCOMM');
        clean_txt = strrep(clean_txt, 'if false %AUTCOMM', 'catch');

        if not(isempty(clean_txt)) && ~isempty(occurencies)
            fprintf('Opening file %3d/%3d: %s', i, length(list), file_name);
            fid = fopen(file_name, 'w');
            fwrite(fid, clean_txt);
            fclose(fid);
            fprintf(' -> changed\n');
        end
                end
    end
    toc;
end

author: goGPS-Project


text:
% Test file for LEGPOLY.

function pass = test_legpoly(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

%% Test method 1 on [-1 1]:

% Test 1 confirms that the elements of the array-valued chebfun p
% are Legendre polynomials:
p = legpoly(900:1100, [-1 1], 0, 1);
xx = linspace(-1, 1, 10);
P = legendre(900, xx);
err = norm(feval(p(:,1), xx) - P(1, :), inf);
tol = 5e3*eps*vscale(p);
pass(1) = err < tol;

% Test 2 confirms orthogonality:
err = norm(p'*p - diag(diag(p'*p)));
tol = 5e1*eps*vscale(p);
pass(2) = err < tol;

% Test 3 confirms othonormality:
p = legpoly(900:1100, [-1 1], 'normalize',1);
err = norm(p'*p - eye(201));
tol = 5e3*eps*vscale(p);
pass(3) = err < tol;

%% Test method 1 on [-1 1] (no domain passed):

% Test 4 confirms that the elements of the array-valued chebfun p
% are Legendre polynomials:
p = legpoly(900:1100,[-1,1],0,1);
xx = linspace(-1, 1, 10);
P = legendre(900, xx);
err = norm(feval(p(:,1), xx) - P(1, :), inf);
tol = 5e3*eps*vscale(p);
pass(4) = err < tol;

% Test 5 confirms orthogonality:
err = norm(p'*p - diag(diag(p'*p)));
tol = 5e1*eps*vscale(p);
pass(5) = err < tol;

% Test 6 confirms othonormality:
p = legpoly(900:1100, 'normalize',1);
err = norm(p'*p - eye(201));
tol = 5e3*eps*vscale(p);
pass(6) = err < tol;

%% Test method 1 on [0 10000]:

% Test 7 confirms orthogonality:
p = legpoly(900:1100, [0 10000], 'normalize',1);
err = norm(p'*p - diag(diag(p'*p)));
tol = 1e5*eps*vscale(p);
pass(7) = err < tol;

% Test 8 confirms othonormality:
p = legpoly(900:1100, [0 10000], 'normalize',1);
err = norm(p'*p - eye(201));
tol = 1e5*eps*vscale(p);
pass(8) = err < tol;

%% Test method 2 on [-1 1]:

% Test 9 confirms that p is a Legendre polynomial:
p = legpoly(40, [-1 0.2 1]);
xx = linspace(-1, 1, 10);
P = legendre(40, xx);
err = norm(feval(p, xx) - P(1, :), inf);
pass(9) = err < 50*eps*vscale(p);

% Test 10 confirms orthogonality:
p = legpoly(1:100, [-1 -0.2 0.3 1]);
err = norm(p'*p - diag(diag(p'*p)));
pass(10) = err < 10*eps*vscale(p);

% Test 11 confirms othonormality:
p = legpoly(1:100, [-1 0.145 1], 'normalize');
err = norm(p'*p - eye(100));
tol = 100*eps*vscale(p);
pass(11) = err < tol;

%% Test method 2 on [-1 1] (no domain passed):

% Test 12 confirms that p is a Legendre polynomial:
p = legpoly(40);
xx = linspace(-1, 1, 10);
P = legendre(40, xx);
err = norm(feval(p, xx) - P(1, :), inf);
tol = 50*eps*vscale(p);
pass(12) = err < tol;

% Test 13 confirms orthogonality:
p = legpoly(1:100);
err = norm(p'*p - diag(diag(p'*p)));
tol = 10*eps*vscale(p);
pass(13) = err < tol;

% Test 14 confirms othonormality:
p = legpoly(1:100, 'normalize');
err = norm(p'*p - eye(100));
tol = 100*eps*vscale(p);
pass(14) = err < tol;

%% Test method 2 on [0 10000]:

% Test 15 confirms orthogonality:
p = legpoly(1:100, [0 155 3333 10000]);
err = norm(p'*p - diag(diag(p'*p)));
pass(15) = err < 1e6*eps*vscale(p);

% Test 16 confirms othonormality:
p = legpoly(1:100, [0 3333 10000], 'normalize');
err = norm(p'*p - eye(100));
tol = 1e4*eps*vscale(p);
pass(16) = err < tol;

%% Test method 3 on [-1 1]:

% Test 17 confirms that p is a Legendre polynomial:
p = legpoly(1500, [-1 -0.5 -0.3 1]);
xx = linspace(-1, 1, 10);
P = legendre(1500, xx);
err = norm(feval(p, xx) - P(1, :), inf);
tol = 1e6*eps*vscale(p);
pass(17) = err < tol;

% Test 18 confirms normaliztion:
p = legpoly(1500, [-1 0 1], 'normalize');
err = norm(p'*p-1);
pass(18) = err < 1e4*eps*vscale(p);

%% Test method 3 on [-1 1] (no domain passed):

% Test 19 confirms that p is a Legendre polynomial:
p = legpoly(1500);
xx = linspace(-1, 1, 10);
P = legendre(1500, xx);
err = norm(feval(p, xx) - P(1, :), inf);
tol = 1e4*eps*vscale(p);
pass(19) = err < tol;

% Test 20 confirms normaliztion:
p = legpoly(1500, 'normalize');
err = norm(p'*p-1);
tol = 1e4*eps*vscale(p);
pass(20) = err < tol;

% Test 21 & 22 confirms vectorized version of METHOD 3:
p = legpoly([1000 1500]);
xx = linspace(-1, 1, 10);
P1 = legendre(1000, xx);
P2 = legendre(1500, xx);
err1 = norm(feval(p(:,1), xx) - P1(1, :), inf);
err2 = norm(feval(p(:,2), xx) - P2(1, :), inf);
tol = 1e4*eps*vscale(p);
pass(21) = err1 < tol;
pass(22) = err2 < tol;

% Test 23 confirms normaliztion for vectorized version of METHOD 3:
p = legpoly([1000 1500], 'normalize');
err = norm(p'*p-eye(2));
tol = 1e4*eps*vscale(p);
pass(23) = err < tol;

%% Test method 2 on [-1 1] with a row input N:

% Test 24 confirms orthogonality:
p = legpoly([1;2;3;4;5;6;7;8;9;10], [-1 0.2 1]);
err = norm(p*p' - diag(diag(p*p')));
tol = 5*eps*vscale(p);
pass(24) = err < tol;

% Test 25 confirms othonormality:
p = legpoly([1;2;3;4;5;6;7;8;9;10], [-1 0.6 1], 'normalize');
err = norm(p*p' - eye(10));
tol = 10*eps*vscale(p);
pass(25) = err < tol;

% Test 26 checks empty case:
p = legpoly([], [-1 0.66 1], 'normalize');
pass(26) = isempty(p);

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function out = contamination_prob(x, n_samples, seed)

% Declare gamma factor (Lagrange constants)
gamma = 1;

% Find total number of input samples
num_inputs = size(x,1);
out = zeros(num_inputs,1);

for i=1:num_inputs

    % Run contamination study
    [cost, ~, ~, ~, constraint, ~, ~, ~] = Contamination(x(i,:)', n_samples, seed);

    % Compute total output
    out(i) = cost - sum(gamma*constraint);

end

author: baptistar

repo: BOCS

save_path: github-repos/MATLAB/baptistar-BOCS


text:
% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_newton(Y,r,Index_norm,A_true,S, mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X,max_restart,no_iter,alphaS,alpha,CostFun,lambda,alpha0,tau,niter_selected)
%
% Non-negative Matrix Factorization (NMF) with the quasi-Newton method
%
%
% [A,X]=nmf_newton(Y,r,Index_norm,A_true,S,mc_on,NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X,max_restart,no_iter,alphaS,alpha,L_type,lambda,alpha0,tau)
%
%       produces mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg_A:    indicates the selected algorithm for computation
%                        of the mixing matrix,
%       > type_alg_X:    indicates the selected algorithm for computation of the sources,
%       > no_iter:       number of inner iterations,
%       > alphaS:        parameter of non-linear projection in computation
%                        of the sources,
%       > alpha:         parameter "alpha" in the alpha-divergence,
%       > CostFun        cost function: (1) - Euclidean,
%                                       (2) - alpha-divergence
%       > lambda         Tikhonov regularization parameter in QP-NMF
%       > alpha0:        initial magnitude in the exponential model for regularization parameter,
%       > tau:           damping factor in the exponential model for regularization parameter,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
% #########################################################################
A = [];
X = [];
if (nargin < 21) | isempty(niter_selected) | max(size(niter_selected) > 1)
   disp('Default number of alternating steps');
   niter_selected = 1000;
end
if (nargin < 20) | isempty(tau) | max(size(tau) > 1)
   disp('Incorrect the damping factor in the exponential model');
   return
end
if (nargin < 19) | isempty(alpha0) | max(size(alpha0) > 1)
   disp('Incorrect the initial magnitude in the exponential model');
   return
end
if (nargin < 18) | isempty(lambda) | max(size(lambda) > 1)
   disp('Incorrect regularization parameter');
   return
end
if (nargin < 17) | isempty(CostFun) | (CostFun < 1) | max(size(CostFun) > 1)
   disp('Incorrect cost function');
   return
end
if (nargin < 16) | isempty(alpha) | max(size(alpha) > 1)
   disp('Incorrect parameter alpha in alpha-divergence');
   return
end
if (nargin < 15) | isempty(alphaS) | max(size(alphaS) > 1)
   disp('Incorrect parameter alphaS');
   return
end
if (nargin < 14) | isempty(no_iter)  | (no_iter < 0) | max(size(no_iter) > 1)
   disp('Incorrect number of inner iterations');
   return
end
if (nargin < 13) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 12) | isempty(type_alg_X) | (type_alg_X < 1) | max(size(type_alg_X) > 1)
   disp('Incorrect algorithm for X');
   return
end
if (nargin < 11) | isempty(type_alg_A) | (type_alg_A < 1) | max(size(type_alg_A) > 1)
   disp('Incorrect algorithm for A');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end
if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

Y = Y + eps;

[m,T]=size(Y);
%niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 10; % maximum number of iterations for each random sample
epsil_normA = 1E-8; % tolerance for alternating

if (alpha == 0) & (type_alg_A == 6)
   type_alg_A = 5;
end

if (type_alg_A == 8) & (size(A_true,1) ~= size(Y,1))
   disp('Multilayer technique cannot be used with A fixed');
   Distance_output = [];
   return
end

% Monte Carlo and alternatings adjustment
if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

% Parameters for QP-NMF
rho = 1E-3;
eta = 1E-4;
epsil_A = 1E-6;

% Declaration for A and X
A=zeros(m,r);
Ap = A;
X=zeros(r,T);
Ainit = A;
Xinit = X;
Z = zeros(m,T);
KL_outer_temp = 0;
Z_outer_temp = 0;
nr = 0; restart_on = 0; norm_A = 10; nr_best = -1;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T;
s_dist = 0;

% Newton
HX = spalloc(r*T,r*T,T*r^2);
HA = spalloc(m*r,m*r,m*r^2);
E = ones(m,T);
I = eye(r);
Zx = zeros(m,T);
alpha_h_init = 1E-13;

while (nr <= max_restart)

   % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          Ainit(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          Ainit = Ainit/max(max(Ainit));

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          Ainit=rand(m,r);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
        Ainit = Ainit*diag(1./sum(Ainit,1));

        if (nr == max_restart)&(max_restart > 0)
           A = A_best;
           X = X_best;
        else
           A = Ainit;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

    switch type_alg_X

        case 1 % ALS

            X = max(1E6*eps,pinv(A'*A)*A'*Y);

        case 2 % HALS

                 for j = 1:r
                     Ys = Y - A*X + A(:,j)*X(j,:);
                     X(j,:) = max(1E6*eps,(A(:,j)'*Ys)/(A(:,j)'*A(:,j)));
                 end

        case 3 % Regularized ALS

            alpha_reg = alpha0*exp(-k/tau);
            if isnan(A)  disp('Matrix A is too much ill-conditioned. Try again.'); break; end
            if cond(A) > 1E6 alphaX = 1E-6; else alphaX = 0; end
            X = max(1E6*eps,pinv(A'*A + alpha_reg + alphaX*I)*A'*Y); %Updating of X

        case 4 % GPCG

            X = gpcg_nmf(Y,A,X,no_iter,CostFun,alpha);

        case 5 % Kullback-Leibler (EMML)

             for t = 1:no_iter
                 X = (X.*(A'*(Y./(A*X + eps)))).^(1 + alphaS);
             end
             X = X + 100*eps;

        case 6 % Frobenius (ISRA)

             for t = 1:no_iter
                 X = X.*((A'*Y)./(A'*A*X + eps));
             end
             X = X + 100*eps;

        case 7 % Newton applied to Frobenius

            hX = A'*A;
            GX = A'*Y - hX*X;
            HX = kron(speye(T),-hX); % Hessian
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) - .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 8  % Newton applied to KL

            Zx = A*X+1E5*eps;
            GX = A'*(E - Y./Zx);
            Zxx = (Y+100*eps)./Zx.^2;
            for t = 1:T
                HX(((t-1)*r+1):t*r,((t-1)*r+1):t*r) = A'*diag(Zxx(:,t))*A;
            end
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) + .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 9 % Newton applied to dual KL (KL2)

            Zx = A*X+10*eps;
            Zxx = 1./Zx + eps;
            GX = A'*log(Zx./(Y + 10*eps));
            for t = 1:T
                HX(((t-1)*r+1):t*r,((t-1)*r+1):t*r) = A'*diag(Zxx(:,t))*A;
            end
            hk = 0;
            alpha_h = alpha_h_init;
            while condest(HX) > 1E7
                  hk = hk + 1;
                  alpha_h = 10*alpha_h;
                  HX = HX + alpha_h*speye(r*T);
            end
            [QX,RX] = qr(HX,GX(:));
            X(:) = X(:) - .9*RX\QX;
            X(X <= 0) = 100*eps;

        case 10 % QP with barrier function

            X = iptrnr(Y',A',no_iter,lambda,rho,eta,epsil_A);
            X = X';

        case 11 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)

    end % switch for X

    switch type_alg_A

        case 1 % QP with barrier function

            Ap = A;
            A = iptrnr(Y,X,no_iter,lambda,rho,eta,epsil_A);
            A = A*diag(1./(sum(A,1)+eps));

        case 2 % GPCG

            Ap = A;
            A = gpcg_nmf(Y',X',A',no_iter,CostFun,alpha);
            A = A';

        case 3 % Newton applied to Frobenius

            Ap = A;
            hA = X*X';
            hA = hA + (1E-12 + 1E8*exp(-k))*eye(r);  % Levenberg-Marquardt regularization
            GA = A*hA - Y*X';  % Gradient
            A = A - .9*GA*pinv(hA);
            A(A <= 0) = 1E2*eps;
            A = A*diag(1./sum(A,1));

        case 4 % Newton applied to KL

            Ap = A;
            Zx = A*X+100*eps;
            Zxx = (Y+eps)./Zx.^2;
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r) = (X.*repmat(Zxx(i,:),r,1))*X'; % Hessian
            end
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
            GA = X*(1 - (Y+eps)./Zx)'; % Gradient
        %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
          %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 5 % Newton applied to dual KL (KL2)

            Zx = A*X+100*eps; Zxx = 1./Zx;
            GA = X*(log(Zx./(Y+eps)))'; % Gradient
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r)...
                = (X.*repmat(Zxx(i,:),r,1))*X';  % Hessian
            end
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
           %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
          %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 6 % Newton applied to alpha-divergence

            Ap = A;
            Zx = A*X+100*eps;
            Zxx = ((Y+eps).^alpha)./(Zx.^(alpha + 1));
            for i = 1:m
                HA(((i-1)*r+1):i*r,((i-1)*r+1):i*r) =...
                    (X.*repmat(Zxx(i,:),r,1))*X'; % Hessian
            end
            GA = (1/alpha)*X*(1 - ((Y+eps)./Zx).^alpha)'; % Gradient
            HA = HA + (1E-12 + 1E15*exp(-2*k))*speye(m*r);
       %    [QA,R] = qr(HA,GA(:)); % Q-less QR factorization
            cond_HA = condest(HA);
            if isinf(cond_HA)
               fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
               break;
            end
            A = A';
            A(:) = A(:) - .9*inv(HA)*GA(:);
         %  A(:) = A(:) - .9*R\QA;
            A(A < 0) = 100*eps;
            A = A';
            A = A*diag(1./sum(A,1));

        case 7 % Regularized ALS

            Ap = A;
            alpha_reg = alpha0*exp(-k/tau);
            A = max(1E6*eps, Y*X'*pinv(X*X' + alpha_reg));
            A = A*diag(1./sum(A,1));

        case 8 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

    end % switch for A

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                    norm_A = norm(abs(A - Ap),'fro');
                    fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A*X + eps;
                       Z = diag(1./sqrt(var(Z')))*Z;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));
                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
Z = AL*A*X;
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

           if (nr == 0) | (KL_outer < KL_outer_temp)
              A_best = A; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

% One-Variance scaling
X(X <= 0) = eps;

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end

author: aludnam

repo: MATLAB

save_path: github-repos/MATLAB/aludnam-MATLAB


text:
function f=fun5(x);
f=sin(x)+3;

author: Eurus-Holmes


text:
function img = discreteSphereEighth(varargin)
%DISCRETESPHEREEIGHTH Discretize a 3D sphere eighth
%
%   IMG = discreteSphereEighth(LX, LY, LZ, SPHEIGHTH)
%   Creates a 3D image of a eighth of a sphere.
%
%   Example
%   img = discreteSphereEighth(1:100, 1:100, 1:100, [50 50 50 30 10 60 45]);
%   img = discreteSphereEighth([1 1 100;1 1 100;1 1 100], [50 50 50], 30, 10);
%   img = discreteSphereEighth([1 1 100;1 1 100;1 1 100], [50 50 50 30 10]);
%
%   See Also
%   imShapes, discreteBall, discreteCube
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-03-31
% Copyright 2015 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY

% compute coordinate of image voxels
[lx, ly, lz, varargin] = parseGridArgs3d(varargin{:});
[x, y, z] = meshgrid(lx, ly, lz);

% default parameters
center  = [lx(ceil(end/2)) ly(ceil(end/2)) lz(ceil(end/2))];
side    = center;
theta   = 0; phi = 0; psi=0;

% process input parameters
if length(varargin)==1
    var = varargin{1};
    center = var(:,1:3);
    if size(var, 2)>3
        side = var(:,4);
    end
    if size(var, 2)>4
        theta = var(:,5);
    end
    if size(var, 2)>5
        phi = var(:,6);
    end
    if size(var, 2)>6
        psi = var(:,7);
    end

elseif ~isempty(varargin)
    center = varargin{1};
    if length(varargin)>1
        side = varargin{2};
    end
    if length(varargin)>2
        theta = varargin{3};
    end
    if length(varargin)>3
        phi = varargin{4};
    end
    if length(varargin)>4
        psi = varargin{5};
    end
end

if length(side) == 1
    side = [side side side];
end

% compute coordinate of image voxels in cube reference system
trans = composeTransforms3d(...
    createTranslation3d(-center),...
    createRotationOz(-deg2rad(phi)),...
    createRotationOy(-deg2rad(theta)), ...
    createRotationOz(-deg2rad(psi)),...
    createScaling3d(1 ./ side));
[x, y, z] = transformPoint3d(x, y, z, trans);

% create image: simple threshold over 3 dimensions, and over radius
img = sqrt(x.^2 + y.^2 + z.^2) <= 1 & x >= 0 & x <= 1 & y >=0 & y <= 1 & z >= 0 & z <= 1;

author: mattools

repo: matImage

save_path: github-repos/MATLAB/mattools-matImage


text:
classdef KnowledgeBaseUtil_Test < TestCase
    properties
        database
    end

    methods
        function this = KnowledgeBaseUtil_Test(name)
            this = this@TestCase(name);
        end

        function setUp(this)
            this.database = edu.stanford.covert.db.MySQLDatabase(getConfig());
        end

        function tearDown(this)
            this.database.close();
        end
    end

    methods
        function testSelectLatestKnowledgeBase(this)
            import edu.stanford.covert.cell.kb.KnowledgeBaseUtil;
            kbWID = KnowledgeBaseUtil.selectLatestKnowledgeBase(this.database);
            validateattributes(kbWID, {'numeric'}, {'positive', 'real', 'integer', 'finite'});
        end

        function testSelectLatestSimulation(this)
            import edu.stanford.covert.cell.kb.KnowledgeBaseUtil;

            kbWID = KnowledgeBaseUtil.selectLatestKnowledgeBase(this.database);
            simWID = KnowledgeBaseUtil.selectLatestSimulation(kbWID, this.database);

            if isempty(simWID)
                simWID2 = this.saveSimulation(kbWID, this.database);
                simWID = KnowledgeBaseUtil.selectLatestSimulation(kbWID, this.database);
                this.deleteSimulation(simWID2, this.database);
                assertEqual(simWID2, simWID);
            end

            validateattributes(simWID, {'numeric'}, {'positive', 'real', 'integer', 'finite', 'scalar'});
        end

        function testGetSimulationCodeArchive(this)
            import edu.stanford.covert.cell.kb.KnowledgeBaseUtil;

            kbWID = KnowledgeBaseUtil.selectLatestKnowledgeBase(this.database);
            simWID = KnowledgeBaseUtil.selectLatestSimulation(kbWID, this.database);

            %if no simulation already saved, save small simulation to database
            newSim = 0;
            if isempty(simWID)
                newSim = 1;
                simWID = this.saveSimulation(kbWID, this.database);
            end

            %get code archive
            fileName = 'output/runSmallTests/differencesFromRevision.txt';
            if exist(fileName, 'file')
                delete(fileName);
            end
            KnowledgeBaseUtil.getSimulationCodeArchive(simWID, fileName, this.database);
            assertEqual(2, exist(fileName, 'file'));

            %delete the added small simulation
            if newSim
                this.deleteSimulation(simWID, this.database);
            end
        end

        function testGetContact(this)
            import edu.stanford.covert.cell.kb.KnowledgeBaseUtil;

            contact = struct('email', 'jkarr@stanford.edu');

            kbWID = KnowledgeBaseUtil.selectLatestKnowledgeBase(this.database);
            wid = KnowledgeBaseUtil.getContact(contact, kbWID, this.database);
            validateattributes(wid, {'numeric'}, {'positive', 'real', 'integer', 'finite'});
        end

        function testNewContact(this)
            import edu.stanford.covert.cell.kb.KnowledgeBaseUtil;

            contact = struct(...
                'firstName', 'Jonathan', ...
                'lastName', 'Karr', ...
                'affiliation', 'Covert Lab, Department of Bioengineering, Stanford University', ...
                'email', 'jkarr@stanford.edu');

            kbWID = KnowledgeBaseUtil.selectLatestKnowledgeBase(this.database);
            wid = KnowledgeBaseUtil.newContact(contact, kbWID, this.database);
            validateattributes(wid, {'numeric'}, {'positive', 'real', 'integer', 'finite'});
        end
    end

    %helper methods
    methods (Static)
        function simWID = saveSimulation(kbWID, database)
            import edu.stanford.covert.cell.sim.SimulationFixture;
            import edu.stanford.covert.cell.sim.util.ConditionSet;
            import edu.stanford.covert.cell.sim.util.DatabaseLogger;

            sim = SimulationFixture.load([], true);
            sim.applyOptions('lengthSec', 10);

            metadata = ConditionSet.parseConditionSet(sim, 'src_test/+edu/+stanford/+covert/+cell/+sim/+util/fixtures/runSimulations.xml').metadata;
            metadata.knowledgeBaseWID = kbWID;
            metadata.revision = edu.stanford.covert.util.revision;

            downsampleStepSec = 2;
            logger = DatabaseLogger(database, downsampleStepSec, metadata);
            logger.addMetadata(metadata);
            sim.run(logger);

            simWID = logger.simulationWID;
        end

        function deleteSimulation(simWID, database)
            database.prepareStatement('CALL delete_simulation("{Si}")', simWID);
            database.query();
        end
    end
end

author: CovertLab

repo: WholeCell

save_path: github-repos/MATLAB/CovertLab-WholeCell


text:
classdef TestMedianBlur
    %TestMedianBlur

    properties (Constant)
        im = fullfile(mexopencv.root(),'test','blox.jpg');
    end

    methods (Static)
        function test_1
            img = imread(TestMedianBlur.im);
            result = cv.medianBlur(img);
            validateattributes(result, {class(img)}, {'size',size(img)});
        end

        function test_2
            img = imread(TestMedianBlur.im);
            result = cv.medianBlur(img, 'KSize',7);
            validateattributes(result, {class(img)}, {'size',size(img)});
        end

        function test_3
            img = imread(fullfile(mexopencv.root(),'test','fruits.jpg'));
            result = cv.medianBlur(img, 'KSize',11);
            validateattributes(result, {class(img)}, {'size',size(img)});
        end

        function test_error_argnum
            try
                cv.medianBlur();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

author: kyamagu

repo: mexopencv

save_path: github-repos/MATLAB/kyamagu-mexopencv


text:
%computes a mel spectrogram from the audio data
%>
%> @param x: time domain sample data, dimension channels X samples
%> @param f_s: sample rate of audio data
%> @param afWindow: FFT window of length iBlockLength (default: hann), can be [] empty
%> @param iBlockLength: internal block length (default: 4096 samples)
%> @param iHopLength: internal hop length (default: 2048 samples)
%> @param bNormalize: normalize input audio (default: True)
%> @param bMagnitude: return magnitude instead of complex spectrum (default: True)
%>
%> @retval X spectrogram
%> @retval f frequency bands
%> @retval t time stamps
% ======================================================================
function [X, f, t] = ComputeSpectrogram (x, f_s, afWindow, iBlockLength, iHopLength, bNormalize, bMagnitude)

    % set default parameters if necessary
    if (nargin < 7)
        bMagnitude = true;
    end
    if (nargin < 6)
        bNormalize = true;
    end
    if (nargin < 5)
        iHopLength = 2048;
    end
    if (nargin < 4)
        iBlockLength = 4096;
    end
    if (nargin < 3 || isempty(afWindow))
        afWindow = hann(iBlockLength,'periodic');
    end

    if (length(afWindow) ~= iBlockLength)
        error('window length mismatch');
    end

    if (size(afWindow, 1) < size(afWindow, 2))
        afWindow = afWindow';
    end
    if (size(x, 1) < size(x, 2))
        x = x';
    end

    % pre-processing: down-mixing
    x = ToolDownmix(x);

    % pre-processing: normalization
    if bNormalize
        x = ToolNormalizeAudio(x);
    end

    [x_b, t] = ToolBlockAudio (x, iBlockLength, iHopLength, f_s);

    X = zeros(size(x_b, 2)/2+1, size(x_b, 1));
    f = linspace(0, f_s/2, (size(X, 1)));

    for n = 1:size(X,2)
        tmp = fft(x_b(n, :)' .* afWindow);

        if bMagnitude
            X(:, n) = abs(tmp(1:size(X, 1))) * 2 / iBlockLength;
        else
            X(:, n) = (tmp(1:size(X, 1))) * 2 / iBlockLength;
        end
    end

    % normalization
    X([1 end],:) = X([1 end],:) / sqrt(2);
end

author: alexanderlerch

repo: ACA-Code


text:
function [VAD, logVAD]= apply_VAD( data, Nsamples)

global Downsample MINSPEECHLGTH JOINSPEECHLGTH

Nwindows= floor( Nsamples/ Downsample);
%number of 4ms window

VAD= zeros( 1, Nwindows);
for count= 1: Nwindows
    VAD( count)= sum( data( (count-1)* Downsample+ 1: ...
        count* Downsample).^ 2)/ Downsample;
end
%VAD is the power of each 4ms window

LevelThresh = sum( VAD)/ Nwindows;
%LevelThresh is set to mean value of VAD

LevelMin= max( VAD);
if( LevelMin > 0 )
    LevelMin= LevelMin* 1.0e-4;
else
    LevelMin = 1.0;
end
%fprintf( 1, 'LevelMin is %f\n', LevelMin);

VAD( find( VAD< LevelMin))= LevelMin;

for iteration= 1: 12
    LevelNoise= 0;
    len= 0;
    StDNoise= 0;

    VAD_lessthan_LevelThresh= VAD( find( VAD<= LevelThresh));
    len= length( VAD_lessthan_LevelThresh);
    LevelNoise= sum( VAD_lessthan_LevelThresh);
    if (len> 0)
        LevelNoise= LevelNoise/ len;
        StDNoise= sqrt( sum( ...
        (VAD_lessthan_LevelThresh- LevelNoise).^ 2)/ len);
    end
    LevelThresh= 1.001* (LevelNoise+ 2* StDNoise);
end
%fprintf( 1, 'LevelThresh is %f\n', LevelThresh);

LevelNoise= 0;
LevelSig= 0;
len= 0;
VAD_greaterthan_LevelThresh= VAD( find( VAD> LevelThresh));
len= length( VAD_greaterthan_LevelThresh);
LevelSig= sum( VAD_greaterthan_LevelThresh);

VAD_lessorequal_LevelThresh= VAD( find( VAD<= LevelThresh));
LevelNoise= sum( VAD_lessorequal_LevelThresh);

if (len> 0)
    LevelSig= LevelSig/ len;
else
    LevelThresh= -1;
end
%fprintf( 1, 'LevelSig is %f\n', LevelSig);

if (len< Nwindows)
    LevelNoise= LevelNoise/( Nwindows- len);
else
    LevelNoise= 1;
end
%fprintf( 1, 'LevelNoise is %f\n', LevelNoise);

VAD( find( VAD<= LevelThresh))= -VAD( find( VAD<= LevelThresh));
VAD(1)= -LevelMin;
VAD(Nwindows)= -LevelMin;

start= 0;
finish= 0;
for count= 2: Nwindows
    if( (VAD(count) > 0.0) && (VAD(count-1) <= 0.0) )
        start = count;
    end
    if( (VAD(count) <= 0.0) && (VAD(count-1) > 0.0) )
        finish = count;
        if( (finish - start)<= MINSPEECHLGTH )
            VAD( start: finish- 1)= -VAD( start: finish- 1);
        end
    end
end
%to make sure finish- start is more than 4

if( LevelSig >= (LevelNoise* 1000) )
    for count= 2: Nwindows
        if( (VAD(count)> 0) && (VAD(count-1)<= 0) )
            start= count;
        end
        if( (VAD(count)<= 0) && (VAD(count-1)> 0) )
            finish = count;
            g = sum( VAD( start: finish- 1));
            if( g< 3.0* LevelThresh* (finish - start) )
                VAD( start: finish- 1)= -VAD( start: finish- 1);
            end
        end
    end
end

start = 0;
finish = 0;
for count= 2: Nwindows
    if( (VAD(count) > 0.0) && (VAD(count-1) <= 0.0) )
        start = count;
        if( (finish > 0) && ((start - finish) <= JOINSPEECHLGTH) )
            VAD( finish: start- 1)= LevelMin;
        end
    end
    if( (VAD(count) <= 0.0) && (VAD(count-1) > 0.0) )
        finish = count;
    end
end

start= 0;
for count= 2: Nwindows
    if( (VAD(count)> 0) && (VAD(count-1)<= 0) )
        start= count;
    end
end
if( start== 0 )
    VAD= abs(VAD);
    VAD(1) = -LevelMin;
    VAD(Nwindows) = -LevelMin;
end

count = 4;
while( count< (Nwindows-1) )
    if( (VAD(count)> 0) && (VAD(count-2) <= 0) )
        VAD(count-2)= VAD(count)* 0.1;
        VAD(count-1)= VAD(count)* 0.3;
        count= count+ 1;
    end
    if( (VAD(count)<= 0) && (VAD(count-1)> 0) )
        VAD(count)= VAD(count-1)* 0.3;
        VAD(count+ 1)= VAD(count-1)* 0.1;
        count= count+ 3;
    end
    count= count+ 1;
end

VAD( find( VAD< 0))= 0;

% fid= fopen( 'mat_vad.txt', 'wt');
% fprintf( fid, '%f\n', VAD);
% fclose( fid);

if( LevelThresh<= 0 )
    LevelThresh= LevelMin;
end

logVAD( find( VAD<= LevelThresh))= 0;
VAD_greaterthan_LevelThresh= find( VAD> LevelThresh);
logVAD( VAD_greaterthan_LevelThresh)= log( VAD( ...
    VAD_greaterthan_LevelThresh)/ LevelThresh);

author: vipchengrui

repo: traditional-speech-enhancement


text:
function [vec, num2, factor] = mydatebase ()
    year = 2000;
    vec = horzcat(year, zeros(1,5));
    num2 = datenum(vec);
    factor = 60 * 60 * 24;  % datenum is in decimal days, mydatenum is in seconds.
end

%!test
%! [vec, num2, factor] = mydatebase ();
%! assert(isequal(size(vec), [1,6]))
%! assert(isscalar(num2))
%! assert(isscalar(factor))

author: Sable


text:
function v = setdiff(v1,v2,varargin)
% remove vectors v2 from a set of vectors v1
%
% Syntax
%   v = setdiff(v1,v2)
%   v = setdiff(v1,v2,'antipodal')
%
% Input
%  v1, v2 - @vector3d
%
% Output
%  v - vector3d
%
% Options
%  antipodal - include antipodal symmetry
%

isEqu = isnull(dot_outer(v1,v2,varargin{:}) - reshape(sqrt(dot(v1,v1)),[],1) * reshape(sqrt(dot(v2,v2)),1,[]),1e-3);

v = v1.subSet(~any(isEqu,2));

author: mtex-toolbox

repo: mtex

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


text:
% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function decentr_state = initDecentrState(full_state)

num_robots = numel(full_state);

decentr_state = cell(size(full_state));
for i = 1:num_robots
    decentr_state{i}.Sim_W_O = eye(4);
    decentr_state{i}.Sim_O_C = cell(0);
    decentr_state{i}.Sim_Cprev_C = cell(0);
    decentr_state{i}.original_T_O_C = cell(0);
    decentr_state{i}.times = [];
    decentr_state{i}.netvlad = [];
    decentr_state{i}.descs = {};
    decentr_state{i}.wids3 = {};
    decentr_state{i}.wids4 = {};
    decentr_state{i}.lms_in_frame = cell(0);
    decentr_state{i}.gt_T_W_C = cell(0);
    decentr_state{i}.T_gt_O_ate = full_state{i}.T_W_C{1};

    % DVPR related members
    decentr_state{i}.dvpr_queries_netvlad = [];
    decentr_state{i}.dvpr_queries_robot_i = [];
    decentr_state{i}.dvpr_queries_frame_i = [];

    % Robust relpose
    decentr_state{i}.consistent_groups = cell(num_robots, 1);
    for j = 1:num_robots
        decentr_state{i}.consistent_groups{j}.members = {};
        decentr_state{i}.consistent_groups{j}.floating = true;
    end

    decentr_state{i}.place_matches = cell(0);
    decentr_state{i}.grouped_with = [];
    decentr_state{i}.converged = true;
    % Pre-loading landmarks -> more managable
    T_O_W = full_state{i}.T_W_C{1} ^ -1;
    decentr_state{i}.p_O_lm = ...
        T_O_W(1:3, 1:3) * full_state{i}.p_W_lm' + T_O_W(1:3, 4);
end

end

author: uzh-rpg


text:
function visual_odo_main(visualDataFile,groundTruthFile)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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/>.

    %% vo
    % for drawing images for estimating pitch and yaw
    global SUB_TRANS_IMG;
    global SUB_YAW_ROT_IMG;
    global SUB_HEIGHT_V_IMG;

    global ODO_IMG_TRANS_RESIZE_RANGE;
    global ODO_IMG_YAW_ROT_RESIZE_RANGE;
    global ODO_IMG_HEIGHT_V_RESIZE_RANGE;

    global PREV_TRANS_V_IMG_X_SUMS;
    global PREV_YAW_ROT_V_IMG_X_SUMS;
    global PREV_HEIGHT_V_IMG_Y_SUMS;

    %% read imgs as VT input

    % get the visual data info
    [subFoldersPathSet, numSubFolders] = get_images_data_info(visualDataFile);

    % load ground truth data
    [frameId, gt_x, gt_y, gt_z, gt_rx, gt_ry, gt_rz] = load_ground_truth_data(groundTruthFile);
    groundTruthData = [gt_x gt_y gt_z];

    % define temp variables of previous profiles
    preProfilesTransImg = zeros(1,ODO_IMG_TRANS_RESIZE_RANGE(2));
    preProfilesYawRotImg = zeros(1,ODO_IMG_YAW_ROT_RESIZE_RANGE(2));
    preProfilesPitchRotImg = zeros(ODO_IMG_HEIGHT_V_RESIZE_RANGE(2), 1);

    % For drawing the current rot direction
    curRotDir = [0 0 0];
    curRotDir(1,3) = 0;

    % The total rotational angle
    sumRotAngle = 0;

    % for drawing the direction theta (rotational angle)
    theta = [0 0 0 0 0 0];
    rho = [0 0 0 0 0 0];
    startPoint =[0 0];
    endPoint = [0.8 0.8];

    % For drawing the velocity
    subRotVel = [];
    transVelVector = [];
    heightVelVector = [];
    sumHeight = [];
    transVelVector(1) = 0;
    heightVelVector(1) = 0;
    subRotVel(1) = 0;
    sumHeight = 0;

     % Transform the angle from degree to radian
    global DEGREE_TO_RADIAN;
    DEGREE_TO_RADIAN = pi / 180;

    % Transform the angle from radian to degree
    global RADIAN_TO_DEGREE;
    RADIAN_TO_DEGREE = 180 / pi;

    RENDER_RATE = 2;

    IMG_TYPE = '*.png';

    global OFFSET_YAW_ROT;
    offset_yaw_rot_vector = [];
    offset_yaw_rot_vector(1) = 0;

    global OFFSET_HEIGHT_V;
    offset_height_V_vector = [];
    offset_height_V_vector(1) = 0;

    odoMapTrajectory(1,1) = 0;
    odoMapTrajectory(1,2) = 0;
    odoMapTrajectory(1,3) = 0;
    odoMapTrajectory(1,4) = 0;

    ODO_MAP_X_SCALING = 1;
    ODO_MAP_Y_SCALING = 1;
    ODO_MAP_Z_SCALING = 1;
    GT_ODO_X_SCALING = 1;
    GT_ODO_Y_SCALING = 1;
    GT_ODO_Z_SCALING = 1;

    curFrame = 0;
%     step = 1;

%     %  up
    startFrame = 1;
    endFrame = 3000;

    % down
%     startFrame = 2484;
%     endFrame = 2621;

    % level_2
%     startFrame = 925;
%     endFrame = 2483;

    %% processing visual odometry
    for iSubFolder = 1 : numSubFolders

        [curFolderPath, imgFilesPathList, numImgs] = get_cur_img_files_path_list(subFoldersPathSet, IMG_TYPE, iSubFolder);

        if numImgs > 0
            for indFrame = startFrame : endFrame
                curFrame = curFrame + 1;
                [curImg] = read_current_image(curFolderPath, imgFilesPathList, indFrame);

                % visual templates and visual odo uses intensity so convert to grayscale
                curGrayImg = rgb2gray(curImg);
                curGrayImg = im2double(curGrayImg);

                % for drawing previous intensity
                preProfilesTransImg = PREV_TRANS_V_IMG_X_SUMS;
                preProfilesYawRotImg = PREV_YAW_ROT_V_IMG_X_SUMS;
                preProfilesHeightVImg = PREV_HEIGHT_V_IMG_Y_SUMS;

                %% step2: processing vo
                % get the odometry from the video
                % computing the 3d odometry based on the current image.
                % yawRotV and pitchRotV in degree
                [transV, yawRotV, heightV] = visual_odometry(curGrayImg);

                subRotVel(curFrame) = yawRotV;
                transVelVector(curFrame) = transV;
                heightVelVector(curFrame) = heightV;

                offset_yaw_rot_vector(curFrame) = OFFSET_YAW_ROT;
                offset_height_V_vector(curFrame) = OFFSET_HEIGHT_V;

                sumHeight(curFrame + 1) = sumHeight(curFrame) + heightV;

                % For drawing the total rotational angle
                if  sumRotAngle + yawRotV >= 360
                    sumRotAngle = mod(sumRotAngle + yawRotV, 360);
                elseif sumRotAngle + yawRotV <= -360
                    sumRotAngle = sumRotAngle + yawRotV + 360;
                else
                    sumRotAngle = sumRotAngle + yawRotV;
                end

                % Transform the vrot from degree to radian
                yawRotV = yawRotV * DEGREE_TO_RADIAN;

                % Direction by raw odometry
                curRotDir(curFrame + 1, 1) = cos(curRotDir(curFrame, 3) + yawRotV );
                curRotDir(curFrame + 1, 2) = sin(curRotDir(curFrame, 3) + yawRotV );
                curRotDir(curFrame + 1, 3) = curRotDir(curFrame, 3) + yawRotV;

                odoMapTrajectory(curFrame + 1,1) = odoMapTrajectory(curFrame,1) + transV * cos(sym(curRotDir(curFrame + 1, 3)));
                odoMapTrajectory(curFrame + 1,2) = odoMapTrajectory(curFrame,2) + transV * sin(sym(curRotDir(curFrame + 1, 3)));
                odoMapTrajectory(curFrame + 1,3) = odoMapTrajectory(curFrame,3) + heightV;
                odoMapTrajectory(curFrame + 1,4) = curRotDir(curFrame + 1, 3);

                % render results information
                if (mod(curFrame, RENDER_RATE) == 0)

                    % computing the normalized profile of horizontal translational image
                    profilesTransImg = sum(SUB_TRANS_IMG);
                    avgIntensity = sum(profilesTransImg) / size(profilesTransImg, 2);
                    profilesTransImg = profilesTransImg / avgIntensity;

                    % computing the normalized profile of rotational image
                    profilesYawRotImg = sum(SUB_YAW_ROT_IMG);
                    avgIntensity = sum(profilesYawRotImg) / size(profilesYawRotImg, 2);
                    profilesYawRotImg = profilesYawRotImg / avgIntensity;

%                     diffYawRotImgs = profilesYawRotImg - preProfilesYawRotImg;
                    % computing the normalized profile of vertical transV image
                    profilesHeightVImg = sum(SUB_HEIGHT_V_IMG,2);
                    avgIntensity = sum(profilesHeightVImg) / size(profilesHeightVImg, 1);
                    profilesHeightVImg = profilesHeightVImg / avgIntensity;

                    diffHeightVImgs = profilesHeightVImg - preProfilesHeightVImg;

                    figure(1)

                    % draw odo map
                    subplot(1, 1, 1, 'replace');
                    hold on
                    plot3((odoMapTrajectory(:,2))* ODO_MAP_X_SCALING , ...
                        (odoMapTrajectory(:,1)) * ODO_MAP_Y_SCALING, ...
                        (odoMapTrajectory(:,3) ) * ODO_MAP_Z_SCALING, '.b');

%                     plot3((gt_x(1:curFrame) - gt_x(1)) * GT_ODO_X_SCALING, ...
%                         (gt_y(1:curFrame) - gt_y(1)) * GT_ODO_Y_SCALING, ...
%                         (gt_z(1:curFrame) - gt_z(1)) * GT_ODO_Z_SCALING, '.r');
                    hold off;
                    view(3);
                    grid on;
%                     view(0, 90);
                    xl = xlabel('odo-map-x');
                    yl = ylabel('odo-map-y');
                    zlabel('odo-map-z');
                    set(xl,'Rotation',15);
                    set(yl,'Rotation',-30);
%                     title('Multilayered Odometry Map');
%                     legend('Result','Truth' ,'1');
%                     axis([-5 110 -5 100 -2 5]);
                    axis on
                    rotate3d on

                    % draw odo map
%                     subplot(1, 2, 2, 'replace');
%                     hold on
%                     plot3((odoMapTrajectory(:,2))* ODO_MAP_X_SCALING , ...
%                         (odoMapTrajectory(:,1)) * ODO_MAP_Y_SCALING, ...
%                         (odoMapTrajectory(:,3) ) * ODO_MAP_Z_SCALING, '.b');
%
% %                     plot3((gt_x(1:curFrame) - gt_x(1)) * GT_ODO_X_SCALING, ...
% %                         (gt_y(1:curFrame) - gt_y(1)) * GT_ODO_Y_SCALING, ...
% %                         (gt_z(1:curFrame) - gt_z(1)) * GT_ODO_Z_SCALING, '.r');
%                     hold off;
%                     grid on;
%                     view(0, 90);
%                     xlabel('odo-map-x');
%                     ylabel('odo-map-y');
%                     zlabel('odo-map-z');
% %                     title('Multilayered Odometry Map (top view)');
% %                     legend('Result','Truth' ,'1');
% %                      axis([-5 110 -5 100]);
%                     axis on
%

                    figure(2)
                    % render the horizontal translational velocity
                    subplot(1, 1, 1, 'replace');
                    plot(subRotVel);
%                     title('Yaw rotational velocity');
                    xlabel('Time');
                    ylabel('vYawRot');
                    axis on

                    figure(3)
                     % render current rot direction
                    subplot(1, 1, 1, 'replace');
                    polar(theta,rho);
                    hold on
                    plot([startPoint(1) curRotDir(curFrame,1)],[startPoint(2) curRotDir(curFrame,2)],'linewidth',1,'color',[0.9 0 0]);
%                     title('Current rotational direction');

                    figure(4)
                    % render the sub image for estimating horizontal translational velocity
                    subplot(3, 4, 1, 'replace');
                    imshow(SUB_TRANS_IMG, []);
                    title('Sub image (vHoriTrans)');
                    xlabel('Width');
                    ylabel('Height');
                    axis on

                    % draw the the normalized profile of horizontal translational image
                    subplot(3,4,5, 'replace');
                    hold on
                    plot(1: size(profilesTransImg,2),profilesTransImg, 'r');
                    plot(1: size(preProfilesTransImg,2),preProfilesTransImg, 'g');
                    hold off
%                     legend('curr', 'g: prev');
                    title('The intensity profiles');
                    xlabel('Width');
                    ylabel('Normalized profile');
                    axis on

                    % render the horizontal translational velocity
                    subplot(3, 4, 9, 'replace');
                    plot(transVelVector);
                    title('Translational velocity');
                    xlabel('Time');
                    ylabel('vTrans');
                    axis on

                    % render the sub image for estimating rotational velocity
                    subplot(3, 4, 2, 'replace');
                    imshow(SUB_YAW_ROT_IMG, []);
                    title('Sub image (vYawRot)');
                    xlabel('Width');
                    ylabel('Height');
                    axis on

                    % draw the the normalized profile of rotational image
                    subplot(3,4,6, 'replace');
                    hold on
                    plot(1: size(profilesYawRotImg,2),profilesYawRotImg, 'r');
                    plot(1: size(preProfilesYawRotImg,2),preProfilesYawRotImg, 'g');
                    hold off
%                     legend('r: curr', 'g: prev');
                    title('The intensity profiles');
                    xlabel('Width');
                    ylabel('Normalized profile');
                    axis on

                                        % render the sub image for estimating pitch velocity
                    subplot(3, 4, 3, 'replace');
                    imshow(SUB_HEIGHT_V_IMG, []);
                    title('Sub image (vPitchRot)');
                    xlabel('Width');
                    ylabel('Height');
                    axis on

                    % draw the the normalized profile of height v image
                    subplot(3,4,7, 'replace');
                    hold on
                    plot(profilesHeightVImg, 1: size(profilesHeightVImg,1), 'r');
                    plot(preProfilesHeightVImg, 1: size(preProfilesHeightVImg,1), 'g');
                    hold off
%                     legend('r: curr', 'g: prev');
                    title('The intensity profiles');
                    xlabel('Width');
                    ylabel('Normalized profile');
                    axis on

                    % render the horizontal translational velocity
                    subplot(3, 4, 11, 'replace');
                    plot(heightVelVector);
                    title('Vertical translational velocity');
                    xlabel('Time');
                    ylabel('vVertTrans');
                    axis on

                    % render the horizontal translational velocity
                    subplot(3, 4, 8, 'replace');
                    plot(sumHeight);
                    title('Height Change');
                    xlabel('Time');
                    ylabel('Height');
                    axis on

                    % render the horizontal translational velocity
                    subplot(3, 4, 12, 'replace');
                    hold on
                    plot(offset_yaw_rot_vector, 'r');
                    plot(offset_height_V_vector, 'g');
                    hold off
                    title('Offset yaw rotation');
                    xlabel('Time');
                    ylabel('Offset');
                    axis on

%                     figure(3)
%
%                      subplot(1,1,1, 'replace');
%                     hold on
%                     plot(1: size(diffHeightVImgs,1),diffHeightVImgs, 'r');
%                     hold off
% %                     legend('r: curr', 'g: prev');
%                     title('The diff between two intensity profiles');
%                     xlabel('Width');
%                     ylabel('Diff');
%                     axis on

                    drawnow
                end
            end
        end
    end
end

author: cognav

repo: NeuroSLAM

save_path: github-repos/MATLAB/cognav-NeuroSLAM