text:
function [out] = vertcat_fields_domains(s, exclude_last)

if nargin < 2 || isempty(exclude_last)
    exclude_last = false;
end

out = struct();
fields = fieldnames(s);
count = length(fields);
for i = 1:count
    field = fields{i};
    if ismatrix(s(1).(field))
        x = vertcat_domains(exclude_last, s.(field));
        if exclude_last
            x(end, :) = [];
        end
        out.(field) = x;
    end
end

end

author: ayonga

repo: frost-dev

save_path: github-repos/MATLAB/ayonga-frost-dev


text:
function makescript(d, filename)

%tstoolbox/@description/makescript
%   Syntax:
%     * makescript (signal, scriptfilename)
%
%   creates a Matlab m-file that contains exactly the the processing steps
%   that have been applied to get the input signal. This gives a kind of
%   macro facility for tstool.
%   Example
%          signal s was calculated through several processing steps from
%          signal s0 (the raw or original signal) Now makescript(s,
%          'foo.m') will create a Matlab m-file named foo.m which, applied
%          to s0, will give s.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,2);

[path,name,ext] = fileparts(filename);

n = findstr(name, '.m');
if isempty(n)
	funname = name;
else
	funname = name(1:n-1);
end

if length(d.commandlines)==0
   error('Signal has no commands that can be used to create a script');
end

[fid, message] = fopen(filename, 'wt');	% open as text file

if fid == -1
	error(message)
end

line = ['function s = ' funname '(s)\n'];
fprintf(fid, line);

for i=1:length(d.commandlines)
   line = [get(d.commandlines, i) '\n'];
   fprintf(fid, line);
end

fclose(fid);

author: benfulcher

repo: hctsa

save_path: github-repos/MATLAB/benfulcher-hctsa


text:
function Mn=dataNorm(M,normOpt)

switch normOpt
    case 1 %Using max and min
        Mn=double(M);
        Mn=Mn-min(Mn(:));
        Mn=Mn./max(Mn(:));
    case 2 %Using max only and letting M(M<0)= 0
        Mn=double(M);
        Mn(Mn<0)=0;
        Mn=Mn./max(Mn(:));
end

%%
% _*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/>.

author: gibbonCode

repo: GIBBON

save_path: github-repos/MATLAB/gibbonCode-GIBBON


text:
function b = imfilter(varargin)
%IMFILTER N-D filtering of multidimensional images.
%   B = IMFILTER(A,H) filters the multidimensional array A with the
%   multidimensional filter H.  A can be logical or it can be a
%   nonsparse numeric array of any class and dimension.  The result,
%   B, has the same size and class as A.
%
%   Each element of the output, B, is computed using double-precision
%   floating point.  If A is an integer or logical array, then output
%   elements that exceed the range of the given type are truncated,
%   and fractional values are rounded.
%
%   B = IMFILTER(A,H,OPTION1,OPTION2,...) performs multidimensional
%   filtering according to the specified options.  Option arguments can
%   have the following values:
%
%   - Boundary options
%
%       X            Input array values outside the bounds of the array
%                    are implicitly assumed to have the value X.  When no
%                    boundary option is specified, IMFILTER uses X = 0.
%
%       'symmetric'  Input array values outside the bounds of the array
%                    are computed by mirror-reflecting the array across
%                    the array border.
%
%       'replicate'  Input array values outside the bounds of the array
%                    are assumed to equal the nearest array border
%                    value.
%
%       'circular'   Input array values outside the bounds of the array
%                    are computed by implicitly assuming the input array
%                    is periodic.
%
%   - Output size options
%     (Output size options for IMFILTER are analogous to the SHAPE option
%     in the functions CONV2 and FILTER2.)
%
%       'same'       The output array is the same size as the input
%                    array.  This is the default behavior when no output
%                    size options are specified.
%
%       'full'       The output array is the full filtered result, and so
%                    is larger than the input array.
%
%   - Correlation and convolution
%
%       'corr'       IMFILTER performs multidimensional filtering using
%                    correlation, which is the same way that FILTER2
%                    performs filtering.  When no correlation or
%                    convolution option is specified, IMFILTER uses
%                    correlation.
%
%       'conv'       IMFILTER performs multidimensional filtering using
%                    convolution.
%
%   Notes
%   -----
%   This function may take advantage of hardware optimization for datatypes
%   uint8, uint16, int16, single, and double to run faster.
%
%   Example
%   -------------
%       originalRGB = imread('peppers.png');
%       h = fspecial('motion',50,45);
%       filteredRGB = imfilter(originalRGB,h);
%       figure, imshow(originalRGB), figure, imshow(filteredRGB)
%       boundaryReplicateRGB = imfilter(originalRGB,h,'replicate');
%       figure, imshow(boundaryReplicateRGB)
%
%   See also FSPECIAL, CONV2, CONVN, FILTER2.

%   Copyright 1993-2013 The MathWorks, Inc.

% Testing notes
% Syntaxes
% --------
% B = imfilter(A,H)
% B = imfilter(A,H,Option1, Option2,...)
%
% A:       numeric, full, N-D array.  May not be uint64 or int64 class.
%          May be empty. May contain Infs and Nans. May be complex. Required.
%
% H:       double, full, N-D array.  May be empty. May contain Infs and Nans.
%          May be complex. Required.
%
% A and H are not required to have the same number of dimensions.
%
% OptionN  string or a scalar number. Not case sensitive. Optional.  An
%          error if not recognized.  While there may be up to three options
%          specified, this is left unchecked and the last option specified
%          is used.  Conflicting or inconsistent options are not checked.
%
%        A choice between these options for boundary options
%        'Symmetric'
%        'Replicate'
%        'Circular'
%         Scalar #  - Default to zero.
%       A choice between these strings for output options
%        'Full'
%        'Same'  - default
%       A choice between these strings for functionality options
%        'Conv'
%        'Corr'  - default
%
% B:   N-D array the same class as A.  If the 'Same' output option was
%      specified, B is the same size as A.  If the 'Full' output option was
%      specified the size of B is size(A)+size(H)-1, remembering that if
%      size(A)~=size(B) then the missing dimensions have a size of 1.
%
%
% IMFILTER should use a significantly less amount of memory than CONVN.

% MATLAB Compiler pragma: iptgetpref is indirectly invoked by the code that
% loads the Intel IPP library.
%#function iptgetpref

[a, h, boundary, sameSize, convMode] = parse_inputs(varargin{:});

[finalSize, pad] = computeSizes(a, h, sameSize);

%Empty Inputs
% 'Same' output then size(b) = size(a)
% 'Full' output then size(b) = size(h)+size(a)-1
if isempty(a)

  b = handleEmptyImage(a, sameSize, finalSize);
  return

elseif isempty(h)

  b = handleEmptyFilter(a, sameSize, finalSize);
  return

end

% Pad input based on dimensions of filter kernel.
a = padImage(a,pad,boundary);

% Separate real and imaginary parts of the filter (h) in MATLAB and
% filter imaginary and real parts of the image (a) in the mex code.
if (isSeparable(a, h))

  % extract the components of the separable filter
  [u,s,v] = svd(h);
  s = diag(s);
  hcol = u(:,1) * sqrt(s(1));
  hrow = v(:,1)' * sqrt(s(1));

  % intermediate results should be stored in doubles in order to
  % maintain sufficient precision
  class_of_a = class(a);
  if ~isa(a,'double')
    change_class = true;
    a = double(a);
  else
    change_class = false;
  end

  % apply the first component of the separable filter (hrow)
  out_size_row = [size(a,1) finalSize(2:end)];
  start = [0 pad(2:end)];
  b_tmp = filterPartOrWhole(a, out_size_row, hrow, start, sameSize, convMode);

  % apply the other component of the separable filter (hcol)
  start = [pad(1) 0 pad(3:end)];
  b = filterPartOrWhole(b_tmp, finalSize, hcol, start, sameSize, convMode);

  if change_class
      b = cast(b, class_of_a);
  end

else % non-separable filter case
    a1=gpuArray(a);
    finalSize1=gpuArray(finalSize);
    h1=gpuArray(h);
    pad1=gpuArray(pad);
    sameSize1=gpuArray(sameSize);
  convMode1=gpuArray(convMode);
  b1 = filterPartOrWhole(a1, finalSize1, h1, pad1, sameSize1, convMode1);
  b=gather(b1);

end

%======================================================================

%--------------------------------------------------------------
function [a, h, boundary, sameSize, convMode] = parse_inputs(a, h, varargin)

narginchk(2,5);

validateattributes(a,{'numeric' 'logical'},{'nonsparse'},mfilename,'A',1);
validateattributes(h,{'double'},{'nonsparse'},mfilename,'H',2);

%Assign defaults
boundary = 0;  %Scalar value of zero
output = 'same';
do_fcn = 'corr';

allStrings = {'replicate', 'symmetric', 'circular', 'conv', 'corr', ...
              'full','same'};

for k = 1:length(varargin)
  if ischar(varargin{k})
    string = validatestring(varargin{k}, allStrings,...
                          mfilename, 'OPTION',k+2);
    switch string
     case {'replicate', 'symmetric', 'circular'}
      boundary = string;
     case {'full','same'}
      output = string;
     case {'conv','corr'}
      do_fcn = string;
    end
  else
    validateattributes(varargin{k},{'numeric'},{'nonsparse'},mfilename,'OPTION',k+2);
    boundary = varargin{k};
  end %else
end

sameSize = strcmp(output,'same');

convMode = strcmp(do_fcn,'conv');

%--------------------------------------------------------------
function separable = isSeparable(a, h)

% check for filter separability
sep_threshold = getSeparableFilterThreshold(class(a));

if ((numel(h) >= sep_threshold) && ...
    (ismatrix(h)) && ...
    all(size(h) ~= 1) && ...
    all(isfinite(h(:))))

  [~,s,~] = svd(h);
  s = diag(s);
  tol = length(h) * max(s) * eps;
  rank = sum(s > tol);

  if (rank == 1)
    separable = true;
  else
    separable = false;
  end

else

  separable = false;

end

%--------------------------------------------------------------
function b = handleEmptyImage(a, sameSize, im_size)

if (sameSize)

  b = a;

else

  if all(im_size >= 0)

    b = zeros(im_size, class(a));

  else

    error(message('images:imfilter:negativeDimensionBadSizeB'))

  end

end

%--------------------------------------------------------------
function b = handleEmptyFilter(a, sameSize, im_size)

if (sameSize)

    b = zeros(size(a), class(a));

else

  if all(im_size>=0)

    b = zeros(im_size, class(a));

  else

    error(message('images:imfilter:negativeDimensionBadSizeB'))

  end

end

%--------------------------------------------------------------
function ippFlag = useIPPL(a,outSize,h,nonzero_h)

% Querying the Java preference for IPP use is computationally expensive,
% particularly in cases where imfilter is called in a loop. As a
% performance optimization, store the preference as persistent state. This
% state is reset whenever iptsetpref('UseIPPL',___) is called.
persistent prefFlag;
needToQueryJavaPreference = isempty(prefFlag);
if needToQueryJavaPreference
    prefFlag = iptgetpref('UseIPPL');
end

%We are disabling the use of IPP for double precision inputs on win32.
if ~isImageIPPFilterType(class(a))
    ippFlag = false;
    return;
end

if numel(nonzero_h)/numel(h) > 0.05
    densityFlag = true;
else
    densityFlag = false;
end

hDimsFlag = ismatrix(h);

% Determine if the image is big depending on datatype
tooBig = isImageTooBigForIPPFilter(a, outSize);

ippFlag = prefFlag && densityFlag && hDimsFlag && (~tooBig);

%--------------------------------------------------------------
function [finalSize, pad] = computeSizes(a, h, sameSize)

rank_a = ndims(a);
rank_h = ndims(h);

% Pad dimensions with ones if filter and image rank are different
size_h = [size(h) ones(1,rank_a-rank_h)];
size_a = [size(a) ones(1,rank_h-rank_a)];

if (sameSize)
  %Same output
  finalSize = size_a;

  %Calculate the number of pad pixels
  filter_center = floor((size_h + 1)/2);
  pad = size_h - filter_center;
else
  %Full output
  finalSize = size_a+size_h-1;
  pad = size_h - 1;
end

%--------------------------------------------------------------
function result = filterPartOrWhole(a, outSize, h, start, sameSize, convMode)

% Create connectivity matrix.  Only use nonzero values of the filter.
conn = h~=0;
nonzero_h = h(conn);

ippFlag  = useIPPL(a,outSize,h,nonzero_h);

if (isreal(h))

  result = imfilter_mex(a, outSize, h, nonzero_h,...
                        conn, start, sameSize, convMode, ippFlag);

else

  b1 = imfilter_mex(a, outSize, real(h), real(nonzero_h),...
                    conn, start, sameSize, convMode, ippFlag);

  b2 = imfilter_mex(a, outSize, imag(h), imag(nonzero_h), ...
                    conn, start, sameSize, convMode, ippFlag);

  if isreal(a)

    % b1 and b2 will always be real; result will always be complex
    result = complex(b1, b2);

  else

    % b1 and/or b2 may be complex;
    % result will always be complex
    result = complex(real(b1) - imag(b2),...
                     imag(b1) + real(b2));

  end

end

%--------------------------------------------------------------
function a = padImage(a,padSize,padding)

if ischar(padding)
    method = padding;
    padVal = [];
else
    method = 'constant';
    padVal = padding;
end

a = padarray_algo(a, padSize, method, padVal, 'both');

repo: Deep-ADMM-Net


text:
% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% dslam_open is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with dslam_open. If not, see <http://www.gnu.org/licenses/>.

function plot3DSubGraph(graph, values, linespec, colorspec, markerspec)
%PLOTGRAPH Plots the Pose3's in a values, with optional covariances
%   Finds all the Pose2 objects in the given Values object and plots the graph.
% If a Marginals object is given, this function will also plot marginal
% covariance ellipses for each pose.

import gtsam.*

if ~exist('linespec', 'var') || isempty(linespec)
    linespec = '-';
end

if ~exist('colorspec', 'var') || isempty(colorspec)
    colorspec = 'r';
end

if ~exist('markerspec', 'var') || isempty(markerspec)
    markerspec = '';
end

holdstate = ishold;
hold on

L = {linespec};
% Draw graph
for i = 0:graph.size - 1
    factor = graph.at(i);
    if size(factor.keys) == 2
        key1 = factor.keys.at(0);
        key2 = factor.keys.at(1);

        try
        pose1 = values.atPose3(key1);
        pose2 = values.atPose3(key2);
        % draw a line - do not show loop closures if showLoopClosures is
        % false
        key1_sym = symbolChr(key1);
        key2_sym = symbolChr(key2);
        key1_index = symbolIndex(key1);
        key2_index = symbolIndex(key2);

        if key1_sym ~= key2_sym
            plot([pose1.x, pose2.x], [pose1.z, pose2.z], 'Linestyle', '-', 'Color', 'k', 'Marker', markerspec, 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'r', 'alpha', 0.5)
        else
            plot([pose1.x, pose2.x], [pose1.z, pose2.z], 'Linestyle', '-o', 'Color', colorspec)
        end

        catch
        end
    end
end

if ~holdstate
    hold off
end

end

author: uzh-rpg


text:
filename='Cylinder_Tetrahedra_Linear_Unstructured';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'cylinder';
fracRadius = 1-1e-6;
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
designVariable = 'LevelSet';
incrementFactor = 1;
filterType = 'PDE';

nsteps = 1;
Vfrac_final = 1;
Perimeter_target=3.5;
optimality_final =1e-5;
constr_final =1e-5;

BCscale_factor = 0.3;
HJiter0 = 1;
e2 = 5;

Vfrac_initial = 1;
optimality_initial = 5e-2;
constr_initial = 5e-2;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

% maxiter = 10;
% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;

plotting = false;
printing = false;
monitoring = false;
monitoring_interval = 0;

author: SwanLab

repo: Swan

save_path: github-repos/MATLAB/SwanLab-Swan


text:
function [dvh,qi] = matRad_indicatorWrapper(cst,pln,resultGUI,refGy,refVol)
% matRad indictor wrapper
%
% call
%   [dvh,qi] = matRad_indicatorWrapper(cst,pln,resultGUI)
%   [dvh,qi] = matRad_indicatorWrapper(cst,pln,resultGUI,refGy,refVol)
%
% input
%   cst:                  matRad cst struct
%   pln:                  matRad pln struct
%   resultGUI:            matRad resultGUI struct
%   refGy: (optional)     array of dose values used for V_XGy calculation
%                         default is [40 50 60]
%   refVol:(optional)     array of volumes (0-100) used for D_X calculation
%                         default is [2 5 95 98]
%                         NOTE: Call either both or none!
%
% output
%   dvh: matRad dvh result struct
%   qi:  matRad quality indicator result struct
%   graphical display of all results
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2017 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isfield(resultGUI,'RBExDose')
    doseCube = resultGUI.RBExDose;
else
    doseCube = resultGUI.physicalDose;
end

if ~exist('refVol', 'var')
    refVol = [];
end

if ~exist('refGy', 'var')
    refGy = [];
end

dvh = matRad_calcDVH(cst,doseCube,'cum');
qi  = matRad_calcQualityIndicators(cst,pln,doseCube,refGy,refVol);

figure,set(gcf,'Color',[1 1 1]);
subplot(2,1,1)
matRad_showDVH(dvh,cst,pln);
subplot(2,1,2)
ixVoi = cellfun(@(c) c.Visible == 1,cst(:,5));
qi = qi(ixVoi);
matRad_showQualityIndicators(qi);

repo: matRad


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

% We test the performance of the inverse method using numerical
% algorithms based on the Kienitz extrapolation methodology
% for generating MC densities and we compare against analytic values

% Generate random numbers from a SABR density using numerical inversion
% of the cumulated probability and the density truncation method

clear; clc;
NSim = 100000;          % number of simulations
U = rand(1,NSim);       % uniforms

% sabr parameters
t =1;
% a = 0.25; b = 0.5; r = -.5;  f = 0.03;  n = 0.2;
% b= 0.5; r=-0.1595; f=0.0495;  n=0.3843;  a=0.1339 * f^(1-b);
b = 0.5;n=0.2;r=-0.2;f=0.03; a=0.9*f^(1-b);
% calibrate the parameters
[kl, mu, cl, bl, al, ku, nu, cu, bu, au] = ...
    psabr_param(a, b, r, n, f, 0.0001:0.0001:0.2, t,4);

% apply the calibrated density
d = @(x) psabr_5(a, b, r, n, f, x, t, ...
    kl, ku, mu, cl, bl, al, nu, cu, bu, au);

x_sabr = 0.001:0.0001:1;                % x_values for table
tic;
y_sabr = FSABR2_1(x_sabr,d);            % values for the table
US = FInvSABR4_2(U,x_sabr, y_sabr);     % inversion using table
toc

nbins = 150;
Nzero = sum(US==1);
Pzero = Nzero / NSim;
[n,xout] = hist(US,nbins);                % calc histogram
n(end) = 0;
pdensity = n/NSim*nbins; cdensity = cumsum(n) / NSim;

n(end) = 0;                             % remove artefact
figure;bar(n/nbins);                    % plot histogram
x = 0:.0001:1;                          % x values for plot
y = d(x);                               % calculate the density
Y = sum(y)*0.0001;
y(1) = 1-Y;
figure('Color', [1 1 1]);
hold on; plot([0 xout],[Pzero pdensity],'ro-'); plot(x,y); xlim([0,.2]); hold off; % plot sim against value
xlabel('Strike'); ylabel('Density p(x)');
x = [0 xout];                          % x values for plot
y = d(x);

figure('Color', [1 1 1]);
hold on; plot(x,[Pzero pdensity],'ro-'); plot(x,y,'gx-'); xlim([0,.25]); hold off; % plot sim against value

figure('Color', [1 1 1]);
plot(x,([Pzero pdensity] - y)./y); xlim([0,.25]); hold off; % plot sim against value

author: Sable

repo: mcbench-benchmarks


text:
% Intrinsic and Extrinsic Camera Parameters
%
% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.
% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.
%            All those complementary variables are saved in the complete matlab data file Calib_Results.mat.
% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/

%-- Focal length:
fc = [ 1588.853342543796771 ; 1589.141065148706048 ];

%-- Principal point:
cc = [ 336.852104684709388 ; 166.220483494858200 ];

%-- Skew coefficient:
alpha_c = 0.000000000000000;

%-- Distortion coefficients:
kc = [ -0.183448717338282 ; -0.034396151458065 ; -0.005473481906443 ; 0.004392952805618 ; 0.000000000000000 ];

%-- Focal length uncertainty:
fc_error = [ 31.801580904116800 ; 29.314132245923947 ];

%-- Principal point uncertainty:
cc_error = [ 49.273756028476519 ; 39.985395373378047 ];

%-- Skew coefficient uncertainty:
alpha_c_error = 0.000000000000000;

%-- Distortion coefficients uncertainty:
kc_error = [ 0.066741056034172 ; 0.873747062658826 ; 0.006527073109278 ; 0.004504614774849 ; 0.000000000000000 ];

%-- Image size:
nx = 640;
ny = 480;

%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):
%-- Those variables are used to control which intrinsic parameters should be optimized

n_ima = 15;						% Number of calibration images
est_fc = [ 1 ; 1 ];					% Estimation indicator of the two focal variables
est_aspect_ratio = 1;				% Estimation indicator of the aspect ratio fc(2)/fc(1)
center_optim = 1;					% Estimation indicator of the principal point
est_alpha = 0;						% Estimation indicator of the skew coefficient
est_dist = [ 1 ; 1 ; 1 ; 1 ; 0 ];	% Estimation indicator of the distortion coefficients

%-- Extrinsic parameters:
%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties

%-- Image #1:
omc_1 = [ -1.557681e+00 ; -2.205804e+00 ; 1.330361e+00 ];
Tc_1  = [ 1.734034e+02 ; 5.794926e+01 ; 2.044993e+03 ];
omc_error_1 = [ 2.933522e-02 ; 2.081152e-02 ; 3.616192e-02 ];
Tc_error_1  = [ 6.355186e+01 ; 5.156988e+01 ; 3.664085e+01 ];

%-- Image #2:
omc_2 = [ -1.702089e+00 ; -2.256995e+00 ; 1.267502e+00 ];
Tc_2  = [ -6.722390e+01 ; 5.941507e+01 ; 2.057353e+03 ];
omc_error_2 = [ 3.035591e-02 ; 2.055681e-02 ; 3.795895e-02 ];
Tc_error_2  = [ 6.380940e+01 ; 5.186705e+01 ; 3.924807e+01 ];

%-- Image #3:
omc_3 = [ -1.701930e+00 ; -2.253495e+00 ; 1.271626e+00 ];
Tc_3  = [ -2.657159e+02 ; 5.535306e+01 ; 2.111954e+03 ];
omc_error_3 = [ 3.119852e-02 ; 2.203487e-02 ; 3.792260e-02 ];
Tc_error_3  = [ 6.555459e+01 ; 5.345885e+01 ; 4.297524e+01 ];

%-- Image #4:
omc_4 = [ 1.893838e+00 ; 1.951405e+00 ; -6.883375e-01 ];
Tc_4  = [ 1.355395e+02 ; 6.304476e+01 ; 2.007853e+03 ];
omc_error_4 = [ 1.462647e-02 ; 2.105632e-02 ; 3.955605e-02 ];
Tc_error_4  = [ 6.234890e+01 ; 5.068514e+01 ; 3.911636e+01 ];

%-- Image #5:
omc_5 = [ 1.908480e+00 ; 1.850529e+00 ; -5.507656e-01 ];
Tc_5  = [ -9.864709e+01 ; 6.448043e+01 ; 2.021515e+03 ];
omc_error_5 = [ 1.405146e-02 ; 2.113590e-02 ; 3.862142e-02 ];
Tc_error_5  = [ 6.272370e+01 ; 5.106967e+01 ; 4.131143e+01 ];

%-- Image #6:
omc_6 = [ 1.889738e+00 ; 1.980019e+00 ; -7.390048e-01 ];
Tc_6  = [ -3.575459e+02 ; 6.164224e+01 ; 2.073652e+03 ];
omc_error_6 = [ 1.076545e-02 ; 2.434070e-02 ; 4.112093e-02 ];
Tc_error_6  = [ 6.427989e+01 ; 5.275973e+01 ; 4.590561e+01 ];

%-- Image #7:
omc_7 = [ 1.904119e+00 ; 1.613811e+00 ; -2.469117e-01 ];
Tc_7  = [ 1.050648e+02 ; 6.953312e+01 ; 1.959883e+03 ];
omc_error_7 = [ 1.877046e-02 ; 1.939786e-02 ; 3.660657e-02 ];
Tc_error_7  = [ 6.086765e+01 ; 4.946251e+01 ; 3.876840e+01 ];

%-- Image #8:
omc_8 = [ 1.874544e+00 ; 1.387578e+00 ; 5.963331e-03 ];
Tc_8  = [ -1.316064e+02 ; 7.296929e+01 ; 1.962026e+03 ];
omc_error_8 = [ 1.991265e-02 ; 1.966408e-02 ; 3.477026e-02 ];
Tc_error_8  = [ 6.097585e+01 ; 4.958890e+01 ; 3.966239e+01 ];

%-- Image #9:
omc_9 = [ 1.867279e+00 ; 1.338834e+00 ; 5.192820e-02 ];
Tc_9  = [ -3.848199e+02 ; 7.244019e+01 ; 2.003159e+03 ];
omc_error_9 = [ 1.905893e-02 ; 2.014607e-02 ; 3.409265e-02 ];
Tc_error_9  = [ 6.267821e+01 ; 5.108076e+01 ; 4.288011e+01 ];

%-- Image #10:
omc_10 = [ -1.715796e+00 ; -2.041805e+00 ; 9.303852e-01 ];
Tc_10  = [ 1.012077e+02 ; 7.501598e+01 ; 1.710616e+03 ];
omc_error_10 = [ 2.385211e-02 ; 2.111071e-02 ; 3.698204e-02 ];
Tc_error_10  = [ 5.316514e+01 ; 4.311025e+01 ; 3.065417e+01 ];

%-- Image #11:
omc_11 = [ -1.805616e+00 ; -2.089567e+00 ; 8.651741e-01 ];
Tc_11  = [ -6.268099e+01 ; 8.236545e+01 ; 1.672806e+03 ];
omc_error_11 = [ 2.455832e-02 ; 2.181052e-02 ; 3.822892e-02 ];
Tc_error_11  = [ 5.190998e+01 ; 4.218558e+01 ; 3.143920e+01 ];

%-- Image #12:
omc_12 = [ -1.897248e+00 ; -2.132223e+00 ; 7.958098e-01 ];
Tc_12  = [ -2.617251e+02 ; 8.772964e+01 ; 1.655251e+03 ];
omc_error_12 = [ 2.597201e-02 ; 2.298575e-02 ; 3.987007e-02 ];
Tc_error_12  = [ 5.154881e+01 ; 4.202592e+01 ; 3.365145e+01 ];

%-- Image #13:
omc_13 = [ 2.114980e+00 ; 2.117562e+00 ; -3.547934e-01 ];
Tc_13  = [ 3.651036e+01 ; 1.009631e+02 ; 1.516712e+03 ];
omc_error_13 = [ 1.733561e-02 ; 1.778940e-02 ; 4.120690e-02 ];
Tc_error_13  = [ 4.712996e+01 ; 3.839118e+01 ; 3.001941e+01 ];

%-- Image #14:
omc_14 = [ 2.128866e+00 ; 2.219968e+00 ; -5.388805e-01 ];
Tc_14  = [ -6.343915e+01 ; 1.021354e+02 ; 1.517822e+03 ];
omc_error_14 = [ 1.478417e-02 ; 2.098690e-02 ; 4.310996e-02 ];
Tc_error_14  = [ 4.710837e+01 ; 3.841050e+01 ; 2.980588e+01 ];

%-- Image #15:
omc_15 = [ 2.121751e+00 ; 2.168024e+00 ; -4.694731e-01 ];
Tc_15  = [ -2.722483e+02 ; 1.012912e+02 ; 1.548764e+03 ];
omc_error_15 = [ 1.690688e-02 ; 2.488503e-02 ; 4.454520e-02 ];
Tc_error_15  = [ 4.814071e+01 ; 3.949586e+01 ; 3.320000e+01 ];

author: zhixy

repo: Laser-Camera-Calibration-Toolbox


text:
%DstarPO D*-PO navigation class
%
% A concrete subclass of the Navigation class that implements the D*
% navigation algorithm; facilitates incremental replanning. This
% implementation of D* is intended for multiobjective optimization (MOO)
% problems - i.e. optimizes over several objectives/criteria - with the use
% of Pareto fronts (see Lavin paper).
%
% Methods::
% plan              Compute the cost map given a goal and map
% path              Compute a path to the goal
% visualize         Display the obstacle map (deprecated)
% plot              Display the obstacle map
% cost_get          Return the specified cost layer
% costmap_modify    Modify the costmap
% modify_cost       Modify the costmap (deprecated, use costmap_modify)
% costmap_get       Return the current costmap
% costmap_set       Set the current costmap
% distancemap_get   Set the current distance map
% display           Print the parameters in human readable form
% char              Convert to string
%
% Properties::
% TBD
%
% Example::
%        load map1              % load map
%        goal = [50,30];
%        start=[20,10];
%        ds = DstarPO(map);     % create navigation object
%        ds.plan(goal,1)        % create plan for specified goal
%        ds.path(start)         % animate path from this start location
% Example 2:
%       goal = [100;100];
%       start = [1;1];
%       ds = DstarPO(0);        % create Navigation object with random occupancy grid
%       ds.addCost(1,L);        % add 1st add'l cost layer L
%       ds.plan(goal,2);        % setup costmap for specified goal
%       ds.path(start);         % plan solution path start-goal, animate
%       P = as.path(start);     % plan solution path start-goal, return path
%
% Notes::
% - Obstacles are represented by Inf in the costmap.
%
% References::
% - The D* algorithm for real-time planning of optimal traverses,
%   A. Stentz, Tech. Rep. CMU-RI-TR-94-37, The Robotics Institute,
%   Carnegie-Mellon University, 1994.
% - A Pareto Optimal D* Search Algorithm for Multiobjective Path Planning,
%   A. Lavin.
% - Robotics, Vision & Control, Sec 5.2.2,
%   Peter Corke, Springer, 2011.
%
% Author::
% Alexander Lavin based on Dstar by Peter Corke
%
% See also Navigation, Dstar, DstarMOO, Astar, DXform.

% Copyright (C) 1993-2015, by Peter I. Corke, Alexander Lavin
%
% 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
%
% This is an original algorithm written by Alexander Lavin.
% http://alexanderlavin.com

% Implementation notes:
%
% All the state is kept in the structure called d
% X is an index into the array of states.
% state pointers are kept as matlab array index rather than row,col format

classdef DstarPO < Navigation

    properties (SetAccess=private, GetAccess=private)

        costmap   % world cost map: obstacle = Inf
        G         % index of goal point
        N         % number of objectives

        % info kept per cell (state)
        b       % backpointer (0 means not set)
        t       % tag: NEW OPEN CLOSED

        cost_g       % path distance summation
        cost_h       % path heuristic (state to goal) cost
        cost_01      % add'l cost layer 01 (unused)
        cost_02      % add'l cost layer 02 (unused)
        cost_03      % add'l cost layer 03 (unused)
                     % add more cost layers if needed...

        priority
        tie

        % list of open states: 2xN matrix
        %   each open point is a column, row 1 = index of cell, row 2 = k
        openlist
        niter

        changed

        openlist_maxlen     % keep track of maximum length
        quiet

        % tag state values
        NEW = 0;
        OPEN = 1;
        CLOSED = 2;
    end

    methods

        % constructor
        function ds = DstarPO(world, varargin)
            %DstarPO.DstarPO D*-PO constructor
            %
            % DS = Dstar(MAP, OPTIONS) is a D* navigation object, and MAP is an
            % occupancy grid, a representation of a planar world as a
            % matrix whose elements are 0 (free space) or 1 (occupied).
            % The occupancy grid is coverted to a costmap with a unit cost
            % for traversing a cell.
            %
            % Options::
            % 'goal',G      Specify the goal point (2x1)
            % 'metric',M    Specify the distance metric as 'euclidean' (default)
            %               or 'cityblock'.
            % 'inflate',K   Inflate all obstacles by K cells.
            % 'quiet'       Don't display the progress spinner
            %
            % Other options are supported by the Navigation superclass.
            %
            % Notes::
            % - If MAP == 0 a random map is created.
            %
            % See also Navigation.Navigation.

            % invoke the superclass constructor
            ds = ds@Navigation(world, varargin{:});

            opt.quiet = false;
            opt = tb_optparse(opt, varargin);
            ds.quiet = opt.quiet;

            ds.occgrid2costmap(ds.occgrid);

            % init the D* state variables
            ds.reset();
            if ~isempty(ds.goal)
                ds.goal_change();
            end
            ds.changed = false;
        end

        function reset(ds)
            %DstarPO.reset Reset the planner
            %
            % DS.reset() resets the D* planner.  The next instantiation
            % of DS.plan() will perform a global replan.

            % build the matrices required to hold the state of each cell for D*
            ds.b = zeros(size(ds.costmap), 'uint32');     % backpointers
            ds.t = zeros(size(ds.costmap), 'uint8');      % tags
            ds.cost_g = Inf*ones(size(ds.costmap));       % path cost estimate
            ds.openlist = zeros(2,0);                     % the open list, one column per point

            ds.openlist_maxlen = -Inf;
        end

        function goal_change(ds)

            if isempty(ds.b)
                return;
            end
            goal = ds.goal;

            % keep goal in index rather than row,col format
            ds.G = sub2ind(size(ds.occgrid), goal(2), goal(1));
            ds.INSERT(ds.G, ds.projectCost(ds.G), 'goalset');
            ds.cost_g(ds.G) = 0;

            % new goal changes cost layers:
            ds.calcHeuristic(ds.occgrid, ds.goal);
        end

        function s = char(ds)
            %DstarPO.char Convert navigation object to string
            %
            % DS.char() is a string representing the state of the Dstar
            % object in human-readable form.
            %
            % See also Dstar.display, Navigation.char.

            % most of the work is done by the superclass
            s = char@Navigation(ds);

            % Dstar specific stuff
            if ~isempty(ds.costmap)
                s = char(s, sprintf('  costmap: %dx%d, open list %d', size(ds.costmap), numcols(ds.openlist)));
            else
                s = char(s, sprintf('  costmap: empty:'));
            end
        end

        function plot(ds, varargin)
            %DstarPO.plot Visualize navigation environment
            %
            % DS.plot() displays the occupancy grid and the goal distance
            % in a new figure.  The goal distance is shown by intensity which
            % increases with distance from the goal.  Obstacles are overlaid
            % and shown in red.
            %
            % DS.plot(P) as above but also overlays a path given by the set
            % of points P (Mx2).
            %
            % See also Navigation.plot.

            plot@Navigation(ds, 'distance', ds.cost_h, varargin{:});
        end

        % invoked by Navigation.step
        function n = next(ds, current)

            if ds.changed
                error('Cost map has changed, replan');
            end
            X = sub2ind(size(ds.costmap), current(2), current(1));
            X = ds.b(X);
            if X == 0
                n = [];
            else
                [r,c] = ind2sub(size(ds.costmap), X);
                n = [c;r];
            end
        end

        function plan(ds, goal, N)
            %DstarPO.plan Plan path to goal
            %
            % DS.plan() updates DS with a costmap of distance to the
            % goal from every non-obstacle point in the map.  The goal is
            % as specified to the constructor.
            %
            % DS.plan(GOAL) as above but uses the specified goal.
            %
            % Note::
            % - If a path has already been planned, but the costmap was
            %   modified, then reinvoking this method will replan,
            %   incrementally updating the plan at lower cost than a full
            %   replan.
            %
            % Inputs:
            %   goal: goal state coordinates
            %   N: number of optimization objectives; standard D* is 2
            %   (i.e. distance and heuristic)

            ds.N = N; % number of optimization objectives
            ds.openlist = zeros(ds.N+1,0);

            % Setup cost layers. If a
            % cost layer is goal-dependent, it's setup function needs to
            % also be called in DS.goal_change(). If more cost layers are
            % needed, add similar to DS.cost_01.

            % initializations first:
            ds.cost_g = zeros(size(ds.occgrid));
            ds.cost_h = zeros(size(ds.occgrid)); % filled after setting goal below
            % if add'l costs haven't been added with addCost()
            if isempty(ds.cost_01)
                ds.cost_01 = zeros(size(ds.occgrid));
            end
            if isempty(ds.cost_02)
                ds.cost_02 = zeros(size(ds.occgrid));
            end
            if isempty(ds.cost_03)
                ds.cost_03 = zeros(size(ds.occgrid));
            end

            if nargin > 1
                ds.goal = goal; % invokes superclass method set.goal()
            end
            % for replanning no goal is needed,
            if isempty(ds.goal)
                error('must specify a goal point');
            end

            % Setup cost layers DS.cost_g and DS.cost_h.
            % assign values to the distance cost layer, set as DS.costmap
            ds.occgrid2costmap(ds.occgrid);
            % assign values to the heuristic cost layer, set as DS.cost_h
            ds.calcHeuristic(ds.occgrid, ds.goal);
            % Additional cost layers are added by the user with the
            % DS.addCost() method

            % Cost priority/tiebreaker: cost_g (distance to node)
            ds.priority = ds.cost_g;
            ds.tie = 1; % first cost: cost_g

            ds.niter = 0;
            while true
                if ~ds.quiet && mod(ds.niter, 20) == 0
                    ds.spinner();
                end
                ds.niter = ds.niter + 1;

                if ds.PROCESS_STATE() < 0
                    break;
                end
                if ds.verbose
                    disp(' ')
                end
            end
            if ~ds.quiet
                fprintf('\r');
            end
            ds.changed = false;
        end

        function layer = cost_get(ds)
        %DstarPO.cost_get Get the specified cost layer
            layer = ds.cost_02;
        end

        function c = distancemap_get(ds)
        %DstarPO.distancemap_get Get the current distance map
        %
        % C = DS.distancemap_get() is the current distance map.  This map is the same size
        % as the occupancy grid and the value of each element is the shortest distance
        % from the corresponding point in the map to the current goal.  It is computed
        % by Dstar.plan.
        %
        % See also Dstar.plan.
            c = ds.cost_h; % heuristic
        end

        function c = costmap_get(ds)
        %DstarPO.costmap_get Get the current costmap
        %
        % C = DS.costmap_get() is the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  It is autogenerated by the class constructor from
        % the occupancy grid such that:
        % - free cell (occupancy 0) has a cost of 1
        % - occupied cell (occupancy >0) has a cost of Inf
        %
        % See also Dstar.costmap_set, Dstar.costmap_modify.

            c = ds.costmap;
        end

        function costmap_set(ds, costmap)
        %DstarPO.costmap_set Set the current costmap
        %
        % DS.costmap_set(C) sets the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  A high value indicates that the cell is more costly
        % (difficult) to traverese.  A value of Inf indicates an obstacle.
        %
        % Notes::
        % - After the cost map is changed the path should be replanned by
        %   calling DS.plan().
        %
        % See also Dstar.costmap_get, Dstar.costmap_modify.
            if ~all(size(costmap) == size(ds.occgrid))
                error('costmap must be same size as occupancy grid');
            end
            ds.costmap = costmap;
            ds.changed = true;
        end

        function costmap_modify(ds, point, newcost)
        %DstarPO.costmap_modify Modify cost map
        %
        % DS.costmap_modify(P, NEW) modifies the cost map at P=[X,Y] to
        % have the value NEW.  If P (2xM) and NEW (1xM) then the cost of
        % the points defined by the columns of P are set to the corresponding
        % elements of NEW.
        %
        % Notes::
        % - After one or more point costs have been updated the path
        %   should be replanned by calling DS.plan().
        % - Replaces modify_cost, same syntax.
        %
        % See also Dstar.costmap_set, Dstar.costmap_get.

            if numel(point) == 2
                % for case of single point ensure it is a column vector
                point = point(:);
            end
            if numcols(point) ~= numcols(newcost)
                error('number of columns in point must match columns in newcost');
            end
            for i=1:numcols(point)
                X = sub2ind(size(ds.costmap), point(2,i), point(1,i));
                ds.costmap(X) = newcost(i);
            end
            if ds.t(X) == ds.CLOSED
                ds.INSERT(X, ds.h(X), 'modifycost');
            end
            ds.changed = true;
        end

        function addCost(ds, layer, values)
        %DstarPO.addCost Add an additional cost layer
        %
        % DS.addCost(layer,values) adds the matrix specified by values as a
        % cost layer.
        % Inputs
        %   layer: 1, 2, or 3 to specify which cost layer to add
        %   values: normalized matrix the size of the environment (100x100)
            if size(values)~=size(ds.occgrid)
                display('Layer size does not match the environment')
                return
            end
            if max(max(values))~=1 || min(min(values))~=0
                display('Layer values are not normalized [0:1]')
                return
            end

            if layer==1
                ds.cost_01 = values;
            elseif layer==2
                ds.cost_02 = values;
            elseif layer==3
                ds.cost_03 = values;
            else
                display('Layer index out of range')
            end
        end

    end % public methods

    methods (Access=protected)

        function occgrid2costmap(ds, og, cost)
            if nargin < 3
                cost = 1;
            end
            ds.costmap = og;
            ds.costmap(ds.costmap==1) = Inf;      % occupied cells have Inf driving cost
            ds.costmap(ds.costmap==0) = cost;     % unoccupied cells have driving cost
        end

        function calcHeuristic(ds, grid, goal)
            ds.cost_h=zeros(size(grid));
            for ii=1:size(grid,1)
                for jj=1:size(grid,2)
                    ds.cost_h(ii,jj)=sqrt((ii-goal(1))^2+(jj-goal(2))^2);
                end
            end
        end

        % The main D* function as per the Stentz paper, comments Ln are the original
        % line numbers.
        function r = PROCESS_STATE(d)
            % States with the lowest cost value are removed from the
            % open list

            % Get Pareto optimal point off the open list
            [idx] = paretofront(normc(d.openlist(2:size(d.openlist,1),:)')); % w/ normalization
            front = d.openlist(:,idx);
            [k_old,col] = min(front(d.tie+1,:)); % d.tie specifies the row (cost_g) for tiebreaker
            X = front(1,col); % L1

            if isempty(X) % L2
                r = -1;
                return;
            end

            d.DELETE(X); % L3

            d.priority = d.cost_g; % updates priority cost layer

            if k_old < d.priority(X) % L4
                d.message('k_old < h(X):  %f %f\n', k_old, d.priority(X));
                for Y=d.neighbours(X) % L5
                    if (d.priority(Y) <= k_old) && (d.priority(X) > d.updateCosts(Y,X,0))  % L6
                        d.b(X) = Y;
                        d.updateCosts(X,Y,d.N); %L7
                    end
                end
            end

            % can we lower the path cost of any neighbours?
            if k_old == d.priority(X) % L8
                d.message('k_old == h(X): %f\n', k_old);
                for Y=d.neighbours(X) % L9
                    if (d.t(Y) == d.NEW) || ... % L10-12
                            ( (d.b(Y) == X) && (d.priority(Y) ~= d.updateCosts(Y,X,0)) ) || ...
                            ( (d.b(Y) ~= X) && (d.priority(Y) > d.updateCosts(Y,X,0)) )
                        % Update and project the costs:
                        d.updateCosts(Y,X,d.N);
                        objspace = d.projectCost(Y,X);
                        d.b(Y) = X;
                        d.INSERT(Y, objspace, 'L13'); % L13
                    end
                 end
            else % L14
                d.message('k_old > h(X)');
                for Y=d.neighbours(X) % L15
                    if (d.t(Y) == d.NEW) || ( (d.b(Y) == X) && (d.priority(Y) ~= d.updateCosts(Y,X,0)) )
                        d.updateCosts(Y,X,d.N);
                        objspace = d.projectCost(Y,X);
                        d.b(Y) = X;
                        d.INSERT(Y, objspace, 'L18'); % L18
                    else
                        if ( (d.b(Y) ~= X) && (d.priority(Y) > d.updateCosts(Y,X,0)) )
                            d.INSERT(X, d.projectCost(X), 'L21');                    % L21
                        else
                            if (d.b(Y) ~= X) && (d.priority(X) > d.updateCosts(Y,X,0)) && ...
                                    (d.t(Y) == d.CLOSED) && d.priority(Y) > k_old
                                d.INSERT(Y, d.projectCost(Y), 'L25');                % L25
                            end
                        end
                    end
                 end
            end

            r = 0;
            return;
        end % process_state

        function k_new = updateCosts(ds, a, b, obj)
            % NOTE: Only for costs that accumulate (i.e. sum) over the
            % path, and for dynamic costs.
            % E.g. the heuristic parameter DS.cost_h only needs updating
            % when the goal state changes; it's values are stored for each
            % cell.
            %
            % Location moving from state b to a.
            if nargout > 0
                k_new = ds.cost_g(b) + ds.dc(b,a);
                return
            end
            if obj == 0                        % just return what the new priority cost would be (k_new)
                return
            end
            if obj > 1                         % base case
                ds.cost_g(a) = ds.cost_g(b) + ds.dc(b,a);
                % (no heuristic update needed)
            end
            if obj > 2                         % w/ cost_01: elevation
                % (no elevation update needed)
            end
            if obj > 3                         % w/ cost_02: solar
                sV = [cos(ds.niter/100);sin(ds.niter/100)]; % rotates 1rad per 100 steps
                ds.cost_02(a) = dot(sV,ds.vc(b,a));
            end
            if obj > 4                         % w/ cost_03: risk
                % (no risk update needed)
            end
        end

        function pt = projectCost(ds, a, b)
            % Returns the projection of state a into objective space. If
            % specified, location is moving from b to a.
            switch nargin
                case 2
                    pt = [ds.cost_g(a);
                          ds.cost_h(a);
                          ds.cost_01(a);
                          ds.cost_02(a);
                          ds.cost_03(a);
                          ];
                case 3
                    pt = [ds.cost_g(b) + ds.dc(a,b);
                          ds.cost_h(a);
                          ds.cost_01(a);
                          ds.cost_02(a);
                          ds.cost_03(a);
                          ];
                otherwise
                    return
            end
        end

        function kk = k(ds, X)
            i = ds.openlist(1,:) == X;
            kk = ds.openlist(2, i);
        end

        function INSERT(ds, X, pt, where)
            % Add state X to the openlist with objective space values
            % specified by pt.

            % where is for diagnostic purposes only
            ds.message('insert (%s) %d = %f\n', where, X, pt);

            i = find(ds.openlist(1,:) == X);
            if length(i) > 1
                error('D*:INSERT: state in open list %d times', X);
            end

            if ds.t(X) == ds.NEW
                % add a new column to the open list
                ds.openlist = [ds.openlist [X; pt]];
            elseif ds.t(X) == ds.OPEN
%                 k_new = min( ds.openlist(2,i), h_new );
            elseif ds.t(X) == ds.CLOSED
                if pt(ds.tie) < ds.priority(X) % break tie w/ ds.tie index
                    % add a new column to the open list
                    ds.openlist = [ds.openlist [X; pt]];
                end
            end

            % keep track of the max length of the openlist:
            if numcols(ds.openlist) > ds.openlist_maxlen
                ds.openlist_maxlen = numcols(ds.openlist);
            end

            ds.t(X) = ds.OPEN;
        end

        function DELETE(ds, X)
            ds.message('delete %d\n', X);
            i = find(ds.openlist(1,:) == X);
            if length(i) ~= 1
                error('D*:DELETE: state %d doesnt exist', X);
            end
            ds.openlist(:,i) = []; % remove the column
            ds.t(X) = ds.CLOSED;
        end

        function kmin = GET_KMIN(ds)
            kmin = min(ds.openlist(2,:));
        end

        % return the distance of moving from state X to state Y
        function cost = dc(ds, X, Y)
            [r,c] = ind2sub(size(ds.costmap), [X; Y]);
            dist = sqrt(sum(diff([r c]).^2));
            dcost = (ds.costmap(X) + ds.costmap(Y))/2;

            cost = dist * dcost;
        end

        % return the robot unit vector; direction of moving from state X to state Y
        function vector = vc(ds, X, Y)
            [Xi,Xj] = ind2sub(size(ds.occgrid),X);
            [Yi,Yj] = ind2sub(size(ds.occgrid),Y);
            vector = [Yi-Xi;Yj-Xj];
            vector = vector/norm(vector);
%             slope = vector(2) / vector(1);
%             theta = dot([0,1],[vector])/(norm([0,1])*norm(vector));
        end

        % return index of neighbour states as a row vector
        function Y = neighbours(ds, X)
            dims = size(ds.costmap);
            [r,c] = ind2sub(dims, X);

            % list of 8-way neighbours
            Y = [r-1 r-1 r-1 r r  r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1];
            % only use neighbors w/in grid bounds...
            k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2));
            Y = Y(:,k);
            Y = sub2ind(dims, Y(1,:)', Y(2,:)')';
        end

    end % end of protected methods
end % classdef

author: petercorke

repo: robotics-toolbox-matlab


text:
function [res, inds] = polylineSubcurve(poly, t0, t1)
%POLYLINESUBCURVE Extract a portion of a polyline.
%
%   POLY2 = polylineSubcurve(POLYLINE, POS0, POS1)
%   Create a new polyline, by keeping vertices located between positions
%   POS0 and POS1, and adding points corresponding to positions POS0 and
%   POS1 if they are not already vertices.
%
%   [POLY2, INDS] = polylineSubcurve(POLYLINE, POS0, POS1)
%   Also returns the indices of the original polyline that were selected.
%   The size of the array INDS may be smaller than the array POLY, due to
%   the addition of new vertices at the extremities.
%
%   Example
%     Nv = 100;
%     poly = circleAsPolygon([10 20 30], Nv);
%     poly2 = polylineSubcurve(poly, 15, 65);
%     drawCurve(poly2);
%
%   See also
%     polygons2d, polygonSubCurve
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2009-04-30, using Matlab 7.7.0.471 (R2008b)
% Copyright 2009-2022 INRAE - Cepia Software Platform

% number of vertices
Nv = size(poly, 1);

if t0 < t1
    % format positions
    t0 = max(t0, 0);
    t1 = min(t1, Nv-1);
end

% indices of extreme vertices inside subcurve
ind0 = ceil(t0)+1;
ind1 = floor(t1)+1;

% get the portion of polyline between 2 extremities
if t0 < t1
    inds = ind0:ind1;
else
    inds = [ind0:Nv 1:ind1];
end

res = poly(inds, :);

% add first point if it is not already a vertex
if t0 ~= ind0-1
    res = [polylinePoint(poly, t0); res];
end

% add last point if it is not already a vertex
if t1 ~= ind1-1
    res = [res; polylinePoint(poly, t1)];
end

author: mattools

repo: matGeom

save_path: github-repos/MATLAB/mattools-matGeom


text:
function a=screensnap(b)
%   screensnap is a mex file to snap the screen
%   the input b is double data
%   when b=0,the snapped screen excluding the matlab window
%   else, the snapped screen including the matlab window
%    compile to use>> mex screensnap.c user32.lib gdi32.lib
%   usage:1, >>a=screensnap(0)     %exclude the matlab window
%            >>imshow(a);
%         2, >>a=screensnap(1);    %include the matlab window
%            >> imshow(a);
%   designed by darnshong
%   chenzushang@sina.com  http://darnshong.52blog.net
%   2005-12-18


text:
function map= cmap_solarizedColors(base)
%CMAP_SOLARIZEDCOLORS - Popular colormap 'solarized' by Ethan Schoonover
%
%Synposis:
% MAP= cmap_solarizedColors(<BASE>)
%
%Input:
% BASE: [BOOL] if true, the base colors BASE03, ... are returned as colormap.
%       Otherwise the colors YELLOW, ORANGE, ..., GREEN are returned.
%Output:
% MAP: A colormap matrix of size [M 3]
%
%Example:
% clf; imagesc(toeplitz(1:8)); colorbar;
% colormap(cmap_solarizedColors);
%
%Reference:
%  http://ethanschoonover.com/solarized
%
%See also COLORMAP, HSV2RGB, CMAP_RAINBOW

% 06-2015 Benjamin Blankertz

if nargin>0 && base,
  map= [  0  43  54
          7  54  66
         88 110 117
        101 123 131
        131 148 150
        147 161 161
        238 232 213
        253 246 227]/255;
else
  map= [181 137   0;
        203  75  22;
        220  50  47;
        211  54 130;
        108 113 196;
         38 139 210;
         42 161 152;
        133 153   0]/255;
end

author: bbci


text:
function MatingPool = MatingStrategy(IMatrix)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Jiawei Yuan

Ps                   = 0.7;     % probability of partner selection
[Num,~]              = size(IMatrix);
[~,Neighboors]       = min(IMatrix,[],2);

AllInd               = 1:Num;
SpouseID             = Neighboors;
ChnageInd            = find(rand(1,Num)>Ps);
SpouseID(ChnageInd)  = AllInd(ceil(rand(1,length(ChnageInd))*Num));

AllInd               = reshape(AllInd,1,Num);
SpouseID             = reshape(SpouseID,1,Num);
MatingPool           = [AllInd,SpouseID];
end

author: BIMK

repo: PlatEMO

save_path: github-repos/MATLAB/BIMK-PlatEMO


text:
function [params, names] = linard2KernExtractParam(kern)

% LINARD2KERNEXTRACTPARAM Extract parameters from the LINARD2 kernel structure.
% FORMAT
% DESC Extract parameters from the automatic relevance
% determination linear kernel structure into a vector of parameters
% for optimisation.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
%
% FORMAT
% DESC Extract parameters and parameter names from the automatic relevance
% determination linear kernel structure.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
% RETURN names : cell array of strings containing parameter names.
%
% SEEALSO linard2KernParamInit, linard2KernExpandParam, kernExtractParam, scg, conjgrad
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006
%
% COPYRIGHT : Michalis K. Titsias, 2009

% KERN

params = [kern.inputScales];
if nargout > 1
  %names = {'variance'};
  for i = 1:length(kern.inputScales)
    names{i} = ['input scale ' num2str(i)];
  end
end

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
classdef prtClassOneClassRvm < prtClass
    %prtClassOneClassRvm < prtClass
    % This is a one-class RVM.  It learns a regression RVM to approximate
    % an estimated PDF.  You can set the base RV used to estimate the PDF
    % with the field baseRv.  By default, it's a prtRvKde.
    %
    % ds = prtDataGenUnimodal;
    % ocRvm = prtClassOneClassRvm;
    % ocRvm = train(ocRvm,ds);
    % plot(ocRvm);
    %
    %

    properties (SetAccess=private)
        name = 'One Class RVM'  % Relevance Vector Machine
        nameAbbreviation = 'ocRVM'           % RVM
        isNativeMary = false;  % False
    end
    properties (Dependent)
        kernels
    end

    properties
        useLogPdf = true;
        internalRegressRvm = prtRegressRvm;
        baseRv = prtRvKde;
    end

    methods
        function self = prtClassOneClassRvm(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

        function self = set.kernels(self,kernelVals)
            self.internalRegressRvm.kernels = kernelVals;
        end
        function outKernels = get.kernels(self)
            outKernels = self.internalRegressRvm.kernels;
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)
            %self = trainAction(self,dataSet)

            dsH1 = dataSet.retainClassesByInd(2);

            rv = self.baseRv;
            rv = rv.mle(dsH1);

            if self.useLogPdf
                dsH1Regress = prtDataSetRegress(dsH1.getX,rv.logPdf(dsH1));
            else
                dsH1Regress = prtDataSetRegress(dsH1.getX,rv.pdf(dsH1));
            end

            self.internalRegressRvm = self.internalRegressRvm.train(dsH1Regress);
        end

        function yOut = runAction(self,dataSet)
            yOut = dataSet;

            regressDataSet = prtDataSetRegress(dataSet.getX);
            regressDataSetOut = self.internalRegressRvm.run(regressDataSet);

            if self.useLogPdf
                yOut.X = exp(regressDataSetOut.X);
            else
                yOut.X = regressDataSetOut.X;
            end

        end
    end
end

author: covartech

repo: PRT

save_path: github-repos/MATLAB/covartech-PRT


text:
//Make some assumptions
// let's assume seq_len = 64 for now
// N d_mod d_ff h dk dv
// 6 512   2048 8 64 64
Constant Seq_Len 128;

Network Transformer {
	//Start encoder
	Layer MH_FC_DimReduce_VKQ_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV
		Stride { X: 1, Y: 1 }	// k = 3 * 512, one for each VKQ
		Dimensions { N: Seq_Len, K: 1536, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	Layer SD_MatMul_QK_00 { //Mat mul, batch is 1
		Type: CONV //MatMul -> M(seql)xK(dv)xN(seql)-> filter = Kx1(m chans), input = KxN
		Stride { X: 1, Y: 1 }
		//N=1, K(conv)=M(matr), C=1, R(conv)=K(matr),S=1,Y(conv)=K(matr), X(conv)=N(matr)
		Dimensions { N: 1, K: Seq_Len, C: 1, R: 64, S: 1, Y:64, X:Seq_Len }
		Dataflow {
			SpatialMap(2,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;

		}
	}

	Layer SD_MatMul_V_00 { //Mat mul, batch is 1
		Type: CONV //MatMul -> M(seql)xK(seql)xN(dv)-> filter = Kx1(m chans), input = KxN
		Stride { X: 1, Y: 1 }
		//N=1, K(conv)=M(matr), C=1, R(conv)=K(matr),S=1,Y(conv)=K(matr), X(conv)=N(matr)
		Dimensions { N: 1, K: Seq_Len, C: 1, R: Seq_Len, S: 1, Y:Seq_Len, X:64 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	}

	// done with h parallel sd layers now
	Layer MH_FC_DimRecast_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV
		Stride { X: 1, Y: 1 }	// v,k,q have been combined
		Dimensions { N: Seq_Len, K: 512, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	/// done with h parallel sd layers now
	Layer FF_A_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV //2048 output neurons, 512->2048
		Stride { X: 1, Y: 1 }
		Dimensions { N: Seq_Len, K: 2048, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	/// done with h parallel sd layers now
	Layer FF_B_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV //2048 -> 512
		Stride { X: 1, Y: 1 }
		Dimensions { N: Seq_Len, K: 512, C: 1, R: 1, S: 2048, Y:1, X:2048 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

}

author: maestro-project

repo: maestro


text:
%  Expand a multiple-scan NIFTI file into multiple single-scan NIFTI files
%
%  Usage: expand_nii_scan(multi_scan_filename, [img_idx], [path_to_save])
%
%  NIFTI data format can be found on: http://nifti.nimh.nih.gov
%
%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
function expand_nii_scan(filename, img_idx, newpath)

   v = version;

   %  Check file extension. If .gz, unpack it into temp folder
   %
   if length(filename) > 2 & strcmp(filename(end-2:end), '.gz')

      if ~strcmp(filename(end-6:end), '.img.gz') & ...
	 ~strcmp(filename(end-6:end), '.hdr.gz') & ...
	 ~strcmp(filename(end-6:end), '.nii.gz')

         error('Please check filename.');
      end

      if str2num(v(1:3)) < 7.1 | ~usejava('jvm')
         error('Please use MATLAB 7.1 (with java) and above, or run gunzip outside MATLAB.');
      else
         gzFile = 1;
      end
   end

   if ~exist('newpath','var') | isempty(newpath), newpath = pwd; end
   if ~exist('img_idx','var') | isempty(img_idx), img_idx = 1:get_nii_frame(filename); end

   for i=img_idx
      nii_i = load_untouch_nii(filename, i);

      fn = [nii_i.fileprefix '_' sprintf('%04d',i)];
      pnfn = fullfile(newpath, fn);

      if exist('gzFile', 'var')
         pnfn = [pnfn '.nii.gz'];
      end

      save_untouch_nii(nii_i, pnfn);
   end

   return;					% expand_nii_scan

author: vigente

repo: gerardus

save_path: github-repos/MATLAB/vigente-gerardus


text:
%  <Purpose>
%     Computing the numerical rank of a updated matrix.
%
%  <Syntax>
%     [r,Basis,C] = NumericalRankUpdate(A,pth,vec,C,RC)
%
%  <Input Parameters>
%   1.   A -- the target matrix;
%   2. pth -- the index of row/column to be inserted;
%   3. vec -- the row/column vector to be inserted;
%   4.   C -- cell array contains information required by updating/downdating;
%   5.  RC -- Set to 'row', then the pth row will be inserted.
%             Set to 'column', then the pth column will be inserted.
%
%  <Output Parameters>
%   1. r     -- the numerical rank of the updated matrix;
%   2. Basis --
%          For high rank cases...
%              a matrix whose columns form an orthonormal basis of
%              the numerical kernel;
%
%          For low rank cases...
%              a matrix whose columns form an orthonormal basis of
%              the numerical range;
%
%   3.     C -- Matlab cell array
%
%      For high rank cases...
%          C{1,1} = rank : the numerical rank of the updated matrix;
%          C{2,1} = Basis : matrix whose columns form an orthonormal kernel basis;
%          C{3,1} = Q : the Q in the QR decomposition of the kernel stacked matrix;
%          C{4,1} = R : the R in the QR decomposition of the kernel stacked matrix;
%          C{5,1} = tau : scaling factor in the kernel stacked matrix;
%          C{6,1} = tol : the rank decision threshold;
%
%      For low rank cases...
%          C{1,1} = rank : the numerical rank of the updated matrix;
%          C{2,1} = U : the U in the USV+E decomposition of the updated matrix;
%          C{3,1} = V : the V in the USV+E decomposition of the updated matrix;
%          C{4,1} = S : the S in the USV+E decomposition of the updated matrix;
%          C{5,1} = tol : the rank decision threshold;
%
%  <Reference>
%    [1] T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating, Downdating
%        and Applications", SIAM J. Matrix Anal. and Appl., 26 (2005), pp. 918--946.
%
%    [2] T.L. Lee, T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating,
%        Downdating and Applications, Part II", SIAM J. Matrix Anal. and Appl. (2009).

author: zarathustr

repo: LibQPEP

save_path: github-repos/MATLAB/zarathustr-LibQPEP


text:
function [ SICD_meta, NITF_meta ] = read_sicd_meta( filename )
%READ_SICD_META Read Sensor Independent Complex Data (SICD) metadata from
%file into a MATLAB structure
%
% SICD (version >= 0.3) is stored in a NITF container.  NITF is a
% complicated format that involves lots of fields and configurations
% possibilities. Fortunately, SICD only really uses a small, specific
% portion of the NITF format.  This function extracts only the few parts of
% the NITF metadata necessary for reading a SICD NITF file.
%
% Written by: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Open file
NITF_meta = read_sicd_nitf_offsets(filename);
% SICD Volume 2, File Format Description, section 3.1.1 says that SICD XML
% metadata must be stored in first DES.
SICD_DES_Offset = NITF_meta.minimal.desOffsets(1);
SICD_DES_Length = NITF_meta.minimal.desLengths(1);

%% Read SICD XML metadata from the data extension segment
fid = fopen(filename);
fseek(fid,SICD_DES_Offset,'bof');
SICD_xml_string = fread(fid,SICD_DES_Length,'uint8=>char')';
fclose(fid);
SICD_meta = sicdxml2struct( xmlread( java.io.StringBufferInputStream( SICD_xml_string ) ) );

%% Adjust frequencies in metadata to be true, not offset values, if
% reference frequency is available
if isfield(SICD_meta,'RadarCollection') && isfield(SICD_meta.RadarCollection,'RefFreqIndex') && ...
        SICD_meta.RadarCollection.RefFreqIndex && exist('sicd_ref_freq','file')
    % Get reference frequency from function the user can place in MATLAB path
    SICD_meta = sicd_apply_ref_freq(SICD_meta, sicd_ref_freq());
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

author: ngageoint


text:
 % Right Stance Domain
 %
 % Contact: Right Toe
function domain = RightStance(model, load_path)
    % construct the right stance domain of RABBIT
    %
    % Parameters:
    % model: the right body model of ATLAS robot

    %% first make a copy of the robot model
    %| @note Do not directly assign with the model variable, since it is a
    %handle object.
    domain = copy(model);
    % set the name of the new copy
    domain.setName('RightStance');

    % Extract state variables
    x = domain.States.x;
    dx = domain.States.dx;

    % add contact
    right_sole = ToContactFrame(domain.ContactPoints.RightToe,...
        'PointContactWithFriction');
    fric_coef.mu = 0.6;
    % load symbolic expressions for contact (holonomic) constraints
    domain = addContact(domain,right_sole,fric_coef, [], load_path);

    % add event
    % height of non-stance foot (left toe)
    p_nsf = getCartesianPosition(domain, domain.ContactPoints.LeftToe);
    h_nsf = UnilateralConstraint(domain,p_nsf(3),'leftFootHeight','x');
    % often very simple, no need to load expression. Compute them directly
    domain = addEvent(domain, h_nsf);

    % phase variable: time
    t = SymVariable('t');
    p = SymVariable('p',[2,1]);
    tau = (t-p(2))/(p(1)-p(2));

    % relative degree two outputs:
    y_q1R = x('q1_right');
    y_q2R = x('q2_right');
    y_q1L = x('q1_left');
    y_q2L = x('q2_left');

    ya_2 = [y_q1R;
            y_q2R;
            y_q1L;
            y_q2L];

    y2_label = {'q1_right',...
                'q2_right',...
                'q1_left',...
                'q2_left'};
    % optional: load expression for virtual constraints while creating
    y2 = VirtualConstraint(domain,ya_2,'time','DesiredType','Bezier','PolyDegree',5,...
        'RelativeDegree',2,'OutputLabel',{y2_label},'PhaseType','TimeBased',...
        'PhaseVariable',tau,'PhaseParams',p,'Holonomic',true,...
        'LoadPath', load_path);

    domain = addVirtualConstraint(domain,y2);

end

author: ayonga


text:
function [varargout] = iscolumn(varargin)

% ISCOLUMNN is a drop-in replacement for the same function that was
% introduced in MATLAB R2010b.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% see https://github.com/fieldtrip/fieldtrip/issues/899

alternatives = which(mfilename, '-all');
if ~iscell(alternatives)
  % this is needed for octave, see https://github.com/fieldtrip/fieldtrip/pull/1171
  alternatives = {alternatives};
end

keep = true(size(alternatives));
for i=1:numel(alternatives)
  keep(i) = keep(i) && ~any(alternatives{i}=='@');  % exclude methods from classes
  keep(i) = keep(i) && alternatives{i}(end)~='p';   % exclude precompiled files
end
alternatives = alternatives(keep);

if exist(mfilename, 'builtin') || any(strncmp(alternatives, matlabroot, length(matlabroot)) & cellfun(@isempty, strfind(alternatives, fullfile('private', mfilename))))
  % remove this directory from the path
  p = fileparts(mfilename('fullpath'));
  sw = warning('off','backtrace');
  warning('Removing "%s" from your path.\nSee http://www.fieldtriptoolbox.org/faq/should_i_add_fieldtrip_with_all_subdirectories_to_my_matlab_path/', p);
  warning(sw);
  rmpath(p);
  % call the original MATLAB function
  if exist(mfilename, 'builtin')
    [varargout{1:nargout}] = builtin(mfilename, varargin{:});
  else
    [varargout{1:nargout}] = feval(mfilename, varargin{:});
  end
  return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% this is where the actual replacement code starts
% function tf = iscolumn(x)

% deal with the input arguments
if nargin==1
  [x] = deal(varargin{1:1});
else
  error('incorrect number of input arguments')
end

tf = length(size(x))==2 && size(x,2)==1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% deal with the output arguments

varargout = {tf};

author: spm


text:
function y = simple_fitness(x)
y = 100*(x(1)^2 - x(2))^2 + (1 - x(3))^2 + abs(0.4 - x(2));


text:
image = imread('Training4.png');
trimap = imread('Training4Trimap.png');

% Closed-form matting
a_cf = closedFormMatting(image, trimap);
% Some alternatives:
% a_ifm = informationFlowMatting(image, trimap);
% a_knn = KNNMatting(image, trimap);

% Get the parameter struct and edit for customization if desired
params = getMattingParams('IFM');
params.useKnownToUnknown = 0;
% params.iu_xyw = 0.1;
% params.loc_mult = 3;
a_ifm = informationFlowMatting(image, trimap, params);

% Trim the trimap
trimmed = patchBasedTrimming(image, trimap);
% An alternative:
% trimmed = trimmingFromUnknownToKnownEdges(image, trimap);

% Run K-to-U information flow to get a rough alpha and confidences
[alphaHat, conf] = knownToUnknownColorMixture(image, trimmed);

% Refine alphaHat shared matting
a_sm_ref = sharedMattingMatteRefinement(image, trimmed, alphaHat, conf);
% Alternative:
% a_ifm_ref = informationFlowMatteRefinement(image, trimmed, alphaHat, conf);

author: yaksoy

repo: AffinityBasedMattingToolbox


text:
function [stat, cfg] = ft_statistics_analytic(cfg, dat, design)

% FT_STATISTICS_ANALYTIC performs a parametric statistical test on the data, based on
% a known (i.e. analytic) distribution of the test statistic. This function should
% not be called directly, instead you should call the function that is associated
% with the type of data on which you want to perform the test.
%
% Use as
%   stat = ft_timelockstatistics(cfg, data1, data2, data3, ...)
%   stat = ft_freqstatistics    (cfg, data1, data2, data3, ...)
%   stat = ft_sourcestatistics  (cfg, data1, data2, data3, ...)
%
% where the data is obtained from FT_TIMELOCKANALYSIS, FT_FREQANALYSIS or
% FT_SOURCEANALYSIS respectively, or from FT_TIMELOCKGRANDAVERAGE,
% FT_FREQGRANDAVERAGE or FT_SOURCEGRANDAVERAGE respectively
% and with cfg.method = 'analytic'
%
% The configuration options that can be specified are:
%   cfg.statistic        = string, statistic to compute for each sample or voxel (see below)
%   cfg.correctm         = string, apply multiple-comparison correction, 'no', 'bonferroni', 'holm', 'hochberg', 'fdr' (default = 'no')
%   cfg.alpha            = number, critical value for rejecting the null-hypothesis (default = 0.05)
%   cfg.tail             = number, -1, 1 or 0 (default = 0)
%   cfg.ivar             = number or list with indices, independent variable(s)
%   cfg.uvar             = number or list with indices, unit variable(s)
%   cfg.wvar             = number or list with indices, within-block variable(s)
%
% The parametric statistic that is computed for each sample (and for
% which the analytic probability of the null-hypothesis is computed) is
% specified as
%   cfg.statistic       = 'indepsamplesT'           independent samples T-statistic,
%                         'indepsamplesF'           independent samples F-statistic,
%                         'indepsamplesregrT'       independent samples regression coefficient T-statistic,
%                         'indepsamplesZcoh'        independent samples Z-statistic for coherence,
%                         'depsamplesT'             dependent samples T-statistic,
%                         'depsamplesFmultivariate' dependent samples F-statistic MANOVA,
%                         'depsamplesregrT'         dependent samples regression coefficient T-statistic,
%                         'actvsblT'                activation versus baseline T-statistic.
% or you can specify your own low-level statistical function.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS, FT_SOURCESTATISTICS
% FT_STATISTICS_MONTECARLO, FT_STATISTICS_STATS, FT_STATISTICS_MVPA,
% FT_STATISTICS_CROSSVALIDATE

% Copyright (C) 2006-2015, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% 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$

% do a sanity check on the input data
assert(isnumeric(dat),    'this function requires numeric data as input, you probably want to use FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS instead');
assert(isnumeric(design), 'this function requires numeric data as input, you probably want to use FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS instead');

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'bonferoni', 'bonferroni'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'holms', 'holm'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'correctm', 'none', 'no'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'statfun', 'depsamplesF', 'ft_statfun_depsamplesFmultivariate'});
cfg = ft_checkconfig(cfg, 'renamedval',  {'statfun', 'ft_statfun_depsamplesF', 'ft_statfun_depsamplesFmultivariate'});

% set the defaults
cfg.correctm = ft_getopt(cfg, 'correctm', 'no');
cfg.alpha    = ft_getopt(cfg, 'alpha', 0.05);
cfg.tail     = ft_getopt(cfg, 'tail', 0);

% fetch function handle to the low-level statistics function
statfun = ft_getuserfun(cfg.statistic, 'statfun');
if isempty(statfun)
  ft_error('could not locate the appropriate statistics function');
else
  ft_info('using "%s" for the single-sample statistics\n', func2str(statfun));
end

% tell the low-level statfun to compute the statistic, critical values, and the probability
cfg.computestat    = 'yes';
cfg.computecritval = 'yes';
cfg.computeprob    = 'yes';
% perform the statistical test
if nargout(statfun)>1
  [stat, cfg] = statfun(cfg, dat, design);
else
  [stat] = statfun(cfg, dat, design);
end
% these are only intenced for the low-level statfun, and should not be returned
cfg = rmfield(cfg, 'computestat');
cfg = rmfield(cfg, 'computeprob');
cfg = rmfield(cfg, 'computecritval');

if ~isfield(stat, 'prob')
  ft_warning('probability was not computed');
else
  switch lower(cfg.correctm)
    case 'bonferroni'
      ft_notice('performing Bonferoni correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      stat.mask = stat.prob<=(cfg.alpha ./ numel(stat.prob));
    case 'holm'
      % test the most significatt significance probability against alpha/N, the second largest against alpha/(N-1), etc.
      ft_notice('performing Holm-Bonferroni correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      [pvals, indx] = sort(stat.prob(:));                                   % this sorts the significance probabilities from smallest to largest
      k = find(pvals > (cfg.alpha ./ ((length(pvals):-1:1)')), 1, 'first'); % compare each significance probability against its individual threshold
      mask = (1:length(pvals))'<k;
      stat.mask = zeros(size(stat.prob));
      stat.mask(indx) = mask;
    case 'hochberg'
      % test the most significant significance probability against alpha/N, the second largest against alpha/(N-1), etc.
      ft_notice('performing Hochberg''s correction for multiple comparisons (this is *not* the Benjamini-Hochberg FDR procedure!)\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      [pvals, indx] = sort(stat.prob(:));                                   % this sorts the significance probabilities from smallest to largest
      k = find(pvals <= (cfg.alpha ./ ((length(pvals):-1:1)')), 1, 'last'); % compare each significance probability against its individual threshold
      mask = (1:length(pvals))'<=k;
      stat.mask = zeros(size(stat.prob));
      stat.mask(indx) = mask;
    case 'fdr'
      ft_notice('performing FDR correction for multiple comparisons\n');
      ft_notice('the returned probabilities are uncorrected, the thresholded mask is corrected\n');
      stat.mask = fdr(stat.prob, cfg.alpha);
    case 'no'
      ft_notice('not performing a correction for multiple comparisons\n');
      stat.mask = stat.prob<=cfg.alpha;
    otherwise
      ft_error('unsupported option "%s" for cfg.correctm', cfg.correctm);
  end
end

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function Write_ActuatorPower(FileWritingSetup)
% Write_Power(FileWritingSetup)
%
% FUNCTION:
%
%   This function writes a function that computes the power being used by
%   the actuators on the robot.
%
%
% INPUTS: FileWritingSetup.
%
%   States = [2N x 2] cell array where the first column is the name of
%       the states, and the second column is comments about that states.
%       The index of a state in this array will match the index in the
%       dynamics function to be written. This function assumes a second
%       order system that is being written in first order form, so if
%       States{i,1} = z, then States{i+N,1} = dz; Furthermore, the output
%       (dStates) will have the form: dStates(i,:) = dz,
%       and dStates(i+N,:) = ddz.
%
%   Power = struct with a field for each actuator.
%
%   Actuators = [P x 2] cell array giving the names of each of the
%       actuators and their comments.
%
%   Directory = the name of the directory to write the files in.
%
%
% OUTPUTS:
%   This function will write the function power()
%
%
% NOTE:
%
%   1)  It is assumed that the symbols in all of the inputs are self
%       consistent; that is - if Contacts{1,1} = H1, then there must be a field
%       in Dyn.(FileNames{i,1}) called H1.
%
%
% Written by Matthew Kelly
% December 8, 2013
% Cornell University
%
% See also DERIVE_EOM

States = FileWritingSetup.States;
Power = FileWritingSetup.Power;
Directory = FileWritingSetup.Directory;
Actuators = FileWritingSetup.Actuators;

CurrDir = cd;
if ~exist(Directory,'dir')
    mkdir(Directory);
end
cd(Directory);

fid = fopen('actuatorPower.m','w');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Function Header                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

fprintf(fid,'function Power = actuatorPower(States, Actuators, Phase)\n');
fprintf(fid,'%% function Power = actuatorPower(States, Actuators, Phase)\n');
fprintf(fid,'%%\n%% Computer Generated File -- DO NOT EDIT \n');
fprintf(fid,['%%\n%% This function was created by the function '...
    'Write_Power()\n']);
fprintf(fid,['%% ' datestr(now) '\n%%\n']);
fprintf(fid,'%%  ARGUMENTS: \n');
fprintf(fid,'%%   States = [Nstate x Ntime] matrix of states \n');
fprintf(fid,'%%   Actuator = [Nactuator x Ntime] matrix of actuator values \n');
fprintf(fid,'%%   Phase = {''D'',''F'',''S1'',S2''} or omit. \n');
fprintf(fid,'%%       Since the ankle torques are acting against the ground, they should \n');
fprintf(fid,'%%       be set to zero when a given leg is in flight. If Phase is included, \n');
fprintf(fid,'%%       then this function will force these inputs to be zero. If it is \n');
fprintf(fid,'%%       ommitted, then it is assumed that the user has done that error \n');
fprintf(fid,'%%       checking. \n');
fprintf(fid,'%% \n');
fprintf(fid,'%%  RETURNS: \n');
fprintf(fid,'%%   Power = a struct with fields for each actuator: \n');
fprintf(fid,'%%       leg one (linear force actuator) \n');
fprintf(fid,'%%       leg two (linear force actuator) \n');
fprintf(fid,'%%       ankle one (torque actuator) \n');
fprintf(fid,'%%       ankle two (torque actuator) \n');
fprintf(fid,'%%       hip (torque from leg one on leg two \n');
fprintf(fid,'%% \n');
fprintf(fid,'%%\n');
fprintf(fid,'%% Matthew Kelly \n%% Cornell University \n%% \n\n');
fprintf(fid,'%% \n');
fprintf(fid,'%% See also DERIVE_EOM \n');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                           Parse Inputs                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

for idxState=1:size(States,1)
    fprintf(fid,[States{idxState,1} ' = States(' num2str(idxState) ',:); %% ' ...
        States{idxState,2} '\n']);
end %idxState
fprintf(fid,'\n');

fprintf(fid,'if nargin==2\n');
fprintf(fid,'    Phase = '''';\n');
fprintf(fid,'end\n');
fprintf(fid,' \n');

for idxAct=1:size(Actuators,1)

    if strcmp(Actuators{idxAct,1},'T1')
        fprintf(fid,'if strcmp(Phase,''F'') || strcmp(Phase, ''S2'') \n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = zeros(1,size(Actuators,2)); %%Foot One not in contact with ground!\n']);
        fprintf(fid,'else\n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
        fprintf(fid,'end\n');
    elseif strcmp(Actuators{idxAct,1},'T2')
        fprintf(fid,'if strcmp(Phase,''F'') || strcmp(Phase, ''S1'') \n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = zeros(1,size(Actuators,2)); %%Foot Two not in contact with ground!\n']);
        fprintf(fid,'else\n');
        fprintf(fid,['    ' Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
        fprintf(fid,'end\n');
    else
        fprintf(fid,[Actuators{idxAct,1} ' = Actuators(' num2str(idxAct) ',:); %% ' Actuators{idxAct,2} '\n']);
    end

end %idxAct
fprintf(fid,'\n');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Write Outputs                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

FieldNames = fieldnames(Power);
for i=1:length(FieldNames)
    fprintf(fid,['Power.' FieldNames{i} ' = ' vectorize(Power.(FieldNames{i})) ';\n']);
end
fprintf(fid,'\n');
fprintf(fid,'end\n');
fclose(fid);

cd(CurrDir);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%  ~~~~~~~~~~~~~~~~~~~ Sub Functions  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function characterExpression = vectorize(symbolicExpression)

        characterExpression = char(symbolicExpression);
        characterExpression = strrep(characterExpression,'*','.*');
        characterExpression = strrep(characterExpression,'/','./');
        characterExpression = strrep(characterExpression,'^','.^');
end

author: MatthewPeterKelly

repo: dscTutorials