text:
function [opttheta] = minFuncSGD(funObj,theta,data,labels,...
                        options)
% Runs stochastic gradient descent with momentum to optimize the
% parameters for the given objective.
%
% Parameters:
%  funObj     -  function handle which accepts as input theta,
%                data, labels and returns cost and gradient w.r.t
%                to theta.
%  theta      -  unrolled parameter vector
%  data       -  stores data in m x n x numExamples tensor
%  labels     -  corresponding labels in numExamples x 1 vector
%  options    -  struct to store specific options for optimization
%
% Returns:
%  opttheta   -  optimized parameter vector
%
% Options (* required)
%  epochs*     - number of epochs through data
%  alpha*      - initial learning rate
%  minibatch*  - size of minibatch
%  momentum    - momentum constant, defualts to 0.9

%%======================================================================
%% Setup
assert(all(isfield(options,{'epochs','alpha','minibatch'})),...
        'Some options not defined');
if ~isfield(options,'momentum')
    options.momentum = 0.9;
end;
epochs = options.epochs;
alpha = options.alpha;
minibatch = options.minibatch;
m = length(labels); % training set size
% Setup for momentum
mom = 0.5;
momIncrease = 20;
velocity = zeros(size(theta));

%%======================================================================
%% SGD loop
it = 0;
for e = 1:epochs

    % randomly permute indices of data for quick minibatch sampling
    rp = randperm(m);

    for s=1:minibatch:(m-minibatch+1)
        it = it + 1;

        % increase momentum after momIncrease iterations
        if it == momIncrease
            mom = options.momentum;
        end;

        % get next randomly selected minibatch
        mb_data = data(:,:,:,rp(s:s+minibatch-1));
        mb_labels = labels(rp(s:s+minibatch-1));

        % evaluate the objective function on the next minibatch
        [cost grad] = funObj(theta,mb_data,mb_labels);

        % Instructions: Add in the weighted velocity vector to the
        % gradient evaluated above scaled by the learning rate.
        % Then update the current weights theta according to the
        % sgd update rule

        %%% YOUR CODE HERE %%%
        velocity = mom * velocity + alpha * grad;
        theta = theta - velocity;

        fprintf('Epoch %d: Cost on iteration %d is %f\n',e,it,cost);
    end;

    % aneal learning rate by factor of two after each epoch
    alpha = alpha/2.0;

end;

opttheta = theta;

end

author: xuzhenqi

repo: cnn

save_path: github-repos/MATLAB/xuzhenqi-cnn


text:
function h = or(f, g)
%|   SINGFUN logical OR.
%   F | G performs a logical OR of the SINGFUN objects F and G and returns a
%   SMOOTHFUN containing elements set to either logical 1 (TRUE) or logical 0
%   (FALSE).  The output SMOOTHFUN is set to 1 if either input SINGFUN
%   contains a non-zero element at that point, otherwise it is set to 0.
%   F and G must either be identically zero or have roots in their domains.
%   If this is not the case, garbage is returned with no warning.

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

if ( isa(f, 'singfun') )
    f = f.smoothPart;
end

if ( isa(g, 'singfun') )
    g = g.smoothPart;
end

h = f | g;

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
%% This is for testing the Homogeneous Transformation functions in the robotics Toolbox

function tests = TransformationsTest
    tests = functiontests(localfunctions);

    clc
end

function teardownOnce(tc)
    close all
end

%% first of all check we can tell a good matrix from a bad one
function isrot_test(tc)
    R1 = diag([1 1 1]);    % proper
    R2 = diag([1 1 -1]);   % not proper
    R3 = diag([1 2 1]);    % not proper
    R4 = diag([2 0.5 1]);  % not proper

    % test shapes
    tc.verifyFalse( isrot(1) )
    tc.verifyFalse( isrot( zeros(2,2) ) )
    tc.verifyFalse( isrot( zeros(4,4) ) )
    tc.verifyFalse( isrot( zeros(3,1) ) )
    tc.verifyFalse( isrot( zeros(1,3) ) )

    % test shapes with validity check
    tc.verifyFalse( isrot(1, 1) )
    tc.verifyFalse( isrot( zeros(2,2), 1 ) )
    tc.verifyFalse( isrot( zeros(4,4) ), 1 )
    tc.verifyFalse( isrot( zeros(4,1) ), 1 )
    tc.verifyFalse( isrot( zeros(1,4) ), 1 )

    % test 3x3
    tc.verifyTrue( isrot(R1) )
    tc.verifyTrue( isrot(R2) )
    tc.verifyTrue( isrot(R3) )
    tc.verifyTrue( isrot(R4) )

    % test 3x3 with validity check
    tc.verifyTrue( isrot(R1, 1) )
    tc.verifyFalse( isrot(R2, 1) )
    tc.verifyFalse( isrot(R3, 1) )
    tc.verifyFalse( isrot(R4, 1) )

    % vector case
    tc.verifyTrue( isrot(cat(3, R1, R1, R1)) )
    tc.verifyTrue( isrot(cat(3, R1, R1, R1), 1) )
    tc.verifyTrue( isrot(cat(3, R1, R2, R3)) )
    tc.verifyFalse( isrot(cat(3, R1, R2, R3), 1) )
end

function ishomog_test(tc)
    T1 = diag([1 1 1 1]);    % proper
    T2 = diag([1 1 -1 1]);   % not proper
    T3 = diag([1 2 1 1]);    % not proper
    T4 = diag([2 0.5 1 1]);  % not proper
    T5 = diag([1 1 1 0]);    % not proper

    % test shapes
    tc.verifyFalse( ishomog(1) )
    tc.verifyFalse( ishomog( zeros(2,2) ) )
    tc.verifyFalse( ishomog( zeros(3,3) ) )
    tc.verifyFalse( ishomog( zeros(4,1) ) )
    tc.verifyFalse( ishomog( zeros(1,4) ) )

    % test shapes with validity check
    tc.verifyFalse( ishomog(1, 1) )
    tc.verifyFalse( ishomog( zeros(2,2), 1 ) )
    tc.verifyFalse( ishomog( zeros(3,3) ), 1 )
    tc.verifyFalse( ishomog( zeros(4,1) ), 1 )
    tc.verifyFalse( ishomog( zeros(1,4) ), 1 )

    % test 4x4
    tc.verifyTrue( ishomog(T1) )
    tc.verifyTrue( ishomog(T2) )
    tc.verifyTrue( ishomog(T3) )
    tc.verifyTrue( ishomog(T4) )
    tc.verifyTrue( ishomog(T5) )

    % test 4x4 with validity check
    tc.verifyTrue( ishomog(T1, 1) )
    tc.verifyFalse( ishomog(T2, 1) )
    tc.verifyFalse( ishomog(T3, 1) )
    tc.verifyFalse( ishomog(T4, 1) )
    tc.verifyFalse( ishomog(T5, 1) )

    % vector case
    tc.verifyTrue( ishomog(cat(3, T1, T1, T1)) )
    tc.verifyTrue( ishomog(cat(3, T1, T1, T1), 1) )
    tc.verifyTrue( ishomog(cat(3, T1, T2, T3)) )
    tc.verifyFalse( ishomog(cat(3, T1, T2, T3), 1) )
end

%% can we convert between rotation matrices and homogeneous coordinate matrices

function r2t_test(tc)

    % SO(3) case
    R = [1 2 3;4 5 6; 7 8 9];
    tc.verifyEqual(r2t(R),...
        [1 2 3 0; 4 5 6 0; 7 8 9 0; 0 0 0 1],'absTol',1e-10);

    % sequence case
    Rs = cat(3, R, R, R, R, R);
    Ts = r2t(Rs);
    verifySize(tc, Ts, [4 4 5]);
    tc.verifyEqual(Ts(:,:,2), ...
        [1 2 3 0; 4 5 6 0; 7 8 9 0; 0 0 0 1],'absTol',1e-10);

end

function t2r_test(tc)
    %Unit test for r2t with variables eul2tr([.1, .2, .3])

    % SE(3) case
    T = [1 2 3 4; 5 6 7 8; 9 10 11 12; 0 0 0 1];
    tc.verifyEqual(t2r(T),...
        [1 2 3; 5 6 7; 9 10 11],'absTol',1e-10);

    % sequence case
    Ts = cat(3, T, T, T, T, T);
    Rs = t2r(Ts);
    verifySize(tc, Rs, [3 3 5]);
    tc.verifyEqual(Rs(:,:,2), ...
        [1 2 3; 5 6 7; 9 10 11],'absTol',1e-10);

end

function rt2tr_test(tc)

    R = [1 2 3;4 5 6; 7 8 9];
    t = [-10; -11; -12];

    tc.verifyEqual(rt2tr(R, t),...
        [1 2 3 -10; 4 5 6 -11; 7 8 9 -12; 0 0 0 1],'absTol',1e-10);

    % sequence case
    Rs = cat(3, R, 2*R, 3*R);
    ts = cat(2, t, 2*t, 3*t);
    Ts = rt2tr(Rs, ts);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,1), ...
        [1 2 3 -10; 4 5 6 -11; 7 8 9 -12; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(Ts(:,:,2), ...
        [2*[1 2 3 -10; 4 5 6 -11; 7 8 9 -12]; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(Ts(:,:,3), ...
        [3*[1 2 3 -10; 4 5 6 -11; 7 8 9 -12]; 0 0 0 1],'absTol',1e-10);
end

function tr2rt_test(tc)
    %Unit test for r2t with variables eul2tr([.1, .2, .3])

    %% SE(3) case
    T = [1 2 3 4; 5 6 7 8; 9 10 11 12; 0 0 0 1];
    [R,t] = tr2rt(T);
    tc.verifyEqual(R, [1 2 3; 5 6 7; 9 10 11], 'absTol',1e-10);
    tc.verifyEqual(t, [4; 8; 12], 'absTol',1e-10);

    % sequence case
    Ts = cat(3, T, T, T, T, T);
    [Rs,ts] = tr2rt(Ts);
    verifySize(tc, Rs, [3 3 5]);
    verifySize(tc, ts, [5 3]);

    tc.verifyEqual(Rs(:,:,2), [1 2 3; 5 6 7; 9 10 11], 'absTol',1e-10);
    tc.verifyEqual(ts(2,:), [4 8 12], 'absTol',1e-10);

end

% Primitives
function rotx_test(tc)
    tc.verifyEqual(rotx(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(rotx(pi/2), [1 0 0; 0 0 -1; 0 1 0],'absTol',1e-10);
    tc.verifyEqual(rotx(pi), [1 0 0; 0 -1 0; 0 0 -1],'absTol',1e-10);

    tc.verifyEqual(rotx(90, 'deg'), [1 0 0; 0 0 -1; 0 1 0],'absTol',1e-10);
    tc.verifyEqual(rotx(180, 'deg'), [1 0 0; 0 -1 0; 0 0 -1],'absTol',1e-10);

    syms q
    R = rotx(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc, R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()rotx([1 2 3]),'SMTB:rotx:badarg');
end

function roty_test(tc)
    tc.verifyEqual(roty(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(roty(pi/2), [0 0 1; 0 1 0; -1 0 0],'absTol',1e-10);
    tc.verifyEqual(roty(pi), [-1 0 0; 0 1 0; 0 0 -1],'absTol',1e-10);

    tc.verifyEqual(roty(90, 'deg'), [0 0 1; 0 1 0; -1 0 0],'absTol',1e-10);
    tc.verifyEqual(roty(180, 'deg'), [-1 0 0; 0 1 0; 0 0 -1],'absTol',1e-10);

    syms q
    R = roty(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc, R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()roty([1 2 3]),'SMTB:roty:badarg');
end

function rotz_test(tc)
    tc.verifyEqual(rotz(0), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(rotz(pi/2), [0 -1 0; 1 0 0; 0 0 1],'absTol',1e-10);
    tc.verifyEqual(rotz(pi), [-1 0 0; 0 -1 0; 0 0 1],'absTol',1e-10);

    tc.verifyEqual(rotz(90, 'deg'), [0 -1 0; 1 0 0; 0 0 1],'absTol',1e-10);
    tc.verifyEqual(rotz(180, 'deg'), [-1 0 0; 0 -1 0; 0 0 1],'absTol',1e-10);

    syms q
    R = rotz(q);
    verifyInstanceOf(tc, R, 'sym');
    verifySize(tc,R, [3 3]);
    tc.verifyEqual(simplify(det(R)), sym(1));

    %test for non-scalar input
    verifyError(tc, @()rotz([1 2 3]),'SMTB:rotz:badarg');
end

function trotx_test(tc)
    tc.verifyEqual(trotx(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(trotx(pi/2), [1 0 0 0; 0 0 -1 0; 0 1 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotx(pi), [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(trotx(90, 'deg'), [1 0 0 0; 0 0 -1 0; 0 1 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotx(180, 'deg'), [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    %test for non-scalar input
    verifyError(tc, @()trotx([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function troty_test(tc)
    tc.verifyEqual(troty(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(troty(pi/2), [0 0 1 0; 0 1 0 0; -1 0 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(troty(pi), [-1 0 0 0; 0 1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(troty(90, 'deg'), [0 0 1 0; 0 1 0 0; -1 0 0 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(troty(180, 'deg'), [-1 0 0 0; 0 1 0 0; 0 0 -1 0; 0 0 0 1],'absTol',1e-10);
    %test for non-scalar input
    verifyError(tc, @()troty([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function trotz_test(tc)
    tc.verifyEqual(trotz(0), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(trotz(pi/2), [0 -1 0 0; 1 0 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotz(pi), [-1 0 0 0; 0 -1 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);

    tc.verifyEqual(trotz(90, 'deg'), [0 -1 0 0; 1 0 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    tc.verifyEqual(trotz(180, 'deg'), [-1 0 0 0; 0 -1 0 0; 0 0 1 0; 0 0 0 1],'absTol',1e-10);
    %test for non-scalar input
    verifyError(tc, @()trotz([1 2 3; 0 0 0 1]),'MATLAB:catenate:dimensionMismatch');
end

function transl_test(tc)

    % transl(P) -> T
    tc.verifyEqual(transl(1, 2, 3), [1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1], 'AbsTol', 1e-10);
    tc.verifyEqual(transl([1, 2, 3]), [1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1], 'AbsTol', 1e-10);

    x = rand(4,3);
    T = transl(x);
    tc.verifyEqual(T(1:3,4,1), x(1,:)', 'AbsTol', 1e-10);
    tc.verifyEqual(T(1:3,4,4), x(4,:)', 'AbsTol', 1e-10);

    % transl(T) -> P
    tc.verifyEqual(transl([1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1]), [1 2 3]', 'AbsTol', 1e-10);
    [a,b,c] = transl([1 0 0 1; 0 1 0 2; 0 0 1 3; 0 0 0 1]);
    tc.verifyEqual(a, 1);
    tc.verifyEqual(b, 2);
    tc.verifyEqual(c, 3);

    tc.verifyEqual(transl(T), x, 'AbsTol', 1e-10);
end

%% angle/vector form
%    angvec2r                   - angle/vector to RM
function angvec2r_test(tc)

    tc.verifyEqual(angvec2r( pi/2, [1 0 0]), rotx(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2r( pi/2, [0 1 0]), roty(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2r( pi/2, [0 0 1]), rotz(pi/2),'absTol',1e-10);

    tc.verifyEqual(angvec2r(0, [1 0 0]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 1 0]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 0 1]), eye(3,3),'absTol',1e-10);
    tc.verifyEqual(angvec2r(0, [0 0 0]), eye(3,3),'absTol',1e-10);

    verifyError(tc, @()angvec2r(1, [0 0 0]),'SMTB:angvec2r:badarg');

    verifyError(tc, @()angvec2r([1,2,3],0.1),'SMTB:angvec2r:badarg');
    verifyError(tc, @()angvec2r(1),'SMTB:angvec2r:badarg');
end

%    angvec2tr                  - angle/vector to HT
function angvec2tr_test(tc)

    tc.verifyEqual(angvec2tr( pi/2, [1 0 0]), trotx(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2tr( pi/2, [0 1 0]), troty(pi/2),'absTol',1e-10);
    tc.verifyEqual(angvec2tr( pi/2, [0 0 1]), trotz(pi/2),'absTol',1e-10);

    tc.verifyEqual(angvec2tr(0, [1 0 0]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 1 0]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 0 1]), eye(4,4),'absTol',1e-10);
    tc.verifyEqual(angvec2tr(0, [0 0 0]), eye(4,4),'absTol',1e-10);

    verifyError(tc, @()angvec2tr(1, [0 0 0]),'SMTB:angvec2r:badarg');

    verifyError(tc, @()angvec2tr([1,2,3],0.1),'SMTB:angvec2r:badarg');
    verifyError(tc, @()angvec2tr(1),'SMTB:angvec2tr:badarg');
end

%    tr2angvec                  - HT/RM to angle/vector form
function tr2angvec_test(tc)
    % null rotation
    % - vector isn't defined here, but RTB sets it (0 0 0)
    [theta, v] = tr2angvec(eye(3,3));
    tc.verifyEqual(theta, 0.0, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 0], 'absTol',1e-6);

    tr2angvec(eye(3,3))

    % canonic rotations
    [theta, v] = tr2angvec(rotx(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [1 0 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(roty(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(rotz(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 1], 'absTol',1e-6);

    % null rotation
    [theta, v] = tr2angvec(eye(4,4));
    tc.verifyEqual(theta, 0.0, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 0], 'absTol',1e-6);

    % canonic rotations
    [theta, v] = tr2angvec(trotx(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [1 0 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(troty(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    [theta, v] = tr2angvec(trotz(pi/2));
    tc.verifyEqual(theta, pi/2, 'absTol',1e-6);
    tc.verifyEqual(v, [0 0 1], 'absTol',1e-6);

    [theta, v] = tr2angvec(roty(pi/2), 'deg');
    tc.verifyEqual(theta, 90, 'absTol',1e-6);
    tc.verifyEqual(v, [0 1 0], 'absTol',1e-6);

    R = cat(3, rotx(pi/2), roty(pi/2), rotz(pi/2));
    [theta, v] = tr2angvec(R);
    tc.verifyEqual(theta, pi/2*[1 1 1]', 'absTol',1e-6);
    tc.verifyEqual(v, eye(3,3), 'absTol',1e-6);

    T = cat(3, trotx(pi/2), troty(pi/2), trotz(pi/2));
    [theta, v] = tr2angvec(T);
    tc.verifyEqual(theta, pi/2*[1 1 1]', 'absTol',1e-6);
    tc.verifyEqual(v, eye(3,3), 'absTol',1e-6);

    %test for scalar input
    verifyError(tc, @()tr2angvec(1), 'SMTB:tr2angvec:badarg');
end

%% 3-angle forms

function eul2r_test(tc)

    % ZYZ
    r2d = 180/pi;

    R = rotz(0.1) * roty(0.2) * rotz(0.3);

    tc.verifyEqual(eul2r(0.1, 0.2, 0.3), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r([0.1, 0.2, 0.3]), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), R, 'absTol',1e-10);
    tc.verifyEqual(eul2r([0.1, 0.2, 0.3]*r2d, 'deg'), R, 'absTol',1e-10);

    % trajectory case
    Rs = eul2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = eul2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()eul2r(1),'SMTB:eul2r:badarg');
end

%    eul2tr                     - Euler angles to HT
function eul2tr_test(tc)
    r2d = 180/pi;

    T = trotz(0.1) * troty(0.2) * trotz(0.3);

    tc.verifyEqual(eul2tr(0.1, 0.2, 0.3), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr([0.1, 0.2, 0.3]), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), T, 'absTol',1e-10);
    tc.verifyEqual(eul2tr([0.1, 0.2, 0.3]*r2d, 'deg'), T, 'absTol',1e-10);

    % trajectory case
    Ts = eul2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = eul2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()eul2tr(1),'SMTB:eul2r:badarg');
end

function rpy2r_test(tc)

    r2d = 180/pi;

    %% default zyx order
    R = rotz(0.3) * roty(0.2) * rotx(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %% xyz order

    R = rotx(0.3) * roty(0.2) * rotz(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3, 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3], 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'xyz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg', 'xyz'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'xyz');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'xyz', 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %% yxz order

    R = roty(0.3) * rotx(0.2) * rotz(0.1);

    tc.verifyEqual(rpy2r(0.1, 0.2, 0.3, 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3], 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'yxz'), R, 'absTol',1e-10);
    tc.verifyEqual(rpy2r([0.1, 0.2, 0.3]*r2d, 'deg', 'yxz'), R, 'absTol',1e-10);

    % trajectory case
    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'yxz');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    Rs = rpy2r( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'yxz', 'deg');
    verifySize(tc, Rs, [3 3 3]);
    tc.verifyEqual(Rs(:,:,2), R, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()rpy2tr(1),'SMTB:rpy2r:badarg');
end

function rpy2tr_test(tc)

    r2d = 180/pi;

    T = trotz(0.3) * troty(0.2) * trotx(0.1);
    seq = 'zyx';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    T = trotx(0.3) * troty(0.2) * trotz(0.1);
    seq = 'xyz';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    T = troty(0.3) * trotx(0.2) * trotz(0.1);
    seq = 'yxz';
    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', seq), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', seq), T, 'absTol',1e-10);

    % trajectory case
    T = trotz(0.3) * troty(0.2) * trotx(0.1);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]);
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    T = trotx(0.3) * troty(0.2) * trotz(0.1);

    tc.verifyEqual(rpy2tr(0.1, 0.2, 0.3, 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3], 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr(0.1*r2d, 0.2*r2d, 0.3*r2d, 'deg', 'xyz'), T, 'absTol',1e-10);
    tc.verifyEqual(rpy2tr([0.1, 0.2, 0.3]*r2d, 'deg', 'xyz'), T, 'absTol',1e-10);

    % trajectory case
    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3], 'xyz');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    Ts = rpy2tr( [0.1, 0.2, 0.3; 0.1 0.2 0.3; 0.1 0.2 0.3]*r2d, 'xyz', 'deg');
    verifySize(tc, Ts, [4 4 3]);
    tc.verifyEqual(Ts(:,:,2), T, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()rpy2tr(1),'SMTB:rpy2r:badarg');
end

function tr2eul_test(tc)

    eul = [0.1 0.2 0.3];
    R = eul2r(eul);
    tc.verifyEqual(tr2eul(R), eul,'absTol',1e-10);
    tc.verifyEqual(tr2eul(R, 'deg'), eul*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2eul(Rs);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul,'absTol',1e-10);
    x = tr2eul(Rs, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul*180/pi,'absTol',1e-10);

    T = eul2tr(eul);
    tc.verifyEqual(tr2eul(T), eul,'absTol',1e-10);
    tc.verifyEqual(tr2eul(T, 'deg'), eul*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2eul(Ts);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul,'absTol',1e-10);
    x = tr2eul(Ts, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), eul*180/pi,'absTol',1e-10);

    % test singularity case
    eul = [0.1 0 0.3];
    R = eul2r(eul);
    tc.verifyEqual(eul2r( tr2eul(R) ), R,'absTol',1e-10);
    tc.verifyEqual(eul2r( tr2eul(R, 'deg'), 'deg'), R,'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()tr2eul(1),'SMTB:tr2eul:badarg');

    % test flip
    eul = [-0.1 0.2 0.3];
    R = eul2r(eul);
    eul2 = tr2eul(R, 'flip');
    tc.verifyTrue(eul2(1) > 0);
    tc.verifyEqual(eul2r(eul2), R,'absTol',1e-10);
end

function tr2rpy_test(tc)
    rpy = [0.1 0.2 0.3];
    R = rpy2r(rpy);
    tc.verifyEqual(tr2rpy(R), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(R, 'deg'), rpy*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2rpy(Rs);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Rs, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    T = rpy2tr(rpy);
    tc.verifyEqual(tr2rpy(T), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(T, 'deg'), rpy*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2rpy(Ts);
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Ts, 'deg');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    % xyz order
    R = rpy2r(rpy, 'xyz');
    tc.verifyEqual(tr2rpy(R, 'xyz'), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(R, 'deg', 'xyz'), rpy*180/pi,'absTol',1e-10);

    Rs = cat(3, R, R, R, R);
    x = tr2rpy(Rs, 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Rs, 'deg', 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    T = rpy2tr(rpy, 'xyz');
    tc.verifyEqual(tr2rpy(T, 'xyz'), rpy,'absTol',1e-10);
    tc.verifyEqual(tr2rpy(T, 'deg', 'xyz'), rpy*180/pi,'absTol',1e-10);

    Ts = cat(3, T, T, T, T);
    x = tr2rpy(Ts, 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy,'absTol',1e-10);
    x = tr2rpy(Ts, 'deg', 'xyz');
    verifySize(tc, x, [4 3]);
    tc.verifyEqual(x(2,:), rpy*180/pi,'absTol',1e-10);

    % corner cases
    seq = 'zyx';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    seq = 'xyz';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    seq = 'yxz';
    ang = [pi 0 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 0 pi];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 pi/2 0]; % singularity
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);
    ang = [0 -pi/2 0];
    a = rpy2tr(ang, seq);
    tc.verifyEqual(rpy2tr(tr2rpy(a, seq), seq), a, 'absTol',1e-10);

    %test for scalar input
    verifyError(tc, @()tr2rpy(1),'SMTB:tr2rpy:badarg');
end

%    oa2r                       - orientation and approach vector to RM
function oa2r_test(tc)
    %Unit test for oa2r with variables ([0 1 0] & [0 0 1])
    tc.verifyEqual(oa2r([0 1 0], [0 0 1]),...
        [1     0     0
        0     1     0
        0     0     1],'absTol',1e-10);
    %test for scalar input
    verifyError(tc, @()oa2r(1),'SMTB:oa2r:badarg');
end

%    oa2tr                      - orientation and approach vector to HT
function oa2tr_test(tc)
    %Unit test for oa2tr with variables ([0 1 0] & [0 0 1])
    tc.verifyEqual(oa2tr([0 1 0], [0 0 1]),...
        [1     0     0     0
        0     1     0     0
        0     0     1     0
        0     0     0     1],'absTol',1e-10);
    %test for scalar input
    verifyError(tc, @()oa2tr(1),'SMTB:oa2tr:badarg');
end

function trchain_test(tc)
    a1 = 0;

    T = trchain('Tx(a1) Ty(a1) Tz(a1) Rx(a1) Ry(a1) Rz(a1)');
    tc.verifyEqual(T, eye(4,4), 'abstol', 1e-10);

    a1 = 1; a2 = 2; a3 = 3;
    tc.verifyEqual( trchain('Tx(a1) Ty(a2) Tz(a3)'), transl(1,2,3), 'abstol', 1e-10);

    a1 = 0.3; a2 = 0.4; a3 = 0.5;
    tc.verifyEqual( trchain('Rx(a1) Ry(a2) Rz(a3)'), trotx(0.3)*troty(0.4)*trotz(0.5), 'abstol', 1e-10);

    tc.verifyEqual( trchain('Rx(q1) Ry(q2) Rz(q3)', [.3,.4,.5]), trotx(0.3)*troty(0.4)*trotz(0.5), 'abstol', 1e-10);

    syms q1 q2 q3 a1 a2 a3
    tc.verifyEqual( trchain('Rx(q1) Tx(a1) Ry(q2) Ty(a2) Rz(q3) Tz(a3)', [q1 q2 q3]), trotx(q1)*transl(a1,0,0)*troty(q2)*transl(0,a2,0)*trotz(q3)*transl(0,0,a3) );

    syms q1(t) q2(t) q3(t) t a1 a2 a3
    tc.verifyEqual( trchain('Rx(q1) Tx(a1) Ry(q2) Ty(a2) Rz(q3) Tz(a3)', [q1 q2 q3]), formula(trotx(q1)*transl(a1,0,0)*troty(q2)*transl(0,a2,0)*trotz(q3)*transl(0,0,a3)) );
end

function trchain2_test(tc)
    a1 = 0;

    T = trchain2('Tx(a1) Ty(a1) R(a1)');
    tc.verifyEqual(T, eye(3,3), 'abstol', 1e-10);

    a1 = 1; a2 = 2;
    tc.verifyEqual( trchain2('Tx(a1) Ty(a2)'), transl2(1,2), 'abstol', 1e-10);

    a1 = 0.3; a2 = 0.4;
    % R() is the same as Rz()
    tc.verifyEqual( trchain2('R(a1) Rz(a2)'), trot2(0.3)*trot2(0.4), 'abstol', 1e-10);

    syms q1 a1 a2
    tc.verifyEqual( trchain2('R(q1) Tx(a1) Ty(a2)', [q1]), trot2(q1)*transl2(a1,0)*transl2(0,a2) );
end

function trinterp_test(tc)
    %% between two transforms
    T0 = transl(1,2,3);
    T1 = transl(-1,-2,-3)*trotx(pi);
    Tm = trotx(pi/2);

    T = trinterp(T0, T1, 0);
    tc.verifyEqual(size(T), [4 4]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp(T0, T1, 3);   % interpolate in 3 steps
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    %% between identity and transform
    T0 = eye(4,4);
    T1 = transl(2,4,6)*trotx(pi);
    Tm = transl(1,2,3)*trotx(pi/2);

    T = trinterp(T1, 0);
    tc.verifyEqual(size(T), [4 4]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp(T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp(T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp(T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp(T1, 3);   % interpolate in 3 steps
    tc.verifyEqual(size(T), [4 4 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    tc.verifyError( @() trinterp(T0, T1, -1), 'SMTB:trinterp:badarg');
    tc.verifyError( @() trinterp(T0, T1, 1.7), 'SMTB:trinterp:badarg');
    tc.verifyError( @() trinterp(T0), 'SMTB:trinterp:badarg');

end

function trinterp2_test(tc)
    %% between two transforms
    T0 = transl2(1,2);
    T1 = transl2(-1,-2)*trot2(pi);
    Tm = trot2(pi/2);

    T = trinterp2(T0, T1, 0);
    tc.verifyEqual(size(T), [3 3]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp2(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp2(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp2(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    T = trinterp2(T0, T1, 3);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);

    %% between identity and transform
    T0 = eye(3, 3);
    T1 = transl2(2,4)*trot2(pi);
    Tm = transl2(1,2)*trot2(pi/2);

    T = trinterp2(T1, 0);
    tc.verifyEqual(size(T), [3 3]);
    tc.verifyEqual(T, T0, 'abstol', 1e-10);

    tc.verifyEqual(trinterp2(T0, T1, 1), T1, 'abstol', 1e-10);
    tc.verifyEqual(trinterp2(T0, T1, 0.5), Tm, 'abstol', 1e-10);

    T = trinterp2(T0, T1, [0.5 0 1]);
    tc.verifyEqual(size(T), [3 3 3]);
    tc.verifyEqual(T(:,:,1), Tm, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,2), T0, 'abstol', 1e-10);
    tc.verifyEqual(T(:,:,3), T1, 'abstol', 1e-10);
end

%    trnorm                     - normalize HT
function trnorm_test(tc)

    R = [0.9 0 0; .2 .6 .3; .1 .2 .4]';
    tc.verifyEqual(det(trnorm(R)), 1, 'absTol', 1e-14);

    t = [1 2 3]';
    T = rt2tr(R, t);
    Tn = trnorm(T);
    tc.verifyEqual(det(trnorm(t2r(Tn))), 1, 'absTol', 1e-14);
    tc.verifyEqual(Tn(1:3,4), t);

    % vector input
    RR = cat(3, R, R, R, R);
    RRn = trnorm(RR);
    verifySize(tc, RRn, [3 3 4]);
    tc.verifyEqual(det(RRn(:,:,1)), 1, 'absTol', 1e-14);
    tc.verifyEqual(det(RRn(:,:,1)), 1, 'absTol', 1e-14);

    %HACK)    tc.verifyEqual(arrayfun( @(x) det(trnorm(x)), RR

    %test for scalar input
    verifyError(tc, @()trnorm(1),'SMTB:trnorm:badarg');
end

function trprint_test(tc)

    % null case

    trprint(eye(4,4))

    a = transl([1,2,3]) * eul2tr([.1, .2, .3]);

    trprint(a);

    trprint('a') % equivalent to trprint a on the command line

    s = evalc( 'trprint(a)' );
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);
    s = evalc( 'trprint(cat(3, a, a, a))' );
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    s = trprint(a);
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    trprint(a, 'euler');
    trprint(a, 'euler', 'radian');
    trprint(a, 'rpy');
    trprint(a, 'rpy', 'radian');
    trprint(a, 'rpy', 'radian', 'xyz');
    trprint(a, 'rpy', 'radian', 'zyx');
    trprint(a, 'rpy', 'radian', 'yxz');

    trprint(a, 'angvec');
    trprint(a, 'angvec', 'radian');
    trprint(a, 'angvec', 'radian', 'fmt', '%g');
    trprint(a, 'angvec', 'radian', 'fmt', '%g', 'label', 'bob');

    % vector case

    a = cat(3, a, a, a);
    trprint(a);

    s = evalc( 'trprint(a)' );
    tc.verifyTrue(isa(s, 'char') );
    tc.verifyEqual( length(regexp(s, '\n', 'match')), 4);

    trprint(a, 'euler');
    trprint(a, 'euler', 'radian');
    trprint(a, 'rpy');
    trprint(a, 'rpy', 'radian');
    trprint(a, 'rpy', 'radian', 'xyz');
    trprint(a, 'rpy', 'radian', 'zyx');
    trprint(a, 'rpy', 'radian', 'yxz');

    trprint(a, 'angvec');
    trprint(a, 'angvec', 'radian');
    trprint(a, 'angvec', 'radian', 'fmt', '%g');
    trprint(a, 'angvec', 'radian', 'fmt', '%g', 'label', 'bob');

    % write to a file
    f = fopen('test.txt', 'w');
    trprint(a, 'fid', f);
    fclose(f);
    % read it back
    f = fopen('test.txt', 'r');
    s2 = fread(f, '*char');
    fclose(f);
    delete 'test.txt';
    tc.verifyTrue( all(s(:) == s2) );

end

function trscale_test(tc)

    tc.verifyEqual( trscale(1), eye(4,4) );
    tc.verifyEqual( trscale([1 2 3]), diag([1 2 3 1]) );
    tc.verifyEqual( trscale(1, 2, 3), diag([1 2 3 1]) );

end

function vex_test(tc)
    S = [
        0    -3     2
        3     0    -1
        -2     1     0
        ];

    tc.verifyEqual( vex(S), [1 2 3]');

    tc.verifyEqual( vex(-S), -[1  2 3]');
end

function skew_test(tc)
    R = skew([1 2 3]);

    tc.verifyTrue( isrot(R) );  % check size

    tc.verifyEqual( norm(R'+ R), 0, 'absTol', 1e-10); % check is skew

    tc.verifyEqual( vex(R), [1 2 3]'); % check contents, vex already verified

    tc.verifyError( @() skew([1 2]), 'SMTB:skew:badarg')
end

function vexa_test(tc)
    S = [
        0    -6     5     1
        6     0    -4     2
        -5     4     0     3
        0     0     0     0
        ];

    tc.verifyEqual( vexa(S), [1:6]');

    S = [
        0     6     5     1
        -6     0     4    -2
        -5    -4     0     3
        0     0     0     0
        ];
    tc.verifyEqual( vexa(S), [1 -2 3 -4 5 -6]');
end

function skewa_test(tc)
    T = skewa([3 4 5]);

    tc.verifyTrue( ishomog2(T) );  % check size

    R = T(1:2,1:2);
    tc.verifyEqual( norm(R'+ R), 0, 'absTol', 1e-10); % check is skew

    tc.verifyEqual( vexa(T), [3 4 5]'); % check contents, vexa already verified
    tc.verifyError( @() skewa([1 2]), 'SMTB:skewa:badarg')

end

function trlog_test(tc)
    %unit tests for matrix expon stuff

    %%% SO(3) tests
    % zero rotation case
    tc.verifyEqual(trlog( eye(3,3) ), skew([0 0 0]), 'absTol', 1e-6);

    % rotation by pi case
    tc.verifyEqual(trlog( rotx(pi) ), skew([pi 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( roty(pi) ), skew([0 pi 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( rotz(pi) ), skew([0 0 pi]), 'absTol', 1e-6);

    % general case
    tc.verifyEqual(trlog( rotx(0.2) ), skew([0.2 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( roty(0.3) ), skew([0 0.3 0]), 'absTol', 1e-6);
    tc.verifyEqual(trlog( rotz(0.4) ), skew([0 0 0.4]), 'absTol', 1e-6);

    R = rotx(0.2) * roty(0.3) * rotz(0.4);
    [th,w] = trlog(R);
    tc.verifyEqual( logm(R), skew(th*w), 'absTol', 1e-10)

    %%% SE(3) tests

    % pure translation
    tc.verifyEqual(trlog( transl([1 2 3]) ), ...
        [0 0 0 1; 0 0 0 2; 0 0 0 3; 0 0 0 0], 'absTol', 1e-6);

    % mixture
    T = transl([1 2 3])*trotx(0.3);
    tc.verifyEqual(trlog(T), logm(T), 'absTol', 1e-6);

    T = transl([1 2 3])*troty(0.3);
    tc.verifyEqual(trlog(T), logm(T), 'absTol', 1e-6);

    [th,w] = trlog(T);
    tc.verifyEqual( logm(T), skewa(th*w), 'absTol', 1e-10)

    verifyError(tc, @()trlog(0),'SMTB:trlog:badarg');
end

function trexp_test(tc)
    %unit tests for matrix log stuff

    %%% SO(3) tests

    %% so(3)

    % zero rotation case
    tc.verifyEqual(trexp(skew([0 0 0])), eye(3,3), 'absTol', 1e-6);

    %% so(3), theta

    tc.verifyEqual(trexp(skew([0 0 0]), 1), eye(3,3), 'absTol', 1e-6);

    % rotation by pi case
    tc.verifyEqual(trexp(skew([pi 0 0])), rotx(pi), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 pi 0])), roty(pi), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 pi])), rotz(pi), 'absTol', 1e-6);

    % general case
    tc.verifyEqual(trexp(skew([0.2 0 0])), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0.3 0])), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 0.4])), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp(skew([1 0 0]), 0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 1 0]), 0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp(skew([0 0 1]), 0.4), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp([1 0 0], 0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 1 0], 0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 0 1], 0.4), rotz(0.4), 'absTol', 1e-6);

    tc.verifyEqual(trexp([1 0 0]*0.2), rotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 1 0]*0.3), roty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp([0 0 1]*0.4), rotz(0.4), 'absTol', 1e-6);

    %%% SE(3) tests

    %% sigma = se(3)
    % pure translation
    tc.verifyEqual(trexp( skewa([1 2 3 0 0 0]) ), transl([1 2 3]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0.2 0 0]) ), trotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0.3 0]) ), troty(0.3), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0 0.4]) ), trotz(0.4), 'absTol', 1e-6);

    % mixture
    T = transl([1 2 3])*trotx(0.2)*troty(0.3)*trotz(0.4);
    tc.verifyEqual(trexp(logm(T)), T, 'absTol', 1e-6);

    %% twist vector
    tc.verifyEqual(trexp( double(Twist(T))), T, 'absTol', 1e-6);

    %% (sigma, theta)
    tc.verifyEqual(trexp( skewa([1 0 0 0 0 0]), 2), transl([2 0 0]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 1 0 0 0 0]), 2), transl([0 2 0]), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 1 0 0 0]), 2), transl([0 0 2]), 'absTol', 1e-6);

    tc.verifyEqual(trexp( skewa([0 0 0 1 0 0]), 0.2), trotx(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 1 0]), 0.2), troty(0.2), 'absTol', 1e-6);
    tc.verifyEqual(trexp( skewa([0 0 0 0 0 1]), 0.2), trotz(0.2), 'absTol', 1e-6);

    %% (twist, theta)
    tc.verifyEqual(trexp(Twist('R', [1 0 0], [0 0 0]).S, 0.3), trotx(0.3), 'absTol', 1e-6);

    T = transl([1 2 3])*troty(0.3);
    tc.verifyEqual(trexp(logm(T)), T, 'absTol', 1e-6);

    tc.verifyError( @() trexp(1), 'SMTB:trexp:badarg')
end

function e2h_test(tc)
    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    tc.verifyEqual( e2h(P1), [P1; 1]);
    tc.verifyEqual( e2h(P2), [P2; ones(1,5)]);
end

function h2e_test(tc)
    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    tc.verifyEqual( h2e(e2h(P1)), P1);
    tc.verifyEqual( h2e(e2h(P2)), P2);
end

function homtrans_test(tc)

    P1 = [1;2; 3];
    P2 = [1 2 3 4 5; 6 7 8 9 10; 11 12 13 14 15];

    T = eye(4,4);
    tc.verifyEqual( homtrans(T, P1), P1);
    tc.verifyEqual( homtrans(T, P2), P2);

    Q = [-2;2; 4];
    T = transl(Q);
    tc.verifyEqual( homtrans(T, P1), P1+Q);
    tc.verifyEqual( homtrans(T, P2), P2+Q);

    T = trotx(pi/2);
    tc.verifyEqual( homtrans(T, P1), [P1(1); -P1(3); P1(2)], 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2), [P2(1,:); -P2(3,:); P2(2,:)], 'absTol', 1e-6);

    T =  transl(Q)*trotx(pi/2);
    tc.verifyEqual( homtrans(T, P1), [P1(1); -P1(3); P1(2)]+Q, 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2), [P2(1,:); -P2(3,:); P2(2,:)]+Q, 'absTol', 1e-6);

    % projective point case
    P1h = e2h(P1);
    P2h = e2h(P2);
    tc.verifyEqual( homtrans(T, P1h), [P1h(1); -P1h(3); P1h(2); 1]+[Q;0], 'absTol', 1e-6);
    tc.verifyEqual( homtrans(T, P2h), [P2h(1,:); -P2h(3,:); P2h(2,:); ones(1,5)]+[Q;0], 'absTol', 1e-6);

    % sequence case
    TT = cat(3, T, T, T, T, T);
    Q = homtrans(T, TT);
    tc.verifyEqual( Q(:,:,3), T*T);

    % error case
    tc.verifyError( @() homtrans(ones(2,2), P1), 'SMTB:homtrans:badarg')

end

author: petercorke

repo: spatialmath-matlab


text:
function [f] = poch(z,n)
%Pochhammer function (z)n = z(z+1)(z+2)...(z+n-1)
%
%usage:  f = poch(z,n)
%
%tested on version 5.3.1
%
%        z and n may be complex but must be equal in size.
%
%see also: Gamma, Fact

%Paul Godfrey
%pgodfrey@conexant.com
%8-24-00

f = gamma(z+n)./gamma(z);

p=find(z==0 & n==0);
if ~isempty(p)
    f(p)=0;
end

return

author: Sable

repo: mcbench-benchmarks


text:
function [mNewCatalog] = syn_catalog(nNumberEvents, fBValue, fMc, fInc, fMinLat, fMaxLat, fMinLon, fMaxLon, fMinDepth, fMaxDepth, fMinTime, fMaxTime,nSynMode,mCatalog)
    % Creates a synthetic catalog.
    %
    % [mNewCatalog] = syn_catalog(nNumberEvents, fBValue, fMc, fInc, fMinLat, fMaxLat,
    %                                      fMinLon, fMaxLon, fMinDepth, fMaxDepth, fMinTime, fMaxTime)
    %
    %
    % Input parameters:
    %   nNumberEvents     Number of events in the catalog
    %   fBValue           b-value of the catalog
    %   fMc               Magnitude of completeness of the catalog (= minimum magnitude)
    %   fInc              Magnitude increment (usually 0.1)
    %   fMinLat           Minimum latitude value of events in the catalog
    %   fMaxLat           Maximun latitude value of events in the catalog
    %   fMinLon           Minimum longitude value of events in the catalog
    %   fMaxLon           Maximun longitude value of events in the catalog
    %   fMinDepth         Minimum depth of events in the catalog (positive value)
    %   fMaxDepth         Maximun depth of events in the catalog (positive value)
    %   fMinTime          Minimum date/time of events in the catalog (decimal year: e.g. 1983.5)
    %   fMaxTime          Maximun date/time of events in the catalog (decimal year: e.g. 1987.9)
    % nSynMode          Type of synthetic catalog 0:homogeneous distr; 1:based
    % on real catalog 2:hypocenter based on real catalog, magnitude and focal
    % time is randomly computed
    % mCatalog          declustered catalog needed only for nSynMode=1
    %
    % Output parameters:
    %   mNewCatalog       Synthetic catalog
    %
    % Danijel Schorlemmer
    % April 17, 2002
    % Updates
    % Mai  9, 2007  van Stiphout, Thomas     replaced datevec.m by decyear2mat.m

    report_this_filefun();

    % allocate matrix for synthetc catalog
    mNewCatalog = nan(nNumberEvents,14);

    % initial shift of hypocenter from reference event.
    fHypoShift=5;
    if nSynMode == 0
        %     % Create empty catalog
        %     mNewCatalog = nan(nNumberEvents, 10);

        % Create magnitudes
        mNewCatalog(:,6) = syn_create_magnitudes(mNewCatalog, fBValue, fMc, fInc);

        % Randomize
        rng('shuffle');

        % Create location
        mNewCatalog(:,1) = rand(nNumberEvents, 1) * (fMaxLon-fMinLon) + fMinLon;
        mNewCatalog(:,2) = rand(nNumberEvents, 1) * (fMaxLat-fMinLat) + fMinLat;
        mNewCatalog(:,7) = rand(nNumberEvents, 1) * (fMaxDepth-fMinDepth) + fMinDepth;

        % Randomize
        rng('shuffle');

        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        mNewCatalog(:,3)=mNewCatalog(randperm(size(mNewCatalog,1)),3);
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));

        % Remove column 10 (seconds)
        mNewCatalog = mNewCatalog(:,1:end);

    elseif nSynMode==1
        % take from real catalog hypocenters and magnitudes  ....
        for i=1:nNumberEvents
            mNewCatalog(i,:)=mCatalog(ceil(rand(1,1)*size(mCatalog,1)),1:end);
        end
        % create new  times
        % Randomize
        rng('shuffle');
        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));
        mNewCatalog=mNewCatalog(:,1:10);
    elseif nSynMode == 2
        for i=1:nNumberEvents
            mNewCatalog(i,:) = mCatalog(ceil(rand(1,1)*size(mCatalog,1)),1:end);
        end
        % create new magnitudes
        mNewCatalog(:,6) = syn_create_magnitudes(nNumberEvents, fBValue, fMc, fInc);
        % create new  times
        % Randomize
        rng('shuffle');
        % Create focal times
        mNewCatalog(:,3) = rand(nNumberEvents, 1) * (fMaxTime-fMinTime) + fMinTime;
        % vst: datevec does not transform mNewCatalog(:,3) properly. Replaced by decyear2mat.
        [mNewCatalog(:,10) mNewCatalog(:,4) mNewCatalog(:,5) mNewCatalog(:,8) mNewCatalog(:,9) tmp] = decyear2mat(mNewCatalog(:,3));
        % shift hypocenters
        vTmp = km2deg(rand(nNumberEvents,2).*fHypoShift-fHypoShift/2);
        mNewCatalog(:,1:2)=mNewCatalog(:,1:2)+vTmp;
    end
end

author: CelsoReyes


text:
function varargout = power_internal1(varargin)
%power_internal1
% Used for cases such as 2^x, and is treated as evaluation-based operators

switch class(varargin{1})

    case 'double'
        varargout{1} = varargin{2}.^varargin{1};

    case 'sdpvar'
        if isa(varargin{2},'sdpvar')
            x = varargin{2};
            y = varargin{1};
            varargout{1} = exp(y*log(x)); %x^y = exp(log(x^y))
        else
            if length(varargin{1}) > 1 || size(varargin{2},1) ~= size(varargin{2},2)
                error('Inputs must be a scalar and a square matrix. To compute elementwise POWER, use POWER (.^) instead.');
            end
            x = varargin{2};
            y = varargin{1};
            if isa(x,'double') && x==1 && length(y)==1
                varargout{1} = 1;
            else
                varargout{1} = InstantiateElementWise(mfilename,varargin{:});
            end
        end

    case 'char'

        X = varargin{3};
        Y = varargin{4};
        F=[];
        if Y>=1
            operator = struct('convexity','none','monotonicity','increasing','definiteness','positve','model','callback');
        elseif Y>=0
            operator = struct('convexity','none','monotonicity','decreasing','definiteness','positive','model','callback');
        else
            % Base is negative, so the power has to be an integer
            F = (integer(X));
            operator = struct('convexity','none','monotonicity','decreasing','definiteness','none','model','callback');
        end

        operator.bounds = @bounds_power;
        operator.convexhull = @convexhull_power;
        operator.derivative = @(x)derivative(x,Y);
        if Y >= 0
            operator.inverse = @(x,Y)inverse(x,Y);
        end

        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = [X(:);Y(:)];
    otherwise
        error('SDPVAR/power_internal1 called with CHAR argument?');
end

% This should not be hidden here....
function [L,U] = bounds_power(xL,xU,base)
if base >= 1
    L = base^xL;
    U = base^xU;
elseif base>= 0
    L = base^xU;
    U = base^xL;
else
    disp('Not implemented yet. Report bug if you need this')
    error
end

function x = inverse(y,base)
if y <=0
    x = -inf;
else
    x = log(y)/log(base);
end

function df = derivative(x,base)
if length(base)~=length(x)
    base = base*ones(size(x));
end
f = base.^x;
df = log(base)*f;

function [Ax, Ay, b] = convexhull_power(xL,xU,base)
fL = base^xL;
fU = base^xU;
dfL = log(base)*fL;
dfU = log(base)*fU;
[Ax,Ay,b] = convexhullConvex(xL,xU,fL,fU,dfL,dfU);

author: zarathustr

repo: LibQPEP

save_path: github-repos/MATLAB/zarathustr-LibQPEP


text:
function angle = lines_exp_angle_3d ( p1, p2, q1, q2 )

%*****************************************************************************80
%
%% LINES_EXP_ANGLE_3D finds the angle between two explicit lines in 3D.
%
%  Discussion:
%
%    The explicit form of a line in 3D is:
%
%      ( P1, P2 ) = ( (X1,Y1,Z1), (X2,Y2,Z3) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3,1), P2(3,1), two points on the first line.
%
%    Input, real Q1(3,1), Q2(3,1), two points on the second line.
%
%    Output, real ANGLE, the angle in radians between the two
%    lines.  The angle is computed using the ACOS function, and so lies between
%    0 and PI.  But if one of the lines is degenerate, the angle is
%    returned as -1.0.
%
  dim_num = 3;

  pnorm = sqrt ( sum ( ( p2(1:dim_num,1) - p1(1:dim_num,1) ).^2 ) );

  if ( pnorm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LINES_EXP_ANGLE_3D - Fatal error!\n' );
    fprintf ( 1, '  The line (P1,P2) is degenerate!\n' );
    angle = -1.0;
    return
  end

  qnorm = sqrt ( sum ( ( q2(1:dim_num,1) - q1(1:dim_num,1) ).^2 ) );

  if ( qnorm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LINES_EXP_ANGLE_3D - Fatal error!\n' );
    fprintf ( 1, '  The line (Q1,Q2) is degenerate!\n' );
    angle = -1.0;
    return
  end

  pdotq = ( p2(1:dim_num,1) - p1(1:dim_num,1) )' ...
        * ( q2(1:dim_num,1) - q1(1:dim_num,1) );

  ctheta = pdotq / ( pnorm * qnorm );

  angle = r8_acos ( ctheta );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
clear
gcp;

name = 'granule_love2.tif';
if ~exist(name,'file')  % download file if it doesn't exist in the directory
    url = 'https://www.dropbox.com/s/mjmtwn4pdgydkny/granule_love2.tif.zip?dl=1';
    filename = 'granule_love2.tif.zip';
    fprintf('downloading the file...');
    outfilename = websave(filename,url);
    fprintf('...done. Now unzipping...')
    unzip(filename);
    fprintf('done.');
end

tic; Y = read_file(name); toc; % read the file (optional, you can also pass the path in the function instead of Y)
Y = single(Y);                 % convert to single precision
T = size(Y,ndims(Y));
Y = Y - min(Y(:));
%% set parameters (first try out rigid motion correction)

options_rigid = NoRMCorreSetParms('d1',size(Y,1),'d2',size(Y,2),'bin_width',200,'max_shift',15,'us_fac',50,'init_batch',200);

%% perform motion correction
tic; [M1,shifts1,template1,options_rigid] = normcorre(Y,options_rigid); toc

%% now try non-rigid motion correction (also in parallel)
options_nonrigid = NoRMCorreSetParms('d1',size(Y,1),'d2',size(Y,2),'grid_size',[32,32],'mot_uf',4,'bin_width',200,'max_shift',15,'max_dev',3,'us_fac',50,'init_batch',200);
tic; [M2,shifts2,template2,options_nonrigid] = normcorre_batch(Y,options_nonrigid); toc

%% compute metrics

nnY = quantile(Y(:),0.005);
mmY = quantile(Y(:),0.995);

[cY,mY,vY] = motion_metrics(Y,10);
[cM1,mM1,vM1] = motion_metrics(M1,10);
[cM2,mM2,vM2] = motion_metrics(M2,10);
T = length(cY);
%% plot metrics
figure;
    ax1 = subplot(2,3,1); imagesc(mY,[nnY,mmY]);  axis equal; axis tight; axis off; title('mean raw data','fontsize',14,'fontweight','bold')
    ax2 = subplot(2,3,2); imagesc(mM1,[nnY,mmY]);  axis equal; axis tight; axis off; title('mean rigid corrected','fontsize',14,'fontweight','bold')
    ax3 = subplot(2,3,3); imagesc(mM2,[nnY,mmY]); axis equal; axis tight; axis off; title('mean non-rigid corrected','fontsize',14,'fontweight','bold')
    subplot(2,3,4); plot(1:T,cY,1:T,cM1,1:T,cM2); legend('raw data','rigid','non-rigid'); title('correlation coefficients','fontsize',14,'fontweight','bold')
    subplot(2,3,5); scatter(cY,cM1); hold on; plot([0.9*min(cY),1.05*max(cM1)],[0.9*min(cY),1.05*max(cM1)],'--r'); axis square;
        xlabel('raw data','fontsize',14,'fontweight','bold'); ylabel('rigid corrected','fontsize',14,'fontweight','bold');
    subplot(2,3,6); scatter(cM1,cM2); hold on; plot([0.9*min(cY),1.05*max(cM1)],[0.9*min(cY),1.05*max(cM1)],'--r'); axis square;
        xlabel('rigid corrected','fontsize',14,'fontweight','bold'); ylabel('non-rigid corrected','fontsize',14,'fontweight','bold');
    linkaxes([ax1,ax2,ax3],'xy')
%% plot shifts

shifts_r = squeeze(cat(3,shifts1(:).shifts));
shifts_nr = cat(ndims(shifts2(1).shifts)+1,shifts2(:).shifts);
shifts_nr = reshape(shifts_nr,[],ndims(Y)-1,T);
shifts_x = squeeze(shifts_nr(:,1,:))';
shifts_y = squeeze(shifts_nr(:,2,:))';

patch_id = 1:size(shifts_x,2);
str = strtrim(cellstr(int2str(patch_id.')));
str = cellfun(@(x) ['patch # ',x],str,'un',0);

figure;
    ax1 = subplot(311); plot(1:T,cY,1:T,cM1,1:T,cM2); legend('raw data','rigid','non-rigid'); title('correlation coefficients','fontsize',14,'fontweight','bold')
            set(gca,'Xtick',[])
    ax2 = subplot(312); plot(shifts_x); hold on; plot(shifts_r(:,1),'--k','linewidth',2); title('displacements along x','fontsize',14,'fontweight','bold')
            set(gca,'Xtick',[])
    ax3 = subplot(313); plot(shifts_y); hold on; plot(shifts_r(:,2),'--k','linewidth',2); title('displacements along y','fontsize',14,'fontweight','bold')
            xlabel('timestep','fontsize',14,'fontweight','bold')
    linkaxes([ax1,ax2,ax3],'x')

%% plot a movie with the results

figure;
for t = 1:1:T
    subplot(121);imagesc(Y(:,:,t),[nnY,mmY]); xlabel('raw data','fontsize',14,'fontweight','bold'); axis equal; axis tight;
    title(sprintf('Frame %i out of %i',t,T),'fontweight','bold','fontsize',14); colormap('bone')
    subplot(122);imagesc(M2(:,:,t),[nnY,mmY]); xlabel('non-rigid corrected','fontsize',14,'fontweight','bold'); axis equal; axis tight;
    title(sprintf('Frame %i out of %i',t,T),'fontweight','bold','fontsize',14); colormap('bone')
    set(gca,'XTick',[],'YTick',[]);
    drawnow;
    pause(0.02);
end

author: flatironinstitute

repo: NoRMCorre


text:
function l = IsFirstAfter(timestamps,list,varargin)

%IsFirstAfter - Identify first item after each of a list of timestamps.
%
% For each element i of a list of test timestamps, find in a list of reference timestamps
% the first element j larger than i. Returns a list of logical indices (the elements of the
% test list which are greater than all the elements of the reference timestamps are ignored).
%
%  USAGE
%
%    logical = IsFirstAfter(timestamps,list,<options>)
%
%    timestamps     a list of reference timestamps
%    list           a list of test timestamps
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'strict'      'on' for strict (>) comparisons, 'off' otherwise (>=)
%                   (default = 'off')
%    =========================================================================
%
%  SEE
%
%    See also IsLastBefore.

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% 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.

strict = 'off';

if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help IsFirstAfter'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
  if ~ischar(varargin{i}),
    error(['Parameter ' num2str(i+1) ' is not a property (type ''help IsFirstAfter'' for details).']);
  end
  switch(lower(varargin{i})),
    case 'strict',
		strict = lower(varargin{i+1});
		if ~isstring_FMAT(strict,'on','off'),
			error('Incorrect value for property ''strict'' (type ''help IsFirstAfter'' for details).');
		end
    otherwise,
      error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help IsFirstAfter'' for details).']);
  end
end

% Make sure 'timestamps' looks like [t1;t2;...;tn] and 'list' like [l1 l2 ... lm]
if min(size(timestamps)) ~= 1 | min(size(list)) ~= 1,
	error('Incorrect sizes (must be vectors)');
end
if size(timestamps,1) == 1,
	timestamps = timestamps';
end
if size(list,2) == 1,
	list = list';
end

% Now compare [t1 t1 ... t1;t2 t2 ... t2;...;tn tn ... tn] with [l1 l2 ... lm;l1 l2 ... lm;...;l1 l2 ... lm]
if strcmp(strict,'on'),
	where = repmat(timestamps,1,length(list)) > repmat(list,length(timestamps),1);
else
	where = repmat(timestamps,1,length(list)) >= repmat(list,length(timestamps),1);
end
% This yielded something like [0 0 0;0 0 0;...;1 0 0;...;1 1 0;...;1 1 1;...;1 1 1]
% where the transition from 0 to 1 in each column indicates the index of the first element
% in 'timestamps' superior to the corresponding element in 'list'. Now, find these transitions.
% (we append zeros before and ones after 'where' to handle the special cases where the first
% element in 'timestamps' superior to the element of 'list' is the first element of 'timestamps'
% or does not exist).
where = diff([zeros(1,size(where,2));where;ones(1,size(where,2))]);
% Convert the resulting logical matrix into a list of logical indices
l = sum(where,2);
l = l(1:length(timestamps));
l = logical(l==1);

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


text:
function box = intersectBoxes(box1, box2)
%INTERSECTBOXES Intersection of two bounding boxes.
%
%   RES = intersectBoxes(BOX1, BOX2)
%
%   Example
%   box1 = [5 20 5 30];
%   box2 = [0 15 0 15];
%   intersectBoxes(box1, box2)
%   ans =
%       5 15 5 15
%
%   See also
%   boxes2d, drawBox, mergeBoxes
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-07-26, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

% unify sizes of data
if size(box1,1) == 1
    box1 = repmat(box1, size(box2,1), 1);
elseif size(box2, 1) == 1
    box2 = repmat(box2, size(box1,1), 1);
elseif size(box1,1) ~= size(box2,1)
    error('Bad size for inputs');
end

% compute extreme coords
mini = min(box1(:,[2 4]), box2(:,[2 4]));
maxi = max(box1(:,[1 3]), box2(:,[1 3]));

% concatenate result into a new box structure
box = [maxi(:,1) mini(:,1) maxi(:,2) mini(:,2)];

author: mattools

repo: matGeom

save_path: github-repos/MATLAB/mattools-matGeom


text:
%--- help for generic/stoch_simul ---
%
%  stoch_simul -- attempts to emulate dynare's stoch_simul
%
%  ::
%
%     oo_=stoch_simul(obj)
%
%     oo_=stoch_simul(obj,var_list)
%
%     oo_=stoch_simul(obj,var_list,varargin)
%
%  Args:
%
%     obj (dsge | rise ): model object
%
%     var_list (empty | char | cellstr ): List of variables for which to run
%       stoch_simul
%
%     varargin : Pairwise list of extra arguments
%
%  Returns:
%     :
%
%     - **oo_** [struct]: structure containing
%        - **irfs** [struct]: structure containing impulse responses in time
%          series format
%        - **simulations** [struct]: structure containing impulse responses
%          in time series format
%        - **vcov** [matrix]: (simulated) variance-covariance
%        - **skewness** [vector]: skewness measure
%        - **kurtosis** [vector]: kurtosis measure
%        - **variance** [vector]: (simulated) variances for the endogenous
%          variables
%        - **stdev** [vector]: (simulated) standard deviations for the endogenous
%          variables
%        - **corrcoef** [matrix]: (simulated) correlation coefficients for
%          the endogenous variables
%
%

author: jmaih


text:
function a = in( x, y )

error( nargchk( 2, 2, nargin ) );
b = newcnstr( evalin( 'caller', 'cvx_problem', '[]' ), x, y, '==' );
if nargout, a = b; end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.

repo: REST


text:
function[inds] = indicesOfAInB(A, B)
% [inds] = indicesOfAInB(A, B)
%
% Get the indices of a list B such that B(inds) == A.
% This assumes that each element of A is in B (but not the other way
% around).
% Note: % B need not be unique, but A does!
% Afterwards: B(indicesOfAInB(A, B)) == A
%
% Copyright by Holger Caesar, 2014

% Make column vectors
A = A(:)';
B = B(:)';

% Check for duplicates in A
Aun = unique(A, 'stable');
assert(isequal(A, Aun), 'Error: This function only works for unique elements in A (but not in B)!');

% Intersect A and B
[~, indsIntoB] = intersect(B, A);

% Since inter is sorted, we need to sort A as well
[~, AunSorting] = sort(A);

% Additional assertions to understand what's going on
% assert(isequal(A(Asorting), inter));
% assert(isequal(inter, B(indsIntoB)));

% Check if A is really included in B
checkIncluded = ismember(A, B);
if ~all(checkIncluded),
    notIncludedList = strjoin(A(find(~checkIncluded, 5)), ', ');
    error('Error: Some elements of A are not included in B! Examples are %s', notIncludedList);
end;

% Apply mappings from B to inter and then from inter to A
inds = indsIntoB(invertPermutation(AunSorting));

% Consistency check
assert(isequal(A, B(inds)));

author: nightrome

repo: matconvnet-calvin


text:
function [D, Dsm] = distMatrixShow( D, IDX, show )
% Useful visualization of a distance matrix of clustered points.
%
% D is sorted into k blocks, where the ith block contains all the points in
% cluster i. When D is displayed the blocks are shown explicitly.  Hence
% for a good clustering (under a spherical gaussian assumption) the
% 'diagonal' blocks ought to be mostly dark, and all other block ought to be
% relatively white.  One can thus quickly visualize the quality of the
% clustering, or even how clusterable the points are.  Outliers (according
% to IDX) are removed from D.
%
% USAGE
%  [D, Dsm] = distMatrixShow( D, IDX, [show] )
%
% INPUTS
%  D       - nxn distance matrix
%  IDX     - cluster membership [see kmeans2.m]
%  show    - [1] will display results in figure(show)
%
% OUTPUTS
%  D       - sorted nxn distance matrix
%  Dsm     - sorted and smoothed nxn distance matrix
%
% EXAMPLE
%  % not the best example since points are already ordered
%  [X,IDX] = demoGenData(100,0,5,2,10,2,0);
%  distMatrixShow( pdist2(X,X), IDX );
%
% See also VISUALIZEDATA, KMEANS2
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(show) ); show=1; end

k = max(IDX);
n = size(D,1);

%%% remove outliers from D and IDX
inliers = IDX>0;
D = D( inliers, inliers );
IDX = IDX( inliers );

%%% get order of points and rearrange D and IDX
order = IDX2order( IDX );
IDX = IDX( order );
D = D( order, order );

%%% compute smoothed version of D
cnts = zeros(1,k); for i=1:k; cnts(i)=sum(IDX==i); end
cumCnts = cumsum(cnts);  cumCnts2=[0 cumCnts];  Dsm = D;
inds = 1:k; inds = inds( cnts>0 );
for i=inds
  rs = cumCnts2(i)+1:cumCnts2(i+1);
  for j=inds
    cs = cumCnts2(j)+1:cumCnts2(j+1);
    ds = D( rs, cs  );
    Dsm( rs, cs ) = mean(ds(:));
  end;
end;

%%% show D and lines seperating super clusters.
if(show)
  figure(show); clf;
  subplot(1,2,1); im(D); hold('on')
  for i=1:k-1
    line( [.5,n+.5], [cumCnts(i)+.5,cumCnts(i)+.5] );
    line( [cumCnts(i)+.5,cumCnts(i)+.5], [.5,n+.5] );
  end;
  hold('off');
  subplot(1,2,2); im( Dsm );
end

author: pdollar

repo: toolbox

save_path: github-repos/MATLAB/pdollar-toolbox


text:
function kernelS = realign_kernels(kernelS, tmax)

nt = size(kernelS, 1);

isub = linspace(1, nt, (nt-1)*10 + 1);

for j = 1:size(kernelS,2)
   ker = kernelS(:,j);
   kup = interp1(1:1:nt, ker, isub, 'spline');

   [~, imax] = max(kup);
   itmax = isub(imax); % the time of the max in units, needs to be at tmax

   kup = interp1(1:1:nt, ker, isub + itmax - tmax, 'spline', 0);

   kernelS(:,j) = kup(1:10:end);
end

author: cortex-lab


text:
function model = ZF_for_Fast_RCNN_VOC0712(model)

model.solver_def_file        = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF', 'solver_30k60k.prototxt');
model.test_net_def_file      = fullfile(pwd, 'models', 'fast_rcnn_prototxts', 'ZF', 'test.prototxt');

model.net_file               = fullfile(pwd, 'models', 'pre_trained_models', 'ZF', 'ZF.caffemodel');
model.mean_image             = fullfile(pwd, 'models', 'pre_trained_models', 'ZF', 'mean_image');

end

author: ShaoqingRen


text:
function channels = GetChannels(groups)

%GetChannels - Get list of channels for one or more spike groups.
%
%  USAGE
%
%    channels = GetChannels(groups)
%
%    group          one or more optional electrode groups (default = all groups)
%
%  EXAMPLES
%
%    all = GetChannels;           % list all channels
%    channels = GetChannels(2);   % list all channels for electrode group 2

% Copyright (C) 2004-2012 by Michaël Zugaro
%
% 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.

global DATA;
if isempty(DATA),
	error('No session defined (did you forget to call SetCurrentSession? Type ''help Data'' for details).');
end

nGroups = DATA.spikeGroups.nGroups;
if nargin < 1,
	groups = 1:nGroups;
else
	if ~isivector(groups,'>0',['<=' int2str(nGroups)]),
		error('Incorrect group list (type ''help GetChannels'' for details).');
	end
end

channels = [DATA.spikeGroups.groups{groups}]';


text:
%  高斯消去法 Gaussian_Elimination
function [x] = my_ge(A, b)

[m,n]=size(A);
if (m~=n)
    fprintf('\n Error: A 不是方阵!\n'); return;
end

m = zeros(n);
%消元过程
for k = 1:n-1
  for i = k+1:n
    m(i,k) = A(i,k)/A(k,k);
    A(i,:) = A(i,:) - m(i,k) * A(k,:);
    b(i) = b(i) - m(i,k) * b(k);
  end
end
%回代过程
x(n) = b(n)/A(n,n);
for k = n-1 : -1 : 1
    summary = 0;
    for j = k+1 : n
        summary = summary + A(k,j) * x(j);
    end
    x(k) = (b(k) - summary) / A(k,k);
end

repo: NumericalAnalysis


text:
function dz = dynamics(z,u)
% dz = dynamics(z,u)
%
% This function returns the dynamics of the toy car example. The state of
% the car is its position and orientation in the plane, and the control is
% the rate of change in steering angle.
%
% INPUTS:
%   z = [3, n] = [x;y;θ] = state = [pos; pos; angle];
%   u = [1, n] = steering rate
%
% OUTPUTS:
%  dz = dz/dt
%

% x = z(1,:);   %Dynamics do not depend on x position
% y = z(2,:):   %Dynamics do not depend on y position
th = z(3,:);

dx = cos(th);
dy = sin(th);
dth = u;

dz = [dx;dy;dth];

end

author: MatthewPeterKelly

repo: OptimTraj


text:
%% Random phase scheme
close all
clear all

%% system parameters
load('user_channel.mat','K','N','M','ite','pd','ps','Hd_w','theta_init','AP_angle','IRS_angle',...
    'G_sig','User_angle','Hr_sig','eb1','eb2','path_d','path_i');
etai=1;
weight=1./((path_d));
weight=weight./sum(weight);
omega=weight;
snr_w=linspace(0,10,3);
rate_w=zeros(1,length(snr_w));
parfor s0=1:length(snr_w)
    %%
    fprintf('snr=%d\n',snr_w(s0));
    Pt=10.^(snr_w(s0)/10);
    rate=zeros(1,ite);
    for t0=1:ite
        %%
        beta=zeros(1,K);
        Hd=pd.*Hd_w(:,:,t0);
        theta=theta_init(:,:,t0);
        Theta=diag(theta');
        %%
        G=channel_G(AP_angle,IRS_angle,G_sig(:,:,t0),eb1,eb2,N,M);
        Hr=ps.*channel_Hr(User_angle,Hr_sig(:,:,t0),eb1,eb2,K,N);
        %%
        H=Hd+Hr*Theta*G;
        [ ~,~,f1 ] = init_W( H,M,K,Pt,omega );
         rate(t0)=f1;
    end
    rate_w(s0)=mean(rate);
end
%%
figure
plot(snr_w,rate_w,'ro-');
save('down_IRS_phaserand.mat','snr_w','rate_w','M','K','N','omega');
 %%

author: guohuayan


function [price] = PROJ_Double_Barrier(N,alph,call,L,U, S_0,W,M,T,r,rnCHF)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Discrete Double Barrier Options using PROJ method
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
% Author: Justin Lars Kirkby
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% q   = dividend yield (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=1)
% M   = number of subintervals of [0,T] (total of M+1 monitoring points in time grid, including S_0)
% call = 1 for call (else put)
% [L,U] = barriers
% rnCHF = risk netural characteristic function (function handle with single argument)
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% N     = number of grid/basis points (power of 2, e.g. 2^12), resolution = 2*alph/(N-1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

l = log(L/S_0);  u = log(U/S_0);  dt = T/M;

K = N/2;
dx = (u-l)/(K-1);
a  = 1/dx;

E  = ceil(2*alph/(u-l));
if M<= 12
    E = min(E,4);
else
    E = min(E,3);
end

a2    = a^2;
N_Ee  = E*N;
Cons2 = 24*a2*exp(-r*dt)/N_Ee ;

grand = @(w)rnCHF(w).*(sin(w/(2*a))./w).^2./(2+cos(w/a));
dw    = 2*pi*a/N_Ee ;
omega = (dw: dw: (N_Ee -1)*dw);      %We calcuate coefficient of w=0 explicitly
zmin  = (1 - E*K)*dx;           %K corresponds to zero
beta  = Cons2*real(fft([1/(24*a^2) exp(-1i*zmin*omega).*feval(grand,omega)]));

toepM = [beta(E*K:-1:(E-1)*K+1)';0; beta((E+1)*K-1:-1:E*K+1)'];
toepM = fft(toepM);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%STEP 1: Payoff Coefficients
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xmin = l;
nnot = floor(1-xmin*a);   %index to left of x_0=0
lws = log(W/S_0);
nbar = floor(a*(lws - xmin)+1);
rho   = lws - (xmin+(nbar - 1)*dx);
zeta  = a*rho;
xnbar = xmin + (nbar - 1)*dx;

Thet  = zeros(K,1);
Cons3 = 1/48;
Cons4 = 1/12;

%%%%%%  Gaussian Quad Constants
q_plus = (1 + sqrt(3/5))/2;  q_minus = (1 - sqrt(3/5))/2;
b3  = sqrt(15); b4 = b3/10;

%%%% PAYOFF CONSTANTS-----------------------------------
varthet_01 = exp(.5*dx)*(5*cosh(b4*dx) - b3*sinh(b4*dx) + 4)/18;
varthet_m10 = exp(-.5*dx)*(5*cosh(b4*dx) + b3*sinh(b4*dx) + 4)/18;
varthet_star = varthet_01 + varthet_m10;

if call==1  %DBC
    sigma = 1 - zeta; sigma_plus = (q_plus-.5)*sigma; sigma_minus = (q_minus-.5)*sigma;
    es1 = exp(dx*sigma_plus); es2 = exp(dx*sigma_minus);
    dbar_0 = .5 + zeta*(.5*zeta-1);
    dbar_1 = sigma*(1 - .5*sigma);

    d_0 = exp((rho+dx)*.5)*sigma^2/18*(5*((1-q_minus)*es2 +(1-q_plus)*es1) + 4);
    d_1 = exp((rho-dx)*.5)*sigma/18*(5*( (.5*(zeta+1) +sigma_minus)*es2 + (.5*(zeta+1) +sigma_plus)*es1 ) + 4*(zeta+1) );

    Thet(nbar)         = W*(exp(-rho)*d_0 - dbar_0);
    Thet(nbar + 1)     = W*(exp(dx-rho)*(varthet_01 +d_1) -(.5 + dbar_1) );
    Thet(nbar + 2:K-1) = exp(xmin +dx*(nbar+1:K-2))*S_0*varthet_star - W;
    Thet(K)            = U*varthet_m10 - W/2;
    %%%%%%%
    p = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
    Val = p(1:K);
    %%%%%%%
    for m=M-2:-1:0
        Thet(1)      = Cons3*(13*Val(1)+15*Val(2)-5*Val(3)+Val(4));
        Thet(K)      = Cons3*(13*Val(K)+15*Val(K-1)-5*Val(K-2)+Val(K-3));
        Thet(2:K -1) = Cons4*(Val(1:K-2)+10*Val(2:K-1)+Val(3:K));
        %%%%%%%
        p        = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
        Val = p(1:K);
    end

else  %DBP
    zeta_plus = zeta*q_plus; zeta_minus = zeta*q_minus;
    rho_plus = rho*q_plus; rho_minus = rho*q_minus;

    ed1 = exp(rho_minus); ed2 = exp(rho/2); ed3 = exp(rho_plus);

    dbar_1 = zeta^2/2;
    dbar_0 = zeta - dbar_1;         %  dbar_1 = zeta + .5*((zeta - 1)^2 - 1);
    d_0    = zeta*(5*( (1-zeta_minus)*ed1 + (1-zeta_plus)*ed3 ) + 4*(2-zeta)*ed2)/18;
    d_1    = zeta*( 5*(zeta_minus*ed1 + zeta_plus*ed3) + 4*zeta*ed2 )/18;

    Thet(1)        =  W/2 - L*varthet_01;
    Thet(2:nbar-1) =  W - exp(xmin +dx*(1:nbar-2))*S_0*varthet_star;
    Thet(nbar)     =  W*(.5 + dbar_0 - exp(-rho)*(varthet_m10 + d_0));
    Thet(nbar + 1) =  W*(dbar_1 - exp(- rho)*d_1);
    %%%%%%%
    p   = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
    Val = p(1:K);
    %%%%%%%
    for m=M-2:-1:0
        Thet(1)      = Cons3*(13*Val(1)+15*Val(2)-5*Val(3)+Val(4));
        Thet(K)      = Cons3*(13*Val(K)+15*Val(K-1)-5*Val(K-2)+Val(K-3));
        Thet(2:K -1) = Cons4*(Val(1:K-2)+10*Val(2:K-1)+Val(3:K));
        %%%%%%%
        p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
        Val  = p(1:K);
    end
end

xnot = l+(nnot-1)*dx;
xs = [xnot-2*dx,xnot-dx,xnot,xnot+dx,xnot+2*dx];
ys = [Val(nnot-2),Val(nnot-1),Val(nnot),Val(nnot+1),Val(nnot+2)];
price = spline(xs,ys,0);

end


text:
function [varargout] = funname(varargin)

% SOLID_ANGLE of a planar triangle as seen from the origin
%
% The solid angle W subtended by a surface S is defined as the surface
% area W of a unit sphere covered by the surface's projection onto the
% sphere. Solid angle is measured in steradians, and the solid angle
% corresponding to all of space being subtended is 4*pi sterradians.
%
% Use:
%   [w] = solid_angle(v1, v2, v3)
% or
%   [w] = solid_angle(pnt, tri)
% where v1, v2 and v3 are the vertices of a single triangle in 3D or
% pnt and tri contain a description of a triangular mesh (this will
% compute the solid angle for each triangle)

% Copyright (C) 2003-2009, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: solid_angle.m 2885 2011-02-16 09:41:58Z roboos $

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The first section contains the plain Matlab implementation. The mex file
% is many times faster and this function is called so frequently (for
% large meshes), that the mex file should be used in all practical cases.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [w] = solid_angle(r1, r2, r3);
%
% if nargin==2
%   % reassign the input arguments
%   pnt = r1;
%   tri = r2;
%   npnt = size(pnt,1);
%   ntri = size(tri,1);
%   w    = zeros(ntri,1);
%   % compute solid angle for each triangle
%   for i=1:ntri
%     r1 = pnt(tri(i,1),:);
%     r2 = pnt(tri(i,2),:);
%     r3 = pnt(tri(i,3),:);
%     w(i) = solid_angle(r1, r2, r3);
%   end
%   return
% elseif nargin==3
%   % compute the solid angle for this triangle
%   cp23_x = r2(2) * r3(3) - r2(3) * r3(2);
%   cp23_y = r2(3) * r3(1) - r2(1) * r3(3);
%   cp23_z = r2(1) * r3(2) - r2(2) * r3(1);
%   nom = cp23_x * r1(1) + cp23_y * r1(2) + cp23_z * r1(3);
%   n1 = sqrt (r1(1) * r1(1) + r1(2) * r1(2) + r1(3) * r1(3));
%   n2 = sqrt (r2(1) * r2(1) + r2(2) * r2(2) + r2(3) * r2(3));
%   n3 = sqrt (r3(1) * r3(1) + r3(2) * r3(2) + r3(3) * r3(3));
%   ip12 = r1(1) * r2(1) + r1(2) * r2(2) + r1(3) * r2(3);
%   ip23 = r2(1) * r3(1) + r2(2) * r3(2) + r2(3) * r3(3);
%   ip13 = r1(1) * r3(1) + r1(2) * r3(2) + r1(3) * r3(3);
%   den = n1 * n2 * n3 + ip12 * n3 + ip23 * n1 + ip13 * n2;
%   if (nom == 0)
%     if (den <= 0)
%       w = nan;
%       return
%     end
%   end
%   w = 2 * atan2 (nom, den);
%   return
% else
%   error('invalid input');
% end

% compile the missing mex file on the fly
% remember the original working directory
pwdir = pwd;

% determine the name and full path of this function
funname = mfilename('fullpath');
mexsrc  = [funname '.c'];
[mexdir, mexname] = fileparts(funname);

try
  % try to compile the mex file on the fly
  warning('trying to compile MEX file from %s', mexsrc);
  cd(mexdir);

  if ispc
    mex -I. -c geometry.c
    mex -I. -c solid_angle.c ; mex solid_angle.c solid_angle.obj geometry.obj
  else
    mex -I. -c geometry.c
    mex -I. -c solid_angle.c ; mex -o solid_angle solid_angle.o geometry.o
  end

  cd(pwdir);
  success = true;

catch
  % compilation failed
  disp(lasterr);
  error('could not locate MEX file for %s', mexname);
  cd(pwdir);
  success = false;
end

if success
  % execute the mex file that was juist created
  funname   = mfilename;
  funhandle = str2func(funname);
  [varargout{1:nargout}] = funhandle(varargin{:});
end

author: PatternRecognition

repo: OpenBMI