%% DEMOLONG  Short demonstration of long numbers
%

%%
% The purpose of the long toolbox was to compute rigorous bounds for the
% value of certain standard functions. Those values are needed to initialize
% the INTLAB system. The long toolbox is slow, but fast enough to do the job.

setround(0)                 % set rounding to nearest
longprecision(0);           % default option

%% Definition of long numbers
%
% Long numbers are stored in midpoint radius representation.
% The midpoint is stored in an array of precision to be specified,
% the error is stored in one number. Long numbers or vectors are generated
% by the constructor long:

x = long(7)
V = long([-1;3])

%%
% Long vectors are always column vectors (or forced to column vectors).

%% Conversion
% Conversion of double to long by the constructor "long" is always into
% the current internal precision.
%
% The internal precision may be specified by "longprecision".
% The call without input parameter gives the current working precision
% in decimals, the call with input parameter sets working precision.

p = longprecision
longprecision(50)

%%
% Now, the statement "x = long(7)" generates a long number with
% approximately 50 decimal digits. This is only approximate because
% internal representation is to some base beta, a power of 2.

%% Output of long numbers
% Output is usually to a little more than double precision. If you want
% to see more digits, say k, use "display" with second parameter equal
% to k. To see all digits, use k=0.

longprecision
x = 1/long(7)
display(x,40)
display(x,0)

%%
% Output of long numbers is not rigorous. All but a few of the last
% digits are correct.

%% Arithmetic operations
% Long operations +,-,*,/ and ^ are supported. Note that operations on
% vectors are always performed elementwise.

x = [ long(3) ; -7 ]
x*x

%% Output of long intervals
% The display routine takes uncertainties into account. Only the
% correct digits plus some extra are displayed.

longprecision(50);
x = long(1)/37;
display(x,0)
for i=1:100
  x=x*x; x=x*37;
end
display(x,0)

%% Interval and non-interval operations
% Computing with uncertainties may be switched off by

longinit('WithoutErrorTerm')
longprecision(50);
x = long(1)/37;
display(x,0)
for i=1:100
  x=x*x; x=x*37;
end
display(x,0)

%%
% In this case all digits including incorrect ones are displayed.
% Computing without error term is a usual long precision arithmetic
% with specified precision. Note that scalar operations suffer from
% quite some interpretation overhead.

%% Conversion between long and double
% Conversion from long to double is approximately to nearest, conversion
% to interval is rigorous.
%
% For example, in the following the function "longpi" calculates
% "pi" to the specified longprecision, "IntPi" is a true inclusion of the
% transcendental number "pi".

longinit('WithErrorTerm');
longprecision(100);
Pi = longpi;
display(Pi,0)
flptPi = long2dble(Pi)
IntPi = long2intval(Pi)
format long
infsup(IntPi)

%% Long numbers with error term
% Long numbers may be specified with an explicit error term.
% For example,

longprecision(50);
x = long(-1.5)
display(x,0)
x = addlongerror(x,1e-40)
display(x,0)

%%
% defines x to be an interval with midpoint -1.5 and radius
% approximately 10^(-40). Only meaningful digits are stored and displayed.

%% Specifying extremely small errors
% For very small errors leaving the range double precision
% floating point numbers, the error may be specified by
% the mantissa and the exponent of the error:

longprecision(50);
x = long(2^-1000)^2;
x = addlongerror(x,1,-620)

%%
% The final x, which is 2^(-2000), is afflicted with an error
% of 10^(-620).

%% Taylor series: an example
% As an example, the following code computes the value of E = exp(x)
% by a Taylor series:

p = 100; longprecision(p);
x = -30;
t = 1; T = long(1); E = T; k = 0;
while abs(t)>10^(-p)
  k = k+1;
  t = t*x/k;
  T = T*x/k;
  E = E + T;
end
k
exp(x)
display(E,0)

%%
% Note that for large negative values of x there quite some
% cancellation. This can be seen by

x = 30;
t = 1; T = long(1); E = T; k = 0;
while abs(t)>10^(-p)
  k = k+1;
  t = t*x/k;
  T = T*x/k;
  E = E + T;
end
k
1/exp(x)
display(1/E,0)

%% Ill-conditioned polynomials
% Consider the following polynomial:

P = inline(' 4999*x.^6 - 200*x.^5 + 102*x.^4 - 2*x.^3 - 2500*x.^2 + 100*x - 1 ')

%%
% This is an example Bugeaud-Mignotte polynomial. The general form is
%
% ( X^n - aX + 1 )^k - 2X^(nk-k)(aX-1)^k
%
% where a>=10, n>=3 and k>=2.
%
% Those polynomials are constructed to have a pair of very close real roots near c=1/a+1/a^(n+1).
% A graph near c looks as follows:

e = 3e-8;
c = 1/50+1/50^4;
x = c*(1+linspace(-e,e));
close
plot(x,P(x),x,0*x)

%%
% From the graph it is not clear whether the polynomial has no, a double or two real roots
% in the interval c*[1-e,1+e]. An evaluation using the long package yields
% the following:

y = long2dble(P(long(x)));
close
plot(x,y,x,0*x)

%% Sample programs
% For sample programs using long numbers, see for example the
% source codes of long\longpi.m or long\@long\exp.m  .

%% Enjoy INTLAB
% INTLAB was designed and written by S.M. Rump, head of the Institute for Reliable Computing,
% Hamburg University of Technology. Suggestions are always welcome to rump (at) tuhh.de


function xdot=dpendulum(t,x)
%% Double Pendulum ODE
%
% ivp=[gamma0; dtgamma0; alpha0; dtalpha0; beta0; lambda; omega; psi; eta];

beta0=x(5); lambda=x(6); omega=x(7); psi=x(8); eta=x(9);
xdot=zeros(9,1); % a column vector

C = cos(beta0)*cos(x(3)-x(1))-sin(beta0)*sin(x(3)-x(1));
S = sin(beta0)*cos(x(3)-x(1))+cos(beta0)*sin(x(3)-x(1));

xdot(1) = x(2);
xdot(2) = ((omega^2)*sin(x(1))-psi*(S*(x(4)^2+C*x(2)^2*eta)+...
    C*(lambda^2)*sin(x(3))))/(-1+(C^2)*eta*psi);
xdot(3) = x(4);
xdot(4) = (S*eta*(x(2)^2+C*x(4)^2*psi)-C*eta*(omega^2)*sin(x(1))+...
    (lambda^2)*sin(x(3)))/(-1+(C^2)*eta*psi);


function imResult = blendMode_Multiply(A, B, offsetW, offsetH)
%% Multiply blending mode: multiplies the numbers for each pixel of the top
%   layer with the corresponding pixel for the bottom layer. The result is a
%   darker picture.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_Multiply));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

C = A .* B;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


% copyright by Jianxiong Xiao http://mit.edu/jxiao
% demo how to sample uniformly on a sphere

%{
Please cite this paper if you use this code in your publication:
J. Xiao, T. Fang, P. Zhao, M. Lhuillier, and L. Quan
Image-based Street-side City Modeling
ACM Transaction on Graphics (TOG), Volume 28, Number 5
Proceedings of ACM SIGGRAPH Asia 2009
%}

clear
clc

tic;
points = icosahedron2sphere(0);
toc;
subplot(1,4,1);
plot3(points(:,1),points(:,2),points(:,3),'.')
title(sprintf('Level %d with %d points',0,size(points,1)))
axis equal
axis tight

tic;
points = icosahedron2sphere(1);
toc;
subplot(1,4,2);
plot3(points(:,1),points(:,2),points(:,3),'.')
title(sprintf('Level %d with %d points',1,size(points,1)))
axis equal
axis tight

tic;
points = icosahedron2sphere(2);
toc;
subplot(1,4,3);
plot3(points(:,1),points(:,2),points(:,3),'.')
title(sprintf('Level %d with %d points',2,size(points,1)))
axis equal
axis tight

tic;
points = icosahedron2sphere(4);
toc;
subplot(1,4,4);
plot3(points(:,1),points(:,2),points(:,3),'.')
title(sprintf('Level %d with %d points',3,size(points,1)))
axis equal
axis tight


function test_ft_denoise_prewhiten

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_denoise_prewhiten

% create some data
data = [];
data.label = {'chan01';'chan02';'chan03'};

% full rank data
mix = [1 0 0;0 1 0;0.5 0.5 0.1];
for k = 1:10
  data.trial{k} = mix*randn(3,1000);
  data.time{k}  = (0:999)./1000;
end

cfg = [];
cfg.covariance = 'yes';
tlck = ft_timelockanalysis(cfg, data);

cfg = [];
datawhite = ft_denoise_prewhiten(cfg, data, tlck);

cfg = [];
cfg.covariance = 'yes';
tlckwhite = ft_timelockanalysis(cfg, datawhite);

cfg = [];
tlckwhite2 = ft_denoise_prewhiten(cfg, tlck, tlck);

% rank deficient data
mix = [1 0 0;0 1 0;0.5 0.5 0];
for k = 1:10
  data.trial{k} = mix*randn(3,1000);
  data.time{k}  = (0:999)./1000;
end

cfg = [];
cfg.covariance = 'yes';
tlck = ft_timelockanalysis(cfg, data);

cfg = [];
datawhite = ft_denoise_prewhiten(cfg, data, tlck);

cfg = [];
cfg.covariance = 'yes';
tlckwhite = ft_timelockanalysis(cfg, datawhite);

cfg = [];
tlckwhite2 = ft_denoise_prewhiten(cfg, tlck, tlck);


function rec = compute_recovery_rate(x,x0)

% compute_recovery_rate - compute the number of recovered coefficients
%
%   rec = compute_recovery_rate(x,x0);
%
%   x is the original signal with 0/1 coefficients
%   x0 is the recovered signal.
%
%   rec=1 means perfect recovery and rec=0 means no recovery (bad).
%
%   Copyright (c) 2007 Gabriel Peyre

% works for positive coefficients 0/1

s = sum(x);
x = double( x(:)>0 );

[tmp,I] = sort(x0(:));
x0 = x0 * 0; x0(I(end-s+1:end)) = 1;
rec = sum(x0.*x)/s;


classdef BT3 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP with bias feature

%------------------------------- Reference --------------------------------
% H. Li, Q. Zhang, and J. Deng, Biased multiobjective optimization and
% decomposition algorithm, IEEE Transactions on Cybernetics, 2017, 47(1):
% 52-66.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            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
        function PopObj = CalObj(obj,X)
            [N,D]  = size(X);
            I1     = 2 : 2 : D;
            I2     = 3 : 2 : D;
            Y      = X - sin(repmat(1:D,N,1)*pi/2/D);
            X(:,1) = abs(X(:,1)).^0.02;
            PopObj(:,1) = X(:,1)         + sum(Y(:,I1).^2+(1-exp(-Y(:,I1).^2/1e-8))/5,2);
            PopObj(:,2) = 1-sqrt(X(:,1)) + sum(Y(:,I2).^2+(1-exp(-Y(:,I2).^2/1e-8))/5,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function G = gsp_stochastic_block_graph(N, k, params)
%GSP_STOCHASTIC_BLOCK_GRAPH  Create a stochastic block graph
%   Usage:  G = gsp_stochastic_block_graph( N );
%           G = gsp_stochastic_block_graph(N , k);
%
%   Input parameters:
%         N     : Number of nodes (default 1024)
%         k     : Number of clusters (default 5)
%         params: Structure of optional parameters
%   Output parameters:
%         G     : Graph structure.
%
%   *param* is an optional structure with the following fields
%
%   * *params.p*        : Intra-cluster edge probability (default 0.7)
%   * *params.q*        : Inter-cluster edge probability (default 0.3/k)
%   * *params.z*        : Assignment vector of nodes (default uniform random)
%   * *params.M*        : Link probability matrix between clusters (default uses p and q)
%   * *params.directed* : Flag the graph as directed or not (default false)
%
%   Use the stochastic block model to create a graph.

% Author: Pierre Vandergheynst, Nathanael Perraudin
% Date  : 2 novemeber 2015 (revision: 6 october 2016 -- Lionel Martin)

%% Stochastic Block Model generator
if nargin<1
    N = 1024; % number of nodes
end
if nargin<2
    k = 5; % number of clusters
end
if nargin < 3
    params = struct;
end

if ~isfield(params, 'p')
    params.p = 0.7;
end

if ~isfield(params, 'q')
    params.q = (1-params.p) / k;
end

if ~isfield(params, 'force_full')
    params.force_full = 0;
end

if ~isfield(params, 'auto_gen_M')
    params.auto_gen_M = 0;
end

if (~isfield(params, 'z') || length(params.z) ~= N || max(params.z) > k || min(params.z) < 1)
    params.z = randi(k, 1, N);
end

if (~isfield(params, 'M') || size(params.M) ~= [k, k])
    if params.force_full
        params.M = params.q * ones(k);
        params.M(1:k+1:end) = params.p;
    end
    params.auto_gen_M = 1;
end

if ~isfield(params, 'directed')
    params.directed = false;
end

%% partition with clusters of homogenous sizes
%
% THIS PART IS NOT NECESSARY BECAUSE SLOW AND REPLACED
% BY THE SIMPLE "UNIFORM" CLUSTER ASSIGNMENT ABOVE
%
% L = ceil(N/k);
% for i=1:k-1
%     z((i-1)*L + 1:i*L) = i;
% end
% z((k-1)*L + 1:end) = k;

%% Generate adjacency
z = params.z;

% for i=1:N
%     for j=i+1:N
%         W(i, j) = ( rand <= M(z(i), z(j)) );
%         if params.directed
%             W(j, i) = ( rand <= M(z(j), z(i)) );
%         else
%             W(j, i) = W(i, j);
%         end
%     end
% end
if params.auto_gen_M && ~params.force_full
    [val_z, idx_z] = sort(z);
    counts = diff(find(diff([0, val_z, N])));

    if length(counts) ~= k
        error('There is at least one empty class. Check your z.');
    end

    W = sprandsym(counts(k), params.p);
    nb_cols_rect = 0;

    for i=k:-1:2
        nb_cols_rect = nb_cols_rect + counts(i);
        rect = sprand(counts(i-1), nb_cols_rect, params.q);
        top_left = sprandsym(counts(i-1), params.p);
        W = vertcat(horzcat(top_left, rect), horzcat(rect', W));
    end

    W(1:N+1:end) = 0;
    G.W(idx_z, idx_z) = abs(W) > 0;

else
    M = params.M;

    if params.directed
        W = rand(N) <= M(z, z);
    else
        A = rand(N);
        A(logical(triu(ones(N)))) = 1;
        W = A <= M(z, z);
        W = W + W';
    end

    G.W = sparse(W);
end

G = gsp_graph_default_parameters(G);
G.info.node_com = z;

G.coords = ones(N, 2);
com_coords = sqrt(N) * [-cos(2*pi*(1:k)/k)', sin(2*pi*(1:k)/k)'];

% create uniformly random points in the unit disc
for ii = 1:N
    % use rejection sampling to sample from a unit disc (probability = pi/4)
    while norm(G.coords(ii, :)) >= 1/2
        % sample from the square and reject anything outside the circle
        G.coords(ii, :) = [rand-.5, rand-.5];
    end
end

% add the offset for each node depending on which community it belongs to
for ii = 1:k
    idx_ii = find(z==ii);
    rad_com = sqrt(numel(idx_ii));
    G.coords(idx_ii, :) = bsxfun(@plus, rad_com * G.coords(idx_ii, :), com_coords(ii, :));
end

end


function [R,eff] = randmio_und_signed(W, ITER)
% RANDMIO_UND_SIGNED	Random graph with preserved signed degree distribution
%
%   R       = randmio_und_signed(W,ITER);
%   [R,eff] = randmio_und_signed(W,ITER);
%
%   This function randomizes an undirected network with positively and
%   negatively signed connections, while preserving the positively and
%   negatively signed degree distribution. The function does not preserve
%   the strength distribution in weighted networks.
%
%   Input:      W,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%
%   Output:     R,      randomized network
%               eff,    number of actual rewirings carried out
%
%   Reference:  Maslov and Sneppen (2002) Science 296:910
%
%
%   2011-2015
%   Dani Bassett, UCSB
%   Olaf Sporns,  Indiana U
%   Mika Rubinov, U Cambridge

%   Modification History:
%   Mar 2011: Original (Dani Bassett, based on randmio_und.m)
%   Mar 2012: Limit number of rewiring attempts,
%             count number of successful rewirings (Olaf Sporns)
%   Dec 2015: Rewritten the core of the rewiring algorithm to allow
%             unbiased exploration of all network configurations. The new
%             algorithm allows positive-positive/negative-negative
%             rewirings, in addition to the previous positive-positive/0-0
%             and negative-negative/0-0 rewirings (Mika Rubinov).

if nargin('randperm')==1
    warning('This function requires a recent (>2011) version of MATLAB.')
end

R     = double(W);              % sign function requires double input
n     = size(R,1);
ITER  = ITER*n*(n-1)/2;

% maximal number of rewiring attempts per 'iter'
maxAttempts = round(n/2);
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)    %while not rewired
        %select four distinct vertices
        nodes = randperm(n,4);
        a = nodes(1);
        b = nodes(2);
        c = nodes(3);
        d = nodes(4);

        r0_ab = R(a,b);
        r0_cd = R(c,d);
        r0_ad = R(a,d);
        r0_cb = R(c,b);

        %rewiring condition
        if      (sign(r0_ab)==sign(r0_cd)) && ...
                (sign(r0_ad)==sign(r0_cb)) && ...
                (sign(r0_ab)~=sign(r0_ad))

            R(a,d)=r0_ab; R(a,b)=r0_ad;
            R(d,a)=r0_ab; R(b,a)=r0_ad;
            R(c,b)=r0_cd; R(c,d)=r0_cb;
            R(b,c)=r0_cd; R(d,c)=r0_cb;

            eff = eff+1;
            break;
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations


function mt = SupIntensity_HP(t, History, para, options)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compute the super bound of intensity function of Hawkes processes
%
% Parameters of Hawkes processes
% para.mu: base exogenous intensity
% para.A: coefficients of impact function
% para.kernel: 'exp', 'gauss'
% para.w: bandwith of kernel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(History)
    mt = sum(para.mu);
else
    Time = History(1, :);
    index = Time<=t;
    Time = Time(index);
    Event = History(2, index);

    MT = sum(para.mu)*ones(1, options.M);
    for m=1:options.M
        t_current = t+(m-1)*options.tstep/options.M;

        basis = Kernel(t_current-Time(:), para);
        A = para.A(Event, :, :);

        for c = 1:size(para.A, 3);
            MT(m) = MT(m) + sum(sum(basis.*A(:,:,c)));
        end
    end
    mt = max(MT);
end

mt = mt.*(mt>0);
end


% A = riemann_mean(B,epsilon,tol)
%
% Calcul du barycentre des matrice de covariances.
% A : baricentre des K matrices NxN
%
% B : Matrice NxNxK
% epsilon : Pas de la descente de gradient
% tol : arret de la descente si le crit�re < tol

function [A critere niter] = opttransp_mean(B,args)
Imat = size(B,3);
N_itermax = 200;
if (nargin<2)||(isempty(args))
    tol = 10^-3;
    %A = mean(B,3);
    A = eye(size(B,1));
else
    tol = args{1};
    A = args{2};
end

niter = 0;
fc = 0;
K = A^(0.5);

while (niter<N_itermax)
    niter = niter+1;

    Ktmp = 0;
    for i=1:Imat
       Ktmp = Ktmp + (K*B(:,:,i)*K)^(0.5);
    end
    Ktmp = (Ktmp)^(0.5);

    fcn = norm(Ktmp-K,'fro');
    K = Ktmp;
    % improvement
    conv = abs((fcn-fc)/fc);
    if conv<tol % break if the improvement is below the tolerance
       break;
    end
    fc = fcn;
end

A = ((1/Imat)*K)^2;

if niter==N_itermax
    disp('Warning : Nombre d''itérations maximum atteint');
end

critere = fc;


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Unified Solutions,
%           Solvability/Observability Analysis and Uncertainty Description
%           in A Globally Optimal Framework.
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com

function [XX, ff] = optimize_quat_cov(AA, bb, q, F, cov_left, epsX, epsQuat, solver, verbose)
X = sdpvar(4, 4);
res = AA * vec(X) - bb;
f = res.' * res;
cons = [
    X - epsX * eye(4) >= 0, ...
    vec(F * X * F.' - cov_left) == 0, ...
    q.' * X * q <= epsQuat
    ];
options = sdpsettings('solver', solver, 'verbose', verbose);
if(strcmp(solver, 'sdpa_gmp'))
    options.sdpa_gmp.epsilonDash = 1.0e-35;
    options.sdpa_gmp.precision = 250;
end
optimize(cons, f, options);
XX = value(X);
ff = value(f);
end


function [pE,pC] = spm_null_priors(A,B,C)
% prior moments for null (Jacobian) model
% FORMAT [pE,pC] = spm_null_priors(A,B,C)
%
% A{1},B{m},C  - binary constraints on extrinsic connections
%
% pE - prior expectation
% pC - prior covariance
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_null_priors.m 5908 2014-03-05 20:31:57Z karl $

% default: a single source model
%--------------------------------------------------------------------------
if nargin < 3
    A   = {1};
    B   = {};
    C   = 1;
end

% orders
%--------------------------------------------------------------------------
n     = size(C,1);                                % number of sources
u     = size(C,2);                                % number of inputs

% parameters for Jacobian
%==========================================================================

% canonical source
%--------------------------------------------------------------------------
a     = 8;
Hz    = [8 16 48];
for i = 1:length(Hz)
   b      = 2*pi*Hz(i);
   J{i,i} = [a b;-b a];
end

% Jacobian
%--------------------------------------------------------------------------
J     = full(spm_cat(J));
nx    = length(J);
pE.A  = logm(kron(eye(n,n),J));
pC.A  = kron(A{1},ones(nx,nx));

% Bilinear terms
%--------------------------------------------------------------------------
for i = 1:u
    pE.B{i} = kron(B{i},zeros(nx,nx));
    pC.B{i} = kron(B{i},ones(nx,nx));
end

% input coeficicents
%--------------------------------------------------------------------------
pE.C  = kron(C,0);
pC.C  = C;

% input coeficicents
%--------------------------------------------------------------------------
pE.D  = kron(zeros(n,n),ones(nx,1));
pC.D  = kron(speye(n,n),ones(nx,1));


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

check_active_images;

if ~exist('solution_error')
   solution_error = zeros(6*n_ima + 15,1);
end;

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

fc_error = solution_error(1:2);
cc_error = solution_error(3:4);
alpha_c_error = solution_error(5);
kc_error = solution_error(6:10);

% Calibration matrix:

KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1];
inv_KK = inv(KK);

% Extract the extrinsic paramters, and recomputer the collineations

for kk = 1:n_ima,

   if active_images(kk),

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

      omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
      Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6);

   	Rckk = rodrigues(omckk);

   	Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];

   	Hkk = Hkk / Hkk(3,3);

   else

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

   end;

   eval(['omc_' num2str(kk) ' = omckk;']);
   eval(['Rc_' num2str(kk) ' = Rckk;']);
   eval(['Tc_' num2str(kk) ' = Tckk;']);
   eval(['H_' num2str(kk) '= Hkk;']);
   eval(['omc_error_' num2str(kk) ' = omckk_error;']);
   eval(['Tc_error_' num2str(kk) ' = Tckk_error;']);

end;


%% contour2logic
% Below is a demonstration of the features of the |contour2logic| function

%%
clear; close all; clc;

%% Syntax
% |[varargout]=contour2logic(M,v,Vcs);|

%% Description
% This function converts contours to a logic or labelled data. The logics
% represent wether voxels are in, on, or outside the contour.
%
% The input consists of:
%%
%
% * A 3D image |M| (or alternatively the size of M).
% * The |vozelSize| a 1x3 vector specifying the size of the voxels in the
% row, column, and slice direction.
% * A cell array |Vcs| containing one or more contours per slice. If the
% image has n slices then Vcs should be an nx1 cell array, i.e. contours
% are defined for each slice.

%% Examples
%
%% Import image data for this demo

defaultFolder = fileparts(fileparts(mfilename('fullpath'))); %Set main folder
pathNameImageData=fullfile(defaultFolder,'data','DICOM','0001_human_calf');
loadNameImageData=fullfile(pathNameImageData,'IMDAT','IMDAT.mat');
IMDAT_struct=load(loadNameImageData); %The image data structure
G = IMDAT_struct.G; %Geometric/spatial information
v=G.v; %The voxel size
M= IMDAT_struct.type_1; %The image data

%%
contourName='imseg_calf_tibia';
pathName=fullfile(defaultFolder,'data','imseg'); %Folder name for contours

%% Compute levelset

loadName=fullfile(pathName,contourName);
load(loadName); %Load segmentation structure
Vcs=saveStruct.ContourSet; %Access the contour data

[logicIn,logicOn,N]=contour2logic(M,v,Vcs);

%%
% Visualize logic image and contours together

%Visualize logic image
sv3(logicIn,v); %Open slice viewer for levelset

%Visualize contours
optionStruct.Color='r';
plotContours({Vcs},optionStruct);  %Plot contours

%Add colorbar labels
[~,hc]=icolorbar;
hc.TickLabels={'Out','In'};

drawnow;

%%
% Visualize labelled image and contours together

%Visualize label image
vizStruct.colormap=viridis(3); %Set colormap for levelset visualization
hf2=sv3(N,v,vizStruct); %Open slice viewer for levelset

%Visualize contours
optionStruct.Color='r';
plotContours({Vcs},optionStruct);  %Plot contours

%Add colorbar labels
[~,hc]=icolorbar;
hc.TickLabels={'Out','On','In'};

drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


function [wX, wY, xdim, ydim, n] = windowizeNARX(X,Y,x_delays,y_delays, steps)
% Re-arrange the data points into a block Hankel matrix for (N)ARX time-series modeling
%
% >> [Xw,Yw] = windowizeNARX(X,Y,xdelays, ydelays, steps)
%
%  Rearrange the points of X and Y in a regressor matrix of the
%  past inputs and outputs (Xw) and the future outputs (Yw).
%
% Full syntax
%
% >> [Xw, Yw, xdim, ydim, n] = windowizeNARX(X, Y, xdelays, ydelays, steps)
%
%       Outputs
%         Xw Matrix of the data used for input including the delays
%         Yw Matrix of the data used for output including the next steps
%         xdim(*) Number of dimensions in new input
%         ydim(*) Number of dimensions in new output
%         n(*) Number of new data points
%       Inputs
%         X       : N x m vector with input data points
%         Y       : N x d vector with output data points
%         xdelays : Number of lags of X in new input
%         ydelays : Number of lags of Y in new input
%         steps(*): Number of future steps of Y in new output (by default 1)
%
% See also:
%   windowize, predict, trainlssvm, simlssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

m=max(x_delays,y_delays);

eval('steps;','steps = 1;');
if steps == 0,
  n = size(X,1)-m;
else
  n = size(X,1)-m -steps+1;
end

wX = zeros(n,size(X,2)*(x_delays+1)+size(Y,2)*(y_delays));
wY = zeros(n,size(Y,2)*steps);
xdim = size(wX,2);
ydim = size(wY,2);

hdx  = (x_delays+1)*size(X,2);

for t=1:n,
  for i=1:x_delays+1,
    wX(t,1+((i-1)*size(X,2):i*size(X,2)-1)) = X(t+m-x_delays+i-1,:);
  end

  for i=1:y_delays,
    wX(t,hdx + (i-1)*size(Y,2) + (1:size(Y,2))) = Y(t+m-y_delays+i-1,:);
  end

  for i=1:steps,
    wY(t,i:i+size(Y,2)-1) = Y(t+m+i-1,:);
  end

end


function x = bradford_cdf_inv ( cdf, a, b, c )

%*****************************************************************************80
%
%% BRADFORD_CDF_INV inverts the Bradford CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Input, real A, B, C, the parameters of the PDF.
%    A < B,
%    0.0 < C.
%
%    Output, real X, the corresponding argument of the CDF.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BRADFORD_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'BRADFORD_CDF_INV - Fatal error!' );
  end

  if ( cdf <= 0.0 )
    x = a;
  elseif ( cdf < 1.0 )
    x = a + ( b - a ) * ( ( c + 1.0 )^cdf - 1.0 ) / c;
  elseif ( 1.0 <= cdf )
    x = b;
  end

  return
end


function [tfVMVariates] = vmrand(fMu, fKappa, varargin)

% vmrand - FUNCTION Draw random variates from the Von Mises circular distribution
%
% Usage: [tfVMVariates] = vmrand(fMu, fKappa)
%        [tfVMVariates] = vmrand(..., M)
%        [tfVMVariates] = vmrand(..., M, N, P, ...)
%        [tfVMVariates] = vmrand(..., [M N P ...])
%
% This function uses an envelope-rejection method based on a wrapped Cauchy
% distribution to draw random variates from an arbitrary Von Mises
% distribution, first proposed in [1].
%
% 'fMu' and 'fKappa' are the mean and variance parameter of the Von Mises
% distribution over [-pi, pi).  'tVMVariates' will be a tensor containing
% random variates drawn from the defined distribution.  If 'fMu' and
% 'fKappa' are non-scalar, then they must be the same size.  In this case
% 'tVMVariates' will be the same size.  If 'fMu' and 'fKappa' are scalar,
% then the number of variates returned can be specified as extra arguments.
%
% If only single dimension 'M' is provided, then the return argument
% 'tfVMVariates' will be M x M.
%
% This implementation is vectorised, and requires O(7.5*N) space.
%
% References:
% [1] D. J. Best and N. I. Fisher, 1979. "Efficient Simulation of the von
% Mises Distribution", Journal of the Royal Statistical Society. Series C
% (Applied Statistics), Vol. 28, No. 2, pp. 152-157.

% Author: Dylan Muir <muir@hifo.uzh.ch>
% Created: 19th June, 2012

% -- Check arguments

if (nargin < 2)
   help vmrand;
   error('VMRAND:Usage', '*** vmrand: Incorrect usage');
end

% -- Check sizes

vbScalarArgs = [isscalar(fMu) isscalar(fKappa)];

if (nnz(vbScalarArgs) == 1)
   if vbScalarArgs(1)
      % - fMu is scalar
      fMu = repmat(fMu, size(fKappa));
   else
      % - fKappa is scalar
      fKappa = repmat(fKappa, size(fMu));
   end

   % - Set return sizes
   vnTensorSize = size(fMu);

elseif (nnz(vbScalarArgs == 0))
   % - Two non-scalar arguments
   if (~isequal(size(fMu), size(fKappa)))
      error('VMRAND:UnequalSizes', ...
         '*** vmrand: ''fMu'' and ''fKappa must be the same size.');
   else
      vnTensorSize = size(fMu);
   end

elseif (~isempty(varargin))
   % - Get argument sizes from varargin (be forgiving)
   varargin = cellfun(@(c)(reshape(c, 1, [])), varargin, 'UniformOutput', false);
   vnTensorSize = [varargin{:}];

   % - Take Matlab semantics to make square matrices
   if (isscalar(vnTensorSize))
      vnTensorSize = vnTensorSize([1 1]);
   end

   fKappa = repmat(fKappa, vnTensorSize);

else
   % - Return a scalar variate
   vnTensorSize = [1 1];
end

% -- Check values

if (any(fKappa < 0))
   error('VMRAND:InvalidArguments', ...
      '*** vmrand: ''fKappa'' must be >= 0');
end

if (any(vnTensorSize < 1))
   error('VMRAND:InvalidArguments', ...
      '*** vmrand: Tensor size dimensions must be positive');
end

% -- Preallocate data

tfVMVariates = nan(vnTensorSize);
tfZ = nan(vnTensorSize);
tfF = nan(vnTensorSize);
tfC = nan(vnTensorSize);

% -- Short-cut fKappa == 0

tbUniform = fKappa == 0;
tfVMVariates(tbUniform) = rand(nnz(tbUniform), 1) * 2*pi - pi;
tbDraw = ~tbUniform;
tbAccept = tbUniform;

% -- Pre-compute what we can

if (all(vbScalarArgs))
   tfTau = sqrt(4 .* fKappa(1).^2 + 1) + 1;
   tfRho = (tfTau - sqrt(2 .* tfTau)) ./ (2 .* fKappa(1));
   tfR = repmat((1 + tfRho.^2) ./ (2 .* tfRho), vnTensorSize);

else
   tfTau = sqrt(4 .* fKappa.^2 + 1) + 1;
   tfRho = (tfTau - sqrt(2 .* tfTau)) ./ (2 .* fKappa);
   tfR = (1 + tfRho.^2) ./ (2 .* tfRho);
   clear tfTau tfRho;   % - To save some space
end

% -- Draw random variates

while (nnz(tbDraw > 0))
   % - Draw partial variates and estimate wrapped Cauchy distribution envelope
   nNumToDraw = nnz(tbDraw);
   tfZ(tbDraw) = cos(pi .* rand(nNumToDraw, 1));
   tfF(tbDraw) = (1 + tfR(tbDraw) .* tfZ(tbDraw)) ./ (tfR(tbDraw) + tfZ(tbDraw));
   tfC(tbDraw) = fKappa(tbDraw) .* (tfR(tbDraw) - tfF(tbDraw));

   % - Filter variates
   vfRand2 = rand(nNumToDraw, 1);
   tbAccept(tbDraw) = (tfC(tbDraw) .* (2 - tfC(tbDraw)) - vfRand2) > 0;
   vbRecheck = ~tbAccept(tbDraw);
   if (any(vbRecheck))
      tbAccept(tbDraw & ~tbAccept) = (log(tfC(tbDraw & ~tbAccept) ./ vfRand2(vbRecheck)) + 1 - tfC(tbDraw & ~tbAccept)) >= 0;
   end

   % - Construct final variates
   nNumToAccept = nnz(tbDraw & tbAccept);
   tfVMVariates(tbDraw & tbAccept) = sign(rand(nNumToAccept, 1) - 0.5) .* acos(tfF(tbDraw & tbAccept));

   % - Mark as being accepted (don't need to try again)
   tbDraw(tbAccept) = false;
end

% -- Shift variates to mean

tfVMVariates = tfVMVariates + fMu;
tfVMVariates(tfVMVariates > pi) = -2*pi + tfVMVariates(tfVMVariates > pi);
tfVMVariates(tfVMVariates < -pi) = 2*pi + tfVMVariates(tfVMVariates < -pi);

% --- END of vmrand.m ---


function sparse_count_test025 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% SPARSE_COUNT_TEST025 tests CFN_E_SIZE_TOTAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST025\n' );
  fprintf ( 1, '  CFN_E_SIZE_TOTAL returns the number of\n' );
  fprintf ( 1, '  points in a CFN_E sparse grid made from \n' );
  fprintf ( 1, '  any closed fully nested family of 1D quadrature\n' );
  fprintf ( 1, '  rules with exponential growth, including:\n' );
  fprintf ( 1, '  * CC_E, the Clenshaw Curtis Exponential Growth family;\n' );
  fprintf ( 1, '  * NCC_E, the Newton Cotes Closed Exponential Growth family.\n' );
  fprintf ( 1, '  No reduction is made because of repeated points.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = cfn_e_size_total ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function perm=topological_order(A,varargin)
% TOPOLOGICAL_ORDER Returns the topological ordering of vertices in a dag
%
% perm=topological_order(A) generates a permutation of the vertices that
% indices a topological order.  A topological order satisfies the property
% that edge (i,j) in A implies i comes before j in the order.  A
% topological order only exists for directed acyclic graphs.  If A is not a
% directed acyclic graph, then the returned perm is empty.
%
% This method works on directed graphs.
% The runtime is O(V+E).
%
% ... = topological_order(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   There are no additional options for this function.
%
% Note: this function does not depend upon the non-zero values of A, but
% only uses the non-zero structure of A.
%
% Example:
%   n = 10; A = sparse(1:n-1, 2:n, 1, n, n); % construct a simple dag
%   p = topological_order(A);
%
% See also TEST_DAG

% David Gleich
% Copyright, Stanford University, 2007-2008

%% History
%  2008-09-23: Reformatted common section.
%  2008-10-07: Fixed example.
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end
if check, check_matlab_bgl(A,struct()); end
if trans, A = A'; end

[perm dag] = topological_order_mex(A);

% in this case it wasn't a dag
if dag == 0
    perm = [];
end


% vgg_vec  De-/vectorization of a matrix.
%
% For a matrix X, vgg_vec(X) = X(:).
% For a N^2-vector x, vgg_vec(x) = reshape(x,N,N).
%
% Classical matrix re-arrangement operator, see book Magnus-Neudecker.
% Trivial function, included mainly for consistency with notation in literature.
%
% Useful for rearranging matrix equations. Matrix from the middle of a product can be put to the right as
%
%    vgg_vec(A*B*C) = kron(C',A)*vgg_vec(B)
%
% See also vgg_vec_swap, vgg_commut_matrix, vgg_duplic_matrix.

function v = vgg_vec(A)

if any(size(A)==1)
  v = reshape(A,[1 1]*sqrt(prod(size(A))));
else
  v = A(:);
end

return


function val=vec(A)
%%VEC Given A matrix, return the values of the matrix stacked columnwise.
%     This is just the same as A(:), but this function can be more
%     convenient to use when programming equations as one can
%     say things like c=b+vec(A*B) rather than temp=A*B; c=b+temp(:).
%
%INPUTS: A A matrix.
%
%OUTPUTS: val The values in the matrix stacked column-wise. If A is a
%             hypermatrix, then the values are stacked in order of the
%             indices of A (rows, columns, hypercolumns, etc.).
%
%February 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

val=A(:);
end

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


function [ber, rate] = MO_method()

global  H  Vn W_mopt Nt  Nrf Nr;
i = 0;

W_equal = W_mopt;
w = trace (W_equal' * W_equal);
%random initialization
V_RF = exp( 1i*unifrnd(0,2*pi,Nt,Nrf));
W_RF = exp( 1i*unifrnd(0,2*pi,Nr,Nrf));
%iteration trigger, the normal initialization just for pass into functions
trigger = 1;
m_MSE_new = 100;

%limit the iterations number by i<10
while (trigger > 1e-5)

    % precoding
    H1 = H' * W_equal;
    Vn1 = Vn * w;
    [V_RF, V_U] = mo_algorithm(V_RF, Vn1, H1);
    V_equal = V_RF *V_U;
    v = trace (V_equal * V_equal');   %beta^(-2)

    %combining
    H2 = H * V_equal;
    Vn2 = Vn * v;
    [W_RF, W_B] = mo_algorithm(W_RF, Vn2, H2);
    W_equal = W_RF * W_B;
    w = trace (W_equal' * W_equal);
    %modified MSE
    H_equal = W_equal'*H2;

    m_MSE_old = m_MSE_new;
    m_MSE_new = trace(H_equal * H_equal' - H_equal - H_equal') + Vn * v * w;
    trigger = m_MSE_old - m_MSE_new;

    i = i + 1;
end

V_B = V_U / sqrt(v);

V = V_RF * V_B;
W = W_RF * W_B;

ber = get_ber(V, W);
rate = get_rate(V, W);


function number = next_prime(number)
    while ~isprime(number + 1)
        number = number + 1;
    end
    number = number + 1;
end


%MDL_PHANTOMX Create model of PhantomX pincher manipulator
%
% MDL_PHANTOMX is a script that creates the workspace variable px which
% describes the kinematic characteristics of a PhantomX Pincher Robot, a 4
% joint hobby class  manipulator by Trossen Robotics.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Notes::
% - Uses standard DH conventions.
% - Tool centrepoint is middle of the fingertips.
% - All translational units in mm.
%
% Reference::
%
% - http://www.trossenrobotics.com/productdocs/assemblyguides/phantomx-basic-robot-arm.html

% MODEL: Trossen Robotics, PhantomX Pincher, 4DOF, standard_DH

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

clear L
L(1) = Revolute('d', 40, 'alpha', -pi/2);
L(2) = Revolute('a', -105, 'alpha', pi, 'offset', pi/2);
L(3) = Revolute('a', -105);
L(4) = Revolute('a', -105);

% Note alpha_2 = pi, needed to account for rotation axes of joints 3 and 4 having
% opposite sign to joint 2.
%
% s='Rz(q1) Tz(L1) Ry(q2) Tz(L2) Ry(q3) Tz(L3) Ry(q4) Tz(L4)'
% DHFactor(s)

px = SerialLink(L, 'name', 'PhantomX', 'manufacturer', 'Trossen Robotics');
qz = [0 0 0 0];
px.tool = trotz(-pi/2) * trotx(pi/2);


% DATES
% Converts between the following date formats:
%  - Year, Month, Day
%  - Year, Day-of-Year
%  - Year, Decimal Year
%  - GPS week, seconds of week
%
% Required M-files: cal2jd, doy2jd, gps2jd, jd2cal, jd2dow, jd2doy, jd2gpsa

% Created 24 May 96, M. Craymer
% Modified 03 Aug 96, Converted to MATLAB
% Modified 29 Sep 98, Convert each option to all other formats
% Modified 25 Mar 99, Converted to use isempty
% Modified 26 Apr 99, Rewrote to use new date conversion routines
%                     Added conversion of GPS week and day of week
% Modified 19 Feb 11, Replaced clear all with clear for specific variables used

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

while 1
clear ver day month buf fmt date ok iyr mn dy jd doy yr dow gpsweek sow rollover
warning on
ver = '1.0 (99.04.26)';
day = ['Sun';'Mon';'Tue';'Wed';'Thu';'Fri';'Sat'];
month = ['Jan';'Feb';'Mar';'Apr';'May';'Jun';'Jul';'Aug';'Sep';'Oct';'Nov';'Dec'];

fprintf(1,'\n\n');
fprintf(1,'-------------------------------------------------\n');
fprintf(1,' DATES: Converts between different date formats.\n');
fprintf(1,'       M.Craymer, v%s\n',ver);
fprintf(1,'-------------------------------------------------\n');

fprintf(1,'\n');
fprintf(1,'Available input date formats:\n');
fprintf(1,' 1) Year, Month, Day\n');
fprintf(1,' 2) Year, Day of Year\n');
fprintf(1,' 3) Year (including decimal year)\n');
fprintf(1,' 4) GPS Week, Sec of Week, Number Rollovers\n');
fprintf(1,' 5) Julian Date\n');
fprintf(1,'\n');

buf = input('Enter date format and date [quit] > ','s');
if isempty(buf)
  fmt = 0;
else
  date = sscanf(buf,'%f');
  fmt = date(1);
end
ok = 1;

%----- Exit program
if fmt == 0
  clear ver day month buf fmt date ok iyr mn dy jd doy yr dow gpsweek sow rollover
  return;

%----- Day, month, year
elseif fmt == 1
  if length(date) < 4
    warning('Too few date arguments entered');
    ok = 0;
  else
    iyr = date(2);
    mn = date(3);
    dy = date(4);
    if mn<1 | mn>12
      warning('Invalid month');
      ok = 0;
    end
    if dy<1 | dy>=32
      warning('Invalid day');
      ok = 0;
    end
    if ok
      jd = cal2jd(iyr,mn,dy);
      doy = jd2doy(jd);
      yr = jd2yr(jd);
      dow = jd2dow(jd);
      [gpsweek,sow,rollover] = jd2gps(jd);
    end
  end

%----- Year, day of year
elseif fmt == 2
  if length(date) < 3
    warning('Too few date arguments entered')
    ok = 0;
  else
    iyr = date(2);
    doy = date(3);
    if doy<1 | doy>=367
      warning('Invliad day of year');
      ok = 0;
    end
    if ok
      jd=doy2jd(iyr,doy);
      [iyr,mn,dy]=jd2cal(jd);
      yr = jd2yr(jd);
      dow = jd2dow(jd);
      [gpsweek,sow,rollover] = jd2gps(jd);
    end
  end

%----- Year & decimal of year
elseif fmt == 3
  if length(date) < 2
    warning('No date arguments entered');
    ok = 0;
  else
    yr = date(2);
    jd=yr2jd(yr);
    [iyr,mn,dy]=jd2cal(jd);
    doy = jd2doy(jd);
    dow = jd2dow(jd);
    [gpsweek,sow,rollover] = jd2gps(jd);
  end

%----- GPS week, sec of week
elseif fmt == 4
  if length(date) < 4
    rollover = 0;
  else
    rollover = date(4);
  end
  if length(date) < 3
    sow = 0;
  else
    sow = date(3);
  end
  if length(date) < 2
    warning('No date arguments entered');
    ok = 0;
  else
    gpsweek = date(2);
    if gpsweek<1 | gpsweek>=1025
      warning('Invalid GPS week');
      ok = 0;
    end
    if sow<0 | sow>604800
      warning('Invliad sec of week');
      ok = 0;
    end
    if ok
      jd = gps2jd(gpsweek,sow,rollover);
      [iyr,mn,dy]=jd2cal(jd);
      doy = jd2doy(jd);
      yr = jd2yr(jd);
      dow = jd2dow(jd);
    end
  end

%----- Julian date
elseif fmt == 5
  if length(date) < 2
    warning('No date arguments entered');
    ok = 0;
  else
    jd = date(2);
    if jd<1
      warning('Invalid Julian date');
      ok = 0;
    end
    if ok
      [iyr,mn,dy]=jd2cal(jd);
      doy = jd2doy(jd);
      yr = jd2yr(jd);
      dow = jd2dow(jd);
      [gpsweek,sow,rollover] = jd2gps(jd);
    end
  end

%----- Invalid date format
else
  warning('Undefined date format entered');
  ok = 0;
end

%----- List results
if ok
  fprintf(1,'\n');
  fprintf(1,'Date    %4d-%2d-%4.1f\n',[iyr,mn,dy]);
  fprintf(1,'Year    %12.4f\n',yr);
  fprintf(1,'Day of Year   %6.1f\n',doy);
  fprintf(1,'Day of Week      %s\n',day(dow,:));
  fprintf(1,'GPS Week      %6d\n',gpsweek);
  fprintf(1,'Sec of Week   %6.0f\n',sow);
  fprintf(1,'Julian Date   %9.1f\n',jd);
end

end %while


function program_05 ( )

%*****************************************************************************80
%
%% PROGRAM_05 displays the points that sample a triangle.
%
%  Discussion:
%
%    This program is similar to program_04, but displays the
%    sample points.
%
%    The program
%    * reads a triangle T (defined by three points),
%    * reads a random number seed;
%    * reads "1" for bad scheme, "2" for good scheme.
%    * reads N, the number of random values to generate;
%    * it then computes N random points in the triangle;
%    * it determines N1, N2, and N3, the number of points
%      that ended up in subtriangles mbc, amc, and abm,
%      where "m" is the centroid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_05 - The Eyeball Test on Sampling\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define a triangle T:\n' );

  t_v1 = input ( '  Enter [ T.v1.x, T.v1.y]: ' );
  t_v2 = input ( '  Enter [ T.v2.x, T.v2.y]: ' );
  t_v3 = input ( '  Enter [ T.v3.x, T.v3.y]: ' );
%
%  Get the random seed.
%
  seed = input ( 'Enter a random number seed:  ' );
  rand ( 'state', seed );
%
%  Get the scheme to use.
%
  scheme = input ( 'Enter 1 for "bad scheme", 2 for "good scheme":  ' );
%
%  Get the number of values to generate.
%
  n = input ( 'Enter the number of samples to generate:  ' );

  n1 = 0;
  n2 = 0;
  n3 = 0;

  p = zeros ( 2, n );

  for i = 1 : n

    if ( scheme == 1 )
      r = rand;
      s = rand;
      t = rand;
      xi1 = r / ( r + s + t );
      xi2 = s / ( r + s + t );
      xi3 = t / ( r + s + t );
    elseif ( scheme == 2 )
      r = rand;
      s = rand;

      xi1 =   1.0       - sqrt ( s );
      xi2 = ( 1.0 - r ) * sqrt ( s );
      xi3 =         r   * sqrt ( s );
    end

    if ( xi1 < xi2 & xi1 < xi3 )
      n1 = n1 + 1;
    elseif ( xi2 < xi1 & xi2 < xi3 )
      n2 = n2 + 1;
    elseif ( xi3 < xi1 & xi3 < xi2 )
      n3 = n3 + 1;
    end

    p(1:2,i) = xi1 * t_v1(1:2) ...
             + xi2 * t_v2(1:2) ...
             + xi3 * t_v3(1:2);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N1 = %8d  %% = %8.4f\n', n1, 100 * n1 / n );
  fprintf ( 1, '  N2 = %8d  %% = %8.4f\n', n2, 100 * n2 / n );
  fprintf ( 1, '  N3 = %8d  %% = %8.4f\n', n3, 100 * n3 / n );
  fprintf ( 1, '  N  = %8d  %% = %8.4f\n', n, 100 );

  scatter ( p(1,:), p(2,:), 'filled' );
  m = ( t_v1 + t_v2 + t_v3 ) / 3;

  line ( [ t_v1(1), t_v2(1) ], [ t_v1(2), t_v2(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v2(1), t_v3(1) ], [ t_v2(2), t_v3(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v3(1), t_v1(1) ], [ t_v3(2), t_v1(2) ], 'Color', 'r', 'LineWidth', 2.0 );

  line ( [ t_v1(1), m(1) ], [ t_v1(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v2(1), m(1) ], [ t_v2(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );
  line ( [ t_v3(1), m(1) ], [ t_v3(2), m(2) ], 'Color', 'r', 'LineWidth', 2.0 );

  p_min(1) = min ( p(1,:) );
  p_min(1) = min ( p_min(1), t_v1(1) );
  p_min(1) = min ( p_min(1), t_v2(1) );
  p_min(1) = min ( p_min(1), t_v3(1) );

  p_max(1) = max ( p(1,:) );
  p_max(1) = max ( p_max(1), t_v1(1) );
  p_max(1) = max ( p_max(1), t_v2(1) );
  p_max(1) = max ( p_max(1), t_v3(1) );

  p_min(2) = min ( p(2,:) );
  p_min(2) = min ( p_min(2), t_v1(2) );
  p_min(2) = min ( p_min(2), t_v2(2) );
  p_min(2) = min ( p_min(2), t_v3(2) );

  p_max(2) = max ( p(2,:) );
  p_max(2) = max ( p_max(2), t_v1(2) );
  p_max(2) = max ( p_max(2), t_v2(2) );
  p_max(2) = max ( p_max(2), t_v3(2) );

  p_range(1) = p_max(1) - p_min(1);
  p_range(2) = p_max(2) - p_min(2);

  margin = 0.025 * max ( p_range(1), p_range(2) );

  x_min = p_min(1) - margin;
  x_max = p_max(1) + margin;
  y_min = p_min(2) - margin;
  y_max = p_max(2) + margin;
%
%  The TITLE function will interpret underscores in the title.
%  We need to unescape such escape sequences!
%
  title_string = 'Sample points in triangle';
  title ( title_string )

  axis ( [ x_min, x_max, y_min, y_max ] );
  axis equal
%
%  Save data to file.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving data to file "triangle_samples.txt".\n' );

  save 'triangle_samples.txt' p -ASCII
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_05\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


% marshalling of parameters and available warping functions
function varargout = warpLog(y,hyp,varargin)
  varargout = cell(nargout, 1);  % allocate the right number of output arguments

  % return number of parameters
  m = 2;
  if nargin<1 && nargout>0, varargout{1} = m; return, end

  if nargin <= 2
      [varargout{:}] = g(y,hyp);
  elseif nargin == 3
      if strcmpi(varargin{1},'inv')
          [varargout{:}] = ig(y,hyp);
      elseif isnumeric(varargin{1})
          [varargout{:}] = g(y,hyp,varargin{1});
      end
  else
      error('Derivative of inverse not supported yet.');
  end

% Logarithmic warping function g(y) and log of the derivative log(g'(y))>0
% or derivatives of the latter w.r.t. ith hyperparameter
function [gy,lgpy] = g(y,hyp,i)

  mu = hyp(1);
  delta = exp(hyp(2));

  idx = y > mu;

  if nargin==2                                                 % function values
    gy = y;
    gy(idx) = delta*log((y(idx) - mu)/delta +1)+mu;

    lgpy = zeros(size(y));
    lgpy(idx) = log(delta) - log(y(idx)-mu+delta);
  else                                                          % derivatives

    if i == 1
        gy = zeros(size(y));
        gy(idx) = (y(idx) - mu)./(y(idx) - mu + delta);
        lgpy = zeros(size(y));
        lgpy(idx) = 1./(y(idx)-mu+delta);
    elseif i == 2
        gy = zeros(size(y));
        gy(idx) = delta*(log(1 + (y(idx) - mu)/delta)+ (mu - y(idx))./(y(idx) - mu + delta));
        lgpy = zeros(size(y));
        lgpy(idx) = (y(idx)-mu)./(y(idx)-mu+delta);
    end
  end

% invert g(y)
function [y,n,d] = ig(z,hyp)
  y = z;
  mu = hyp(1);
  delta = exp(hyp(2));
  idx = z > mu;
  y(idx) = delta*(exp((z(idx)-mu)./delta)-1) + mu;
  n = 0;
  d = 0;


function x = gen_form(L_p,x_s,A,b,K,M)
%GEN_FORM Transform a standard-form problem back to the general-form setting.
%
% x = gen_form(L_p,x_s,A,b,K,M)    (method 1)
% x = gen_form(L_p,x_s,x_0)        (method 2)
%
% Transforms the standard-form solution x_s back to the required
% solution to the general-form problem:
%    x = L_p*x_s + d ,
% where L_p and d depend on the method as follows:
%    method = 1: L_p = pseudoinverse of L, d  = K*(b - A*L_p*x_s)
%    method = 2: L_p = A-weighted pseudoinverse of L, d = x_0.
%
% Usually, the standard-form problem is generated by means of
% function std_form.
%
% Note that x_s may have more that one column.

% References: L. Elden, "Algorithms for regularization of ill-
% conditioned least-squares problems", BIT 17 (1977), 134-145.
% L. Elden, "A weighted pseudoinverse, generalized singular values,
% and constrained lest squares problems", BIT 22 (1982), 487-502.
% M. Hanke, "Regularization with differential operators.  An itera-
% tive approach", J. Numer. Funct. Anal. Optim. 13 (1992), 523-540.

% Per Christian Hansen, IMM, 06/12/93.

% Nargin determines which method.
if (nargin==6)
  [p,q] = size(x_s); Km = size(K,1);
  if (Km==0)
    x = L_p*x_s;
  else
    x = L_p*x_s + K*(M*(b*ones(1,q) - A*(L_p*x_s)));
  end
else
  x_0 = A; [p,q] = size(x_s);
  x = L_p*x_s + x_0*ones(1,q);
end


function pass = test_besselj(pref)

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
xr = 2 * rand(100, 1) - 1;

% Arbitrary value to use for nu in the tests.
% [TODO]:  Use non-integer nu once fractional powers are implemented.
nu = 2;

f = chebfun(@(x) exp(x), [-1 -0.5 0 0.5 1], pref);

h = besselj(nu, f);
pass(1) = norm(feval(h, xr) - besselj(nu, exp(xr)), inf) < ...
    10*eps*vscale(h);

h2 = besselj(nu, f, 0);
pass(2) = normest(h - h2) < ...
    10*max(eps*vscale(h), eps*vscale(h2));

h = besselj(nu, f, 1);
pass(3) = norm(feval(h, xr) - besselj(nu, exp(xr), 1), inf) < ...
    10*eps*vscale(h);

% Test for array-valued chebfun.
f_op = @(x) [exp(-x) 1./(1 + 25*(x - 0.1).^2)];
f = chebfun(f_op, [-1 -0.5 0 0.5 1], pref);

h = besselj(nu, f);
err = feval(h, xr) - besselj(nu, f_op(xr));
pass(4) = norm(err(:), inf) < 1e2*eps*vscale(h);

h2 = besselj(nu, f, 0);
pass(5) = normest(h - h2) < ...
    10*max(eps*vscale(h), eps*vscale(h2));

h = besselj(nu, f, 1);
err = feval(h, xr) - besselj(nu, f_op(xr), 1);
pass(6) = norm(err(:), inf) < 1e2*eps*vscale(h);

%% Test for complex values.
pref.splitting = 1;
f_op = @complex_test_fn;
f = chebfun(f_op, [-1 0 0.5 1], pref);

h = besselj(nu, f, 0, pref);
pass(7) = norm(feval(h, xr) - besselj(nu, f_op(xr), 0), inf) < ...
    1e2*eps*vscale(h);

% Check for error on nu.
try
    f = chebfun(@(x) x, pref);
    h = besselj(1i + 3, f);
    pass(8) = false;
catch ME
    pass(8) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:besselj:nu');
end

%% Test support for vector nu.
nu = [1 2 3];
x = chebfun('x');
f = besselj(nu, x);
g = 0*x;
for k = 1:3
    g(:,k) = besselj(nu(k), x);
end
pass(9) = norm(f - g) < 1e2*eps*vscale(g);

end

function y = complex_test_fn(x)
    y = zeros(size(x));
    y(x <= 0) = exp(4*pi*1i*(x(x <= 0)));
    y(x > 0) = exp(x(x > 0));
end


function [varargout] = p2pSimpleICP(Md, MovData, RefData, Tf0, DistThr )
if nargin == 0
    clc; close all;
    A = pcread('teapot.ply');  %pcread('E:\Dataset\Point Cloud\ply\Bunny\reconstruction\bun_zipper.ply');
    A = pcdownsample(A, 'gridAverage', 0.10 );
    MovData = double(A.Location');
    R0 = eul2rotm( deg2rad([20.0 10.0 10.0]) );
    T0 = [5 5 10.00 ]';
    RefData = Loc2Glo( MovData, R0', T0 );
    Sigma = 0.01;
    rng(100);
    MovData = MovData + Sigma * randn(3,  size(MovData, 2) );
    Md = createns(RefData');
    Tf0 = [ eye(3) mean(RefData-MovData, 2); 0 0 0 1];
    DistThr = Inf;
    [NNIdx, DD] = knnsearch( RefData', RefData', 'k', 2);
    Res = mean(DD(:, 2));
    DistThr = Inf;
    AngThr = cosd(45);
end
bVerbose = false; % true;
nDim = size(RefData, 1);
MaxIter = 30;
R = Tf0(1:nDim, 1:nDim);
T = Tf0(1:nDim, end);
for Iter = 1 : 1 : MaxIter
    %%%%%%%%%%% establish correspondence.
    AftData = Loc2Glo( MovData, R', T );
    [NNIdx, DD] = knnsearch( Md, AftData' );
    idx = find( DD' < DistThr );
    MovIdx = idx;
    RefIdx = NNIdx(idx);
    %%%%%%%%%%% obtain rotation and translation via SVD.
    [ dR, dT ] = RegFun(RefData(:, RefIdx), AftData(:, MovIdx) );
    R = dR * R;
    T = dR * T + dT;
    %%%%%%%%%%% check convergence condition.
    Err = max( norm(dR - eye(nDim)), norm(dT) );  % CalRT_Diff(TotalTf(end).Tf, TotalTf(end-1).Tf );
%     str = sprintf( 'Iter = %02d, Err = %f\n', Iter, Err );
%     disp(str);
    if Err(1) <= 1e-4
        break;
    end
end
if nargout == 1
    varargout{1} = [R T];
end
if nargout == 2
    varargout{1} = R;
    varargout{2} = T;
end
IS_SHOW = 0;
if IS_SHOW
    figure;
    hold on;
    grid on;
    axis equal;
    AftData = Loc2Glo( MovData, R', T );
    if nDim == 3
        view(3);
        showPointCloud(RefData', 'g');
        showPointCloud(MovData', 'r');
        showPointCloud(AftData', 'b');
    else
        plot(RefData(1, :), RefData(2, :), 'g.');
        plot(MovData(1, :), MovData(2, :), 'r.');
        plot(AftData(1, :), AftData(2, :), 'bo', 'markersize', 3);
    end

    if nargin == 0
        ErrR = RotationDiff( R, R0 );
        ErrT = norm(T - T0 );
        title(sprintf( 'Point-to-Point ICP, ErrR = %.4fdegs ErrT = %.4f', rad2deg(ErrR), ErrT ));
    else
        title('Point-to-Point ICP');
    end
end
bTest = 1;
end


% test for local fourier transform

n = 128;
name = 'barb';
M = crop(load_image(name),n);

w = 15; q = 7;
options.window_type = 'sin';
MF = perform_windowed_fourier_transform(M,w,q,n, options);
M1 = perform_windowed_fourier_transform(MF,w,q,n, options);

e = sum(M(:).^2);
ef = sum(abs(MF(:)).^2);
disp( ['Energy conservation error (should be 0) : ' num2str( (e-ef)/e*100 ) '%' ]);


function clusterLabels = gacMerging(graphW, initClusters, groupNumber, strDescr, z)
%% Cluster merging for Graph Agglomerative Clustering
% Implements an agglomerative clustering algorithm based on maiximum graph
%   strcutural affinity of two groups
% Inputs:
%	- graphW: asymmetric weighted adjacency matrix
%   - initClusters: a cell array of clustered vertices
%   - groupNumber: the final number of clusters
%   - strDescr: structural descriptor, 'zeta' or 'path'
%   - z: (I - z*P), default: 0.01
% Outputs:
%   - clusterLabels: 1 x m list whose i-th entry is the group assignment of
%                   the i-th data vector w_i. Groups are indexed
%                   sequentially, starting from 1.
% by Wei Zhang (wzhang009 at gmail.com), June, 8, 2011

%%
numSample = size(graphW,1);
IminuszW = eye(numSample) - z*graphW;
clear graphW
myInf = 1e10;

%% initialization
VERBOSE = true;
switch lower(strDescr)
    case 'zeta'
        complexity_fun = @gacZetaEntropy;
        conditionalComplexity_fun = @gacZetaCondEntropy;
    case 'path'
        complexity_fun = @gacPathEntropy;
        conditionalComplexity_fun = @gacPathCondEntropy;
    otherwise
        error('GAC: Descriptor type is not supported!');
end

numClusters = length(initClusters);
if numClusters <= groupNumber
    error('GAC: too few initial clusters. Do not need merging!');
end

%% compute the structural complexity of each initial cluster
clusterComp = zeros(numClusters,1);
for i = 1 : numClusters
    clusterComp(i) = complexity_fun(IminuszW(initClusters{i}, initClusters{i}));
end

%% compute initial (negative) affinity table (upper trianglar matrix), very slow
if VERBOSE
    disp('   Computing initial table.' );
end
affinityTab = Inf(numClusters);
for j = 1 : numClusters
    for i = 1 : j-1
        affinityTab(i, j) = - conditionalComplexity_fun(IminuszW, initClusters{i}, initClusters{j});
    end
end
affinityTab = bsxfun(@plus, clusterComp, clusterComp') + affinityTab;

if VERBOSE
    disp('   Starting merging process');
end

curGroupNum = numClusters;
while true
    if mod( curGroupNum, 20 ) == 0 && VERBOSE
        disp(['   Group count: ' num2str(curGroupNum)]);
    end
    % Find two clusters with the best affinity
    [minAff, minIndex1] = min(affinityTab(1:curGroupNum, 1:curGroupNum), [], 1);
    [~, minIndex2] = min(minAff);
    minIndex1 = minIndex1(minIndex2);
    if minIndex2 < minIndex1,  [minIndex1, minIndex2] = swap(minIndex1, minIndex2); end

    % merge the two clusters
    new_cluster = unique([initClusters{minIndex1}; initClusters{minIndex2}]);
    % move the second cluster to be merged to the end of the cluster array
    % note that we only need to copy the end cluster's information to
    % the second cluster's position
    if (minIndex2 ~= curGroupNum)
        initClusters{minIndex2} = initClusters{end};
        clusterComp(minIndex2) = clusterComp(curGroupNum);
        % affinityTab is an upper triangular matrix
        affinityTab(1:minIndex2-1, minIndex2) = affinityTab(1:minIndex2-1, curGroupNum);
        affinityTab(minIndex2, minIndex2+1:curGroupNum-1) = affinityTab(minIndex2+1:curGroupNum-1, curGroupNum);
    end

    % update the first cluster and remove the second cluster
    initClusters{minIndex1} = new_cluster;
    initClusters(end) = [];
    clusterComp(minIndex1) = complexity_fun(IminuszW(new_cluster, new_cluster));
    clusterComp(curGroupNum) = myInf;
    affinityTab(:,curGroupNum) = myInf;
    affinityTab(curGroupNum,:) = myInf;
    curGroupNum = curGroupNum - 1;
    if curGroupNum <= groupNumber
        break;
    end

    % update the affinity table for the merged cluster
    for groupIndex1 = 1:minIndex1-1
        affinityTab(groupIndex1, minIndex1) = - conditionalComplexity_fun(IminuszW, initClusters{groupIndex1}, new_cluster);
    end
    for groupIndex1 = minIndex1+1:curGroupNum
        affinityTab(minIndex1, groupIndex1) = - conditionalComplexity_fun(IminuszW, initClusters{groupIndex1}, new_cluster);
    end
    affinityTab(1:minIndex1-1, minIndex1) = clusterComp(1:minIndex1-1) + clusterComp(minIndex1) + affinityTab(1:minIndex1-1, minIndex1);
    affinityTab(minIndex1, minIndex1+1:curGroupNum) = clusterComp(minIndex1+1:curGroupNum)' + clusterComp(minIndex1) + affinityTab(minIndex1, minIndex1+1:curGroupNum);
end

%% generate sample labels
clusterLabels = ones(numSample,1);
for i = 1:length(initClusters)
    clusterLabels(initClusters{i}) = i;
end
if VERBOSE
    disp(['   Final group count: ' num2str(curGroupNum)]);
end

end

function [y, x] = swap (x, y)
end


%%******************************************************************
%% geometric_mean: an example with rotated cone variables
%%
%%  max {prod(d+B*x) : d+B*x > 0, x <= 10}
%%
%%  where B = 4xn matrix,
%%        d = 4x1 vector
%%
%% [blk,At,C,b,xx] = geometric_mean(B,d,solve);
%%
%% E.g. p = 6; m = 4; B = [rand(2,p); -rand(2,p)]; d = rand(m,1);
%%
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [blk,At,C,b,xx] = geometric_mean(B,d,solve);

   if (nargin == 2); solve = 0; end

   n = size(B,2);
   zz = zeros(1,n);
   r2 = sqrt(2);
   blk{1,1} = 'r'; blk{1,2} = [3,3,3];
   blk{2,1} = 'l'; blk{2,2} = [2];
   At{1,1} = -[B(1,:),0,0,0; B(2,:),0,0,0; zz,r2,0,0; ...
               B(3,:),0,0,0; B(4,:),0,0,0; zz,0,r2,0; ...
	       zz, 1,0,0; zz, 0,1,0; zz, 0,0,r2];
   At{2,1} = -[B(1,:),0,0,0; B(3,:),0,0,0];
   C{1,1} = [d(1:2); 0; d(3:4); 0; 0;0;0];
   C{2,1} = [d(1); d(3)];
   b = [zeros(n,1); 0;0;1];
   blk{3,1} = 'l'; blk{3,2} = n;
   At{3,1} = [eye(n), zeros(n,3)]; C{3,1} = 10*ones(n,1);
   if (solve)
      [bblk,AAt,CC,bb,T] = convertRcone(blk,At,C,b);
      [obj,X,y,Z] = sqlp(bblk,AAt,CC,bb);
      xx = y(1:n);
   end
%%******************************************************************


function [thetamat, dthetamat] ...
         = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ...
                                     Ftipmat, Mlist, Glist, Slist, dt, ...
                                     intRes)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint rates,
%       taumat: An N x n matrix of joint forces/torques, where each row is
%               the joint effort at any time step,
%       g: Gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame, in the format
%              of a matrix with the screw axes as the columns,
%       dt: The timestep between consecutive joint forces/torques,
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an
%               integer value greater than or equal to 1.
% Returns thetamat: The N x n matrix of robot joint angles resulting from
%                   the specified joint forces/torques,
%         dthetamat: The N x n matrix of robot joint velocities.
% This function simulates the motion of a serial chain given an open-loop
% history of joint forces/torques. It calls a numerical integration
% procedure that uses ForwardDynamics.
% Example Inputs (3 Link Robot):
%
% clc; clear;
% thetalist = [0.1; 0.1; 0.1];
% dthetalist = [0.1; 0.2; 0.3];
% taumat = [[3.63, -6.58, -5.57]; [3.74, -5.55, -5.5]; ...
%         [4.31, -0.68, -5.19]; [5.18, 5.63, -4.31]; ...
%         [5.85, 8.17, -2.59]; [5.78, 2.79, -1.7]; ...
%         [4.99, -5.3, -1.19]; [4.08, -9.41, 0.07]; ...
%         [3.56, -10.1, 0.97]; [3.49, -9.41, 1.23]];
% %Initialise robot description (Example with 3 links)
% g = [0; 0; -9.8];
% Ftipmat = ones(size(taumat, 1), 6);
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
% Glist = cat(3, G1, G2, G3);
% Mlist = cat(3, M01, M12, M23, M34);
% Slist = [[1; 0; 1;      0; 1;     0], ...
%        [0; 1; 0; -0.089; 0;     0], ...
%        [0; 1; 0; -0.089; 0; 0.425]];
% dt = 0.1;
% intRes = 8;
% [thetamat, dthetamat] ...
% = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ...
%                           Ftipmat, Mlist, Glist, Slist, dt, intRes);
% %Output using matplotlib to plot the joint forces/torques
% Tf = size(taumat, 1);
% time=0: (Tf / size(thetamat, 1)): (Tf - (Tf / size(thetamat, 1)));
% plot(time,thetamat(:, 1),'b')
% hold on
% plot(time,thetamat(:, 2), 'g')
% plot(time,thetamat(:, 3), 'r')
% plot(time,dthetamat(:, 1), 'c')
% plot(time,dthetamat(:, 2), 'm')
% plot(time,dthetamat(:, 3), 'y')
% title('Plot of Joint Angles and Joint Velocities')
% xlabel('Time')
% ylabel('Joint Angles/Velocities')
% legend('Theta1', 'Theta2', 'Theta3', 'DTheta1', 'DTheta2', 'DTheta3')
%

taumat = taumat';
Ftipmat = Ftipmat';
thetamat = taumat;
thetamat(:, 1) = thetalist;
dthetamat = taumat;
dthetamat(:, 1) = dthetalist;
for i = 1: size(taumat, 2) - 1
    for j = 1: intRes
       ddthetalist ...
       = ForwardDynamics(thetalist, dthetalist, taumat(:,i), g, ...
                         Ftipmat(:, i), Mlist, Glist, Slist);
       [thetalist, dthetalist] = EulerStep(thetalist, dthetalist, ...
                                           ddthetalist, dt / intRes);
    end
    thetamat(:, i + 1) = thetalist;
    dthetamat(:, i + 1) = dthetalist;
end
thetamat = thetamat';
dthetamat = dthetamat';
end


function [mm]=misssum(X,def)
%MISSSUM sum of a matrix X with NaN's
%
%[mm]=misssum(X,def)
%
%This function calculates the sum of a matrix X.
%X may hold missing elements denoted by NaN's which
%are ignored.
%
%The result is standardized, that is, corrected for the lower
%number of contributing terms.
%
%Check that for no column of X, all values are missing

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

%Insert zeros for missing, correct afterwards
missidx = isnan(X);
i = find(missidx);
if ~isempty(i),
    X(i) = zeros(size(i));
end;

%Find the number of real(non-missing objects)
if min(size(X))==1,
   n_real=length(X)-sum(missidx);
   weight=length(X);
else
   n_real=size(X,1)-sum(missidx);
   weight=size(X,1);
end

i=find(n_real==0);
if isempty(i) %All values are real and can be corrected
   mm=weight*sum(X)./n_real;
else %There are columns with all missing, insert missing
   n_real(i)=1;
   mm=weight*sum(X)./n_real;
   mm(i)=i + NaN;
end


function FMatrix=kannumfcc(num,s,Fs)
%Author:        Olutope Foluso Omogbenigun
%Email:         olutopeomogbenigun at hotmail.com
%University:    London Metropolitan University
%Date:          11/09/07
%Syntax:        M=mfccf(num,s, Fs);
%Computes and returns the mfcc coefficients for a speech signal s
%where num is the required number of MFCC coefficients. It utilises the
%function 'melbankm' from the toolbox Voicebox by Mike Brooks copyright(c)
%1997 (GNU General Public License), freely available on the internet,
%to implement the triangular mel filter bank

n=512;              %Number of FFT points
Tf=0.025;           %Frame duration in seconds
N=Fs*Tf;            %Number of samples per frame
fn=24;              %Number of mel filters
l=length(s);        %total number of samples in speech
Ts=0.01;            %Frame step in seconds
FrameStep=Fs*Ts;    %Frame step in samples
a=1;
b=[1, -0.97];       %a and b are high pass filter coefficients

noFrames=floor(l/FrameStep);    %Maximum no of frames in speech sample
FMatrix=zeros(noFrames-2, num); %Matrix to hold cepstral coefficients
lifter=1:num;                   %Lifter vector index
lifter=1+floor((num)/2)*(sin(lifter*pi/num));%raised sine lifter version

if mean(abs(s)) > 0.01
    s=s/max(s);                     %Normalises to compensate for mic vol differences
end

%Segment the signal into overlapping frames and compute MFCC coefficients
for i=1:noFrames-2
    frame=s((i-1)*FrameStep+1:(i-1)*FrameStep+N);  %Holds individual frames
    Ce1=sum(frame.^2);          %Frame energy
    Ce2=max(Ce1,2e-22);         %floors to 2 X 10 raised to power -22
    Ce=log(Ce2);
    framef=filter(b,a,frame);   %High pass pre-emphasis filter
    F=framef.*hamming(N);       %multiplies each frame with hamming window
    FFTo=fft(F,N);              %computes the fft
    melf=melbankm(fn,n,Fs);     %creates 24 filter, mel filter bank
    halfn=1+floor(n/2);
    spectr1=log10(melf*abs(FFTo(1:halfn)).^2);%result is mel-scale filtered
    spectr=max(spectr1(:),1e-22);
    c=dct(spectr);              %obtains DCT, changes to cepstral domain
    c(1)=Ce;                    %replaces first coefficient
    coeffs=c(1:num);            %retains first num coefficients
    ncoeffs=coeffs.*lifter';    %Multiplies coefficients by lifter value
    FMatrix(i, :)=ncoeffs';     %assigns mfcc coeffs to succesive rows i
end


% LFMapRectifiedToMeasured - Applies a calibrated camera model to map desired samples to measured samples
%
% Usage:
%
%     InterpIdx = LFMapRectifiedToMeasured( InterpIdx, CalInfo, RectOptions )
%
% Helper function used by LFCalRectifyLF.  Based on a calibrated camera model, including distortion parameters and a
% desired intrinsic matrix, the indices of a set of desired sample is mapped to the indices of corresponding measured
% samples.
%
% Inputs :
%
%     InterpIdx : Set of desired indices, in homogeneous coordinates
%     CalInfo : Calibration info as returned by LFFindCalInfo
%     RectOptions : struct controlling the rectification process, see LFCalRectify
%
% Outputs:
%
%      InterpIdx : continuous-domain indices for interpolating from the measured light field
%
% User guide: LFToolbox.pdf
% See also: LFCalRectifyLF

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

function InterpIdx = LFMapRectifiedToMeasured( InterpIdx, CalInfo, RectOptions )

RectOptions = LFDefaultField( 'RectOptions', 'Precision', 'single' );

%---Cast the to the required precision---
InterpIdx = cast(InterpIdx, RectOptions.Precision);

%---Convert the index of the desired ray to a ray representation using ideal intrinsics---
InterpIdx = RectOptions.RectCamIntrinsicsH * InterpIdx;

%---Apply inverse lens distortion to yield the undistorted ray---
k1 = CalInfo.EstCamDistortionV(1); % r^2
k2 = CalInfo.EstCamDistortionV(2); % r^4
k3 = CalInfo.EstCamDistortionV(3); % r^6
b1 = CalInfo.EstCamDistortionV(4); % decentering of lens distortion
b2 = CalInfo.EstCamDistortionV(5); % decentering of lens distortion

InterpIdx(3:4,:) = bsxfun(@minus, InterpIdx(3:4,:), [b1; b2]); % decentering of lens distortion

%---Iteratively estimate the undistorted direction----
DesiredDirection = InterpIdx(3:4,:);
for( InverseIters = 1:RectOptions.NInverse_Distortion_Iters )
    R2 = sum(InterpIdx(3:4,:).^2); % compute radius^2 for the current estimate
    % update estimate based on inverse of distortion model
    InterpIdx(3:4,:) = DesiredDirection ./ repmat((1 + k1.*R2 + k2.*R2.^2 + k3.*R2.^3),2,1);
end
clear R2 DesiredDirection

InterpIdx(3:4,:) = bsxfun(@plus, InterpIdx(3:4,:), [b1; b2]); % decentering of lens distortion

%---Convert the undistorted ray to the corresponding index using the calibrated intrinsics---
% todo[optimization]: The variable InterpIdx could be precomputed and saved with the calibration
InterpIdx = CalInfo.EstCamIntrinsicsH^-1 * InterpIdx;

%---Interpolate the required values---
InterpIdx = InterpIdx(1:4,:); % drop homogeneous coordinates


function [wz,AA,RA] = Interaction(PopObj,Point)
% Identify the preferred point and all the others are treated as
% non-preferred points

%------------------------------- 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".
%--------------------------------------------------------------------------

    %% Identify the unique and non-dominated solutions
    PopObj = unique(PopObj,'rows');
    PopObj = PopObj(NDSort(PopObj,1)==1,:);

    %% Identify the preferred solution
    ideal = min(PopObj,[],1);
    % Calculate the Tchebycheff function value of each solution on Point
    Fitness = max((PopObj-repmat(ideal,size(PopObj,1),1))./repmat(Point,size(PopObj,1),1),[],2);
    % The one having the minimal function value is treated as the preferred
    % point
    [~,prefer] = min(Fitness);
    AA = PopObj(prefer,:);
    RA = PopObj([1:prefer-1,prefer+1:end],:);

    %% Calculate the weight distribution function value
    RefPoint = max(PopObj,[],1) + 0.1;
    wz = [0,1,1+CalWHV(AA,RefPoint,ones(1,size(AA,1)))/CalWHV(RA,RefPoint,ones(1,size(RA,1)))];
end


function [knn_dist, nn_dist] = smooth_knn_dist(distances, k, varargin)
%SMOOTH_KNN_DIST Compute a continuous version of the distance to the kth
% nearest neighbor. That is, this is similar to knn-distance but allows
% continuous k values rather than requiring an integral k. In esscence we
% are simply computing the distance such that the cardinality of fuzzy set
% we generate is k.
%
% [knn_dist, nn_dist] = SMOOTH_KNN_DIST(distances, k, local_connectivity,
% n_iter, bandwidth)
%
% Parameters
% ----------
% distances: array of size (n_samples, n_neighbors)
%     Distances to nearest neighbors for each samples. Each row should be a
%     sorted list of distances to a given sample's nearest neighbors.
%
% k: double
%     The number of nearest neighbors to approximate for.
%
% n_iter: double (optional, default 64)
%     We need to binary search for the correct distance value. This is the
%     max number of iterations to use in such a search.
%
% local_connectivity: double (optional, default 1)
%     The local connectivity required -- i.e. the number of nearest
%     neighbors that should be assumed to be connected at a local level.
%     The higher this value the more connected the manifold becomes
%     locally. In practice this should be not more than the local intrinsic
%     dimension of the manifold.
%
% bandwidth: double (optional, default 1)
%     The target bandwidth of the kernel, larger values will produce
%     larger return values.
%
% Returns
% -------
% knn_dist: array of size (n_samples, 1)
%     The distance to kth nearest neighbor, as suitably approximated.
%
% nn_dist: array of size (n_samples, 1)
%     The distance to the 1st nearest neighbor for each point.
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
    p=parseArguments();
    parse(p,varargin{:});
    args=p.Results;
    local_connectivity = args.local_connectivity;
    n_iter = args.n_iter;
    bandwidth = args.bandwidth;
    same_set = args.same_set;

    SMOOTH_K_TOLERANCE = 1e-5;
    MIN_K_DIST_SCALE = 1e-3;

    height = size(distances,1);

    target = log2(k)*bandwidth;

    lo = zeros(height,1);
    hi = Inf(height,1);
    mid = ones(height,1);

    zero_dists = sum(distances == 0, 2);

    if any(zero_dists==size(distances,2))
        warning('There are at least n_neighbors identical data points in the raw data. Results may be inaccurate.');
    end

    index = floor(local_connectivity);
    interpolation = local_connectivity - index;
    aug_dists = [lo distances repmat(max(distances, [], 2), [1 index+1])];

    idx = sub2ind(size(aug_dists), (1:height)', zero_dists + index + 1);
    rho = aug_dists(idx) + interpolation*(aug_dists(idx) - aug_dists(idx+height));

    if same_set
        d = distances(:,2:end) - rho;
    else
        d = distances(:,1:end) - rho;
    end

    for n = 1:n_iter

        summands = exp(-max(0, d./repmat(mid, [1 size(d, 2)])));

        psum = sum(summands, 2);

        if all(abs(psum - target) < SMOOTH_K_TOLERANCE)
            break
        end

        b = ~(psum > target).*hi;
        b(isnan(b)) = 0;

        hi = (psum > target).*mid + b;
        lo = (psum > target).*lo + ~(psum > target).*mid;

        c = (psum > target).*((lo + hi)/2);
        c(isnan(c)) = 0;

        mid = c + ~(psum > target).*min(2*lo, (lo + hi)/2);

    end

    result = mid;

    result = max(result, MIN_K_DIST_SCALE * ((rho > 0).*mean(distances,2) + (rho == 0).*mean(mean(distances))));

    knn_dist = result;
    nn_dist = rho;
end

    function p=parseArguments(varargin)
        p = inputParser;
        addParameter(p,'local_connectivity', 1);
        addParameter(p,'n_iter', 64);
        addParameter(p,'bandwidth', 1);
        addParameter(p,'same_set','true');
    end


classdef SamplePointsCreatorFromMxMyForOptimalExponentComputer ...
        < SamplePointsCreatorForOptimalExponentComputer

    properties (Access = private)
        mxMax
        myMax
        mxMin
        myMin
        mxV
        myV
        superellipse
    end

    methods (Access = public)

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

    end

    methods (Access = private)

        function init(obj,cParams)
            nMx  = cParams.nMx;
            nMy  = cParams.nMy;
            nPhi = cParams.nPhi;
            phiMin = cParams.phiMin;
            phiMax = cParams.phiMax;
            obj.mxMax = 0.99;
            obj.myMax = 0.99;
            obj.mxMin = 0.01;
            obj.myMin = 0.01;
            obj.mxV = linspace(obj.mxMin,obj.mxMax,nMx);
            obj.myV = linspace(obj.myMin,obj.myMax,nMy);
            obj.phiV = linspace(phiMin,phiMax,nPhi);
            obj.superellipse = SuperEllipseParamsRelator();
        end

    end

    methods (Access = protected)

        function computeRhoTxiValues(obj)
            nmx = length(obj.mxV);
            nmy = length(obj.myV);
            q = 10^6;
            for imx = 1:nmx
                for imy = 1:nmy
                    mx = obj.mxV(imx);
                    my = obj.myV(imy);
                    index = nmx*(imy - 1) + imx;
                    obj.rhoV(index) = obj.superellipse.rho(mx,my,q);
                    obj.txiV(index) = obj.superellipse.txi(mx,my);
                end
            end
        end

    end

end


function overlaps = bbox_overlap(boxes1,boxes2)
% Copyright (C) 2016 Hakan Bilen.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).
x11 = boxes1(:,1);
y11 = boxes1(:,2);
x12 = boxes1(:,3);
y12 = boxes1(:,4);

x21 = boxes2(:,1);
y21 = boxes2(:,2);
x22 = boxes2(:,3);
y22 = boxes2(:,4);

N1 = size(boxes1,1);
N2 = size(boxes2,1);

area1 = (x12-x11+1) .* (y12-y11+1);
area2 = (x22-x21+1) .* (y22-y21+1);

overlaps = zeros(N1,N2);

for i=1:N1

  xx1 = max(x11(i), x21);
  yy1 = max(y11(i), y21);
  xx2 = min(x12(i), x22);
  yy2 = min(y12(i), y22);

  w = max(0.0, xx2-xx1+1);
  h = max(0.0, yy2-yy1+1);

  inter = w.*h;
  overlaps(i,:) = inter ./ (area1(i) + area2 - inter);
end


function M = lbs_matrix(V,W)
  % LBS_MATRIX  construct a matrix that when multiplied against a column of
  % affine transformation entries computes new coordinates of the vertices
  %
  % M = lbs_matrix(V,W)
  %
  % Input:
  %   V  #V by dim list of vertex rest positions
  %   W  #W by #handles list of correspondence weights
  % Output:
  %   M  #V * dim by #handles * dim * (dim+1) matrix such that
  %     new_V(:) = LBS(V,W,A) = reshape(M * A,size(V)), where A is a column
  %     vectors formed by the entries in each handle's dim by dim+1
  %     transformation matrix. Specifcally, A =
  %       reshape(permute(Astack,[3 1 2]),n*dim*(dim+1),1)
  %     or A = [Lxx;Lyx;Lxy;Lyy;tx;ty], and likewise for other dim
  %     if Astack(:,:,i) is the dim by (dim+1) transformation at handle i
  %
  % Example:
  %  MLBS = lbs_matrix(V,W);
  %  n = size(V,1);
  %  dim = size(V,2);
  %  m = size(W,2);
  %  % stack of identity transformations
  %  Astack = repmat([eye(dim,dim) zeros(dim,1)],[1 1 m]);
  %  % collect transformations into column
  %  A = reshape(permute(Astack,[3 1 2]),m*dim*(dim+1),1);
  %  % apply transformations
  %  new_V = MLBS*A;
  %  new_V = reshape(new_V,[n dim]);
  %
  %  % Alternative:
  %  % Q is #W by 4 list of quats, T is #W by 3 list of translations
  %  % A is #W*4 by 3 stack of transposed affine transformations
  %  A = reshape(cat(2,permute(quat2mat(Q),[2 1 3]),permute(T,[2 3 1])),3,[])';
  %  % M is #V by #W*4 skinning matrix
  %  % M = (𝟙ᵀ⊗ [V 𝟙]) ⊙ (W ⊗ 𝟙ᵀ)
  %  %M = kron(ones(1,size(W,2)),[V ones(size(V,1),1)]).* ...
  %  %    kron(W,ones(1,size(V,2)+1));
  %  M = reshape([V ones(size(V,1),1)].*permute(W,[1 3 2]),size(V,1),[]);
  %  U = M*A;
  %

  % number of mesh (domain) vertices
  n = size(V,1);
  assert(n == size(W,1));
  % dimension of mesh
  dim = size(V,2);
  % number of handles
  m = size(W,2);

  % M = zeros(V*dim,m*dim*(dim+1));

  % repeat vertex positions so that VV(:,:,i) gives #V by #handles matrix where
  % each column is ith coordinates of vertices
  VV = permute(repmat(V,[1 1 m]),[1 3 2]);
  % multiply each column in VV by respective weights for that handle
  VVW = VV.*repmat(W,[1 1 dim]);
  % matrix of zeros
  Z = zeros(n,m);
  switch dim
  case 2
    M = [ ...
      VVW(:,:,1) Z          VVW(:,:,2) Z          W Z; ...
      Z          VVW(:,:,1) Z          VVW(:,:,2) Z W];
  case 3
    M = [ ...
      VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z          Z          W Z Z; ...
      Z          VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z          Z W Z; ...
      Z          Z          VVW(:,:,1) Z          Z          VVW(:,:,2) Z          Z          VVW(:,:,3) Z Z W];
  otherwise
    error('Only dim=2 or dim=3 supported');
  end

end


function [dx] = spm_sde_dx(dfdx,dfdw,f,t)
% returns dx(t) = (expm(dfdx*t) - I)*inv(dfdx)*f + w for SDEs
% FORMAT [dx] = spm_sde_dx(dfdx,dfdw,f,t)
% dfdx   = df/dx - x: states
% dfdw   = df/dw - w: i.i.d. Weiner process
% f      = dx/dt
% t      = integration time: (default t = 1);
%
% dx     = x(t) - x(0)
%--------------------------------------------------------------------------
% Integration of stochastic differential equations using local linearization.
% This scheme accommodates nonlinearities in the state equation by using a
% functional of f(x) = dx/dt.  This uses the equality
%
%             expm([0    0]*t) = expm(dfdx*t) - I)*inv(dfdx)*f
%                  [f dfdx]
%
% When t -> Inf this reduces to
%
%              dx(t) = -inv(dfdx)*f
%
% for the SDE:  dx = dfdx*x*dt + sqrt(2)*dfdw*dw
%
% where w is a standard Wiener process. Unstable modes are removed using
% the systems eigenmodes.
%
% see also spm_dx
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_sde_dx.m 5932 2014-03-28 10:04:32Z karl $

% defaults
%--------------------------------------------------------------------------
if nargin < 3, t = 1; end

% compute stochastic terms {E = exp(dfdx*t), e = exp(dfdx*dt)}
%--------------------------------------------------------------------------
dfdx  = full(dfdx);
m     = length(dfdx);
N     = 256;
dt    = t/N;

% condition unstable eigenmodes
%--------------------------------------------------------------------------
[v,s] = eig(full(dfdx),'nobalance');
s     = diag(s);
u     = pinv(v);
s     = 1j*imag(s) + min(real(s),-4);
eJdt  = real(v*diag(exp(s*dt))*u);
dfdx  = real(v*diag(s)*u);

% flow operators
%--------------------------------------------------------------------------
eJt   = eJdt;
Q     = sparse(m,m);
R     = dfdw*dfdw'*2;
dQ    = eJt*R*eJt';
TOL   = trace(dQ)/64;
for i = 1:N

    % integrate and update exp(dfdx*t)
    %----------------------------------------------------------------------
    Q   = Q + dQ*dt;
    eJt = eJt*eJdt;
    dQ  = eJt*R*eJt';

    % convergence
    %----------------------------------------------------------------------
    if trace(dQ) < TOL, break, end

end

% scaled [Wiener] innovation
%--------------------------------------------------------------------------
w     = spm_sqrtm(Q)*randn(m,1);

% local linear solution plus stochastic term
%==========================================================================
dx    = spm_dx(dfdx,f,t) + w;


function [ radius ] = atomic_radius( Z )
%ATOMIC_RADIUS Get atomic radius of element.
%   r = ATOMIC_RADIUS(Z) returns the atomic radius of element number Z in
%   Angstroms, as given in J. Chem. Phys. 47, 1300 (1967) doi:
%   10.1063/1.1712084. Z can also be a string the the element symbol. For
%   unrecognized symbols/element numbers, a a value of 1.0 is returned.
%
%   See also CHEMSYM2NUMBER, NUMBER2CHEMSYM

    if ischar(Z)
        Z = chemsym2number(Z);
    end

    default_r = 1.0;

    r = [ ...
     0.53 ... H
     0.31 ... He
     1.67 ... Li
     1.12 ... Be
     0.87 ... B
     0.67 ... C
     0.56 ... N
     0.48 ... O
     0.42 ... F
     0.38 ... Ne
     1.90 ... Na
     1.45 ... Mg
     1.18 ... Al
     1.11 ... Si
     0.98 ... P
     0.88 ... S
     0.79 ... Cl
     0.71 ... Ar
     2.43 ... K
     1.94 ... Ca
     1.84 ... Sc
     1.76 ... Ti
     1.71 ... V
     1.66 ... Cr
     1.61 ... Mn
     1.56 ... Fe
     1.52 ... Co
     1.49 ... Ni
     1.45 ... Cu
     1.42 ... Zn
     1.36 ... Ga
     1.25 ... Ge
     1.14 ... As
     1.03 ... Se
     0.94 ... Br
     0.88 ... Kr
     2.65 ... Rb
     2.19 ... Sr
     2.12 ... Y
     2.06 ... Zr
     1.98 ... Nb
     1.90 ... Mo
     1.83 ... Tc
     1.78 ... Ru
     1.73 ... Rh
     1.69 ... Pd
     1.65 ... Ag
     1.61 ... Cd
     1.56 ... In
     1.45 ... Sn
     1.33 ... Sb
     1.23 ... Te
     1.15 ... I
     1.08 ... Xe
     2.98 ... Cs
     2.53 ... Ba
     default_r ... La (missing value)
     default_r ... Ce (missing value)
     2.47 ... Pr
     2.06 ... Nd
     2.05 ... Pm
     2.38 ... Sm
     2.31 ... Eu
     2.33 ... Gd
     2.25 ... Tb
     2.28 ... Dy
     2.26 ... Ho
     2.26 ... Er
     2.22 ... Tm
     2.22 ... Yb
     2.17 ... Lu
%{
     ... Hf
      ... Ta
      ... W
      ... Re
      ... Os
      ... Ir
      ... Pt
      ... Au
      ... Hg
      ... Tl
      ... Pb
      ... Bi
      ... Po
      ... At
      ... Rn
      ... Fr
      ... Ra
      ... Ac
      ... Th
      ... Pa
      ... U
      ... Np
      ... Pu
      ... Am
      ... Cm
      ... Bk
      ... Cf
      ... Es
      ... Fm
      ... Md
      ... No
      ... Lr
      ... Rf
      ... Db
      ... Sg
      ... Bh
      ... Hs
      ... Mt
%}
     ];
     if Z <= numel(r)
        radius = r(Z);
     else
         radius = default_r; % default radius
     end
end