text:
% [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)
% inputs:
% x is the N x 1 matrix of N unidimensional data points
% l is the N x 1 matrix of N corresponding labels, where l(i) can
% be either -1 or 1
% thresh is the best threshold such that almost everything on one
% side of the threshold is of one class and almost of everything on
% the other side is the other class.
% optional:
% costmatrix is the 2 x 2 matrix. the cost of misclassifying a point with
% label costmatrix(1,1) is costmatrix(1,2) and the cost of misclassifying a
% point with label costmatrix(2,1) is costmatrix(2,2). by default, this is
% [-1 1 ; 1 1]. set to [] to use the default.
% weights is a vector of length n, where w(n) is the weight of the nth
% sample. set to [] to use default w(n) = 1 for all n.
% lower is a scalar. if lower == 1, then class 1 must be below the
% threshold and class -1 must be above. if lower == -1, then class
% -1 must be below the threshold and class 1 must be above. if
% lower == 0, then the best ordering is chosen
function [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)

% parse inputs
n = length(x);
if nargin < 3 || isempty(costmatrix),
  costmatrix = [-1 1 ; 1 1];
else
  if numel(costmatrix) ~= 4 || rows(costmatrix) ~= 2,
    error('costmatrix must be 2 x 2');
  end;
  if abs(costmatrix(1,1)) ~= 1 || abs(costmatrix(2,1)) ~= 1,
    error('costmatrix(1,1) and constmatrix(2,1) can either be 1 or -1');
  end;
  if costmatrix(1,1) == costmatrix(2,1),
    error('costmatrix(1,1) == costmatrix(2,1)');
  end;
end;
if nargin < 4 || isempty(weights),
  weights = ones(n,1);
end

if nargin < 5,
  lower = 0;
else
  if abs(lower) ~= 1 && lower ~= 0,
    error('lower can be -1, 0, or 1');
  end;
end;
x = x(:);
l = l(:);
if length(x) ~= length(l),
  error('x and l must both be N x 1 vectors');
end;
classes = unique(l);
if length(classes) ~= 2,
  error('l must contain exactly 2 unique values, -1 and 1');
end;

% sort the data
% Sort the data based on response value
[sortedx,sortedi] = sort(x);
sortedl = l(sortedi);
sortedweights = weights(sortedi);

% collapse entries of x that are identical
isid = [false;diff(sortedx) == 0];
if any(isid),
  [startid,endid] = get_interval_ends(isid);
  for i = 1:length(startid),
    t0 = startid(i)-1;
    t1 = endid(i)-1;
    tmp = sum(sortedweights(t0:t1).*sortedl(t0:t1));
    sortedweights(t0) = abs(tmp);
    sortedl(t0) = sign(tmp);
  end
  sortedx(isid) = [];
  sortedl(isid) = [];
  sortedweights(isid) = [];
end

if lower == 0,
  [thresh_lowerpos,cost_lowerpos] = main(sortedx, sortedl, costmatrix, sortedweights, 1);
  [thresh_lowerneg,cost_lowerneg] = main(sortedx, sortedl, costmatrix, sortedweights, -1);
  if cost_lowerpos < cost_lowerneg,
    thresh = thresh_lowerpos;
    cost = cost_lowerpos;
    lower = 1;
  else
    thresh = thresh_lowerneg;
    cost = cost_lowerneg;
    lower = -1;
  end;
else
  [thresh,cost] = main(sortedx, sortedl, costmatrix, sortedweights,lower);
end;

function [thresh,cost] = main(x, l, costmatrix, weights, lower)

N = length(x);
labels = l;

l(labels==costmatrix(1,1)) = costmatrix(1,1)*costmatrix(1,2).*weights(labels==costmatrix(1,1));
l(labels==costmatrix(2,1)) = costmatrix(2,1)*costmatrix(2,2).*weights(labels==costmatrix(2,1));
suml1 = cumsum([0;l]);
suml2 = [flipud(cumsum(flipud(l)));0];
if lower == -1,
  ncorrect = -suml1 + suml2;
else
  ncorrect = suml1 - suml2;
end
[maxncorrect,besti] = max(ncorrect);

% Choose threshold to be in the middle
if besti == 1,
  thresh = x(1)-eps;
elseif besti == N+1,
  thresh = x(N)+eps;
else
  thresh = mean(x(besti-1:besti));
end;
cost = -maxncorrect;


text:
%compute dyhead_mm

function [data,units]=compute_dyhead_mm(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
dyhead_mm=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    dyhead_mm{1,i}=(trx(larva).yhead_mm(2:end)-trx(larva).yhead_mm(1:end-1))./trx(larva).dt;
end

units=parseunits('mm/s');
data=dyhead_mm;

author: kristinbranson

repo: JAABA


text:
function [EEGout cfg] = pop_sim_eegdata(EEG,typeproc,varargin)
%
% Simulate EEG data using a dynamical modeling framework and
% a forward head model.
%
% Input:
% Optional:
%
%   EEG:            existing EEG dataset (configs will be retrived from
%                   here)
%   typeproc:       if 'nogui' don't generate GUI
%
%   <'Name',value> pairs as defined in sim_varmodel()
%
% Output:
%
%   EEGout:         Simulated EEG structure(s).
%                   Optionally this may be an array of
%                   EEG structs with the second struct
%                   being the ground truth model.
%   cfg:            Argument specification structure.
%
%
% See Also: sim_varmodel(), sim_eegdata(),
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual.
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift
%
% Author: Tim Mullen 2013, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu
%

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

if nargin<1
    EEG = [];
end
if nargin<2
    typeproc = 0;
end
EEGout = EEG;
cfg    = [];

fcnName     = strrep(mfilename,'pop_','');
fcnHandle   = str2func(fcnName);

if isempty(hlp_checkeegset(EEG,{'cat'})) && isfield(EEG.CAT.configs,fcnName)
    % get default configuration (from prior use) and merge with varargin
    varargin = [hlp_struct2varargin(EEG.CAT.configs.(fcnName)) varargin];
end

if strcmpi(typeproc,'nogui')
    % get the default config from function and overload supplied args
    cfg = arg_tovals(arg_report('rich',fcnHandle,varargin),false);
else
    % render the GUI
    [PGh figh] = feval(['gui_' fcnName],varargin{:});

    if isempty(PGh)
        % user chose to cancel
        return;
    end

    % get the specification of the PropertyGrid
    ps = PGh.GetPropertySpecification;
    cfg = arg_tovals(ps,false);
end

drawnow;

if ~cfg.srcdyn.makeEEGset.arg_selection
    error('SIFT:sim_varmodel',['If using pop_' fcnName '(), you must enable the BuildEEGLABStructure option.\n' ...
                               'Use ' fcnName '() from the command-line to return a raw dataset']);
end

if strcmpi(typeproc,'cfg_only')
    return;
end

% execute the low-level function
[EEGout GroundTruth] = feval(fcnHandle,cfg);

if ~isempty(GroundTruth)
    EEGout = eeg_store(EEGout,GroundTruth,2);
end
if ~isempty(cfg)
    for k=1:length(EEGout)
        % store the configuration structure
        EEGout(k).CAT.configs.(fcnName) = cfg;
    end
end

repo: REST


text:
function [vw, figHandle] = plotEccVsPhase(vw, newfig, colored, drawROI, varargin)
%
% [vw figHandle] = plotEccVsPhase(vw, newfig, co)
%
% Plot of eccentricity versus phase for the current scan, for all
% pixels (in all slices) in the current ROI. You can use this function
% to plot visual field coverage.
%
% This function is kind of a hack, in that it expects that the pRF data are
% loaded as per rmLoadDefault: variance explained in the 'co' slot, polar
% angle in the 'ph' slot, and eccentricity (or log10(eccentricity)) in the
% 'map' slot. However, you can overload this by specifying alternate sets
% of co, eccentricity, or phase data by passing them in as parameter/value
% pairs (see OPTIONS below).
%
% the line ROI should be transformed from the cortical surface mesh
% _without_ fill layers.
%
% INPUTS
%  vw: mrVista view struct
%  newfig: whether you open a new figure or not
%  colored: whether you change the color of dots depending on the coherence
%  data.
%
% OPTIONS:
%	'cothresh', [cothresh]: specify the minimum coherence value to include
%	in the plot.
%
%	'eccthresh', [eccthresh]: specify the maximum eccentricity to include
%	in the plot.
%
%	'co', [co data]: overload the view's coherence data with your own set of
%	coherence values. Should match the size of the data in view.co.
%
%	'ecc', [ecc data]: overload the view's eccentricity data with your own set of
%	eccentricity values. Should match the size of the data in view.map.
%
%	'ph', [ph data]: overload the view's phase data with your own set of
%	polar angle values. Should match the size of the data in view.ph.
%
%  SEE ALSO: retinoPlot, rmPlotCoverage.
%
%  08/06 KA made several changes
%  aug 2008: JW added ROI title to the plot
%  sep 2008: JW added option to draw visually-referred polygon ROI
%  jan 2009: JW: removed subroutine 'ROIcaptureSelectedPoints' and put it
%                in an independent function, so that it can be called from
%                other plotting functions
%  apr 2009: RAS: clarified what needs to be set up; allows you to overload
%  new ph, ecc, and co values.

if notDefined('vw'), error('View must be defined.'); end
if notDefined('newfig'), newfig = 1; end
if notDefined('colored'), colored = 0; end

if notDefined('drawROI'), drawROI = 0; end

if isempty(viewGet(vw, 'ROIs')) || viewGet(vw, 'selected ROI') < 1
	error('No Selected ROI!');
end

% Get selpts from current ROI
ROIcoords = viewGet(vw, 'ROI coords');

% Get co and ph (vectors) for the current scan, within the
% current ROI.
I = viewGet(vw, 'ROI indices');
model = viewGet(vw, 'rmmodel');
model = model{1};
co = rmGet(model, 'varexp');
co = co(I);
ph = rmGet(model, 'pol');
ph = ph(I);
ecc = rmGet(model, 'ecc');
ecc = ecc(I);

% Remove NaNs from subCo and subAmp that may be there if ROI
% includes volume voxels where there is no data.
NaNs = find(isnan(co), 1);
if ~isempty(NaNs)
%   myWarnDlg('ROI includes voxels that have no data.  These voxels are being ignored.');
  notNaNs = find(~isnan(co));
  co = co(notNaNs);
  ph = ph(notNaNs);
  ecc = ecc(notNaNs);
end

cothresh = viewGet(vw, 'co thresh');
eccthresh = max(viewGet(vw, 'mapclipmode'));

%% parse options
for ii = 1:2:length(varargin)
	switch lower(varargin{ii})
		case 'cothresh', cothresh = varargin{ii+1};
		case 'eccthresh', eccthresh = varargin{ii+1};
		case 'ecc', ecc = varargin{ii+1};
		case 'ph',  ph = varargin{ii+1};
		case 'co', co = varargin{ii+1};
	end
end

% Find voxels which satisfy cothresh and eccthresh.
coIndices = find(co>cothresh & ecc<=eccthresh);

% Pull out co and ph for desired pixels
subCo =   co(coIndices);
subPh =   ph(coIndices);
subEcc = ecc(coIndices);
if newfig
    figHandle = figure;
else
	%figHandle = selectGraphWin;
end

% selectGraphWin;
% Window header
% headerStr = ['Eccentricity vs. phase, ROI ',ROIname,', scan ',num2str(curScan)];
% set(gcf,'Name',headerStr);
% Plot it
fontSize = 14;
symbolSize = 4;

% polar plot
subX = subEcc.*cos(subPh);
subY = subEcc.*sin(subPh);

% polar plot params
params.grid = 'on';
params.line = 'off';
params.gridColor = [0.6,0.6,0.6];
params.fontSize = fontSize;
params.symbol = 'o';
params.size = symbolSize;
params.color = 'w';
params.fillColor = 'w';
params.maxAmp = eccthresh;
% if eccthresh > 10, params.ringTicks = [0:5:eccthresh];
% else
	params.ringTicks = round( linspace(0, eccthresh, 4) );
% end

% Use 'polarPlot' to set up grid'
% clf
polarPlot(0,params);

% finish plotting it
for i=1:size(subX,2)
    if colored
        h=plot(subX(i),subY(i),'o','MarkerSize',symbolSize,'Color',[1-subCo(i) 1-subCo(i) 1-subCo(i)]);
        set(h,'MarkerFaceColor',[1-subCo(i) 1-subCo(i) 1-subCo(i)])
    else
        h=plot(subX(i),subY(i),'o','MarkerSize',symbolSize,'Color',[0 0 0]);
        set(h,'MarkerFaceColor',[0.5 0.5 0.5])
    end
end

title(vw.ROIs(vw.selectedROI).name, 'FontSize', 24, 'Interpreter', 'none');
hold off

if drawROI,
    vw = roiCapturePointsFromPlot(vw, subX, subY, coIndices, ROIcoords);
end

% Save the data in gca('UserData')
data.co = co;
data.ph = ph;
data.subCo = subCo;
data.subPh = subPh;
set(gca,'UserData',data);

return;

end

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function varargout = scl_slope(varargin)
% file_array's scl_slope property
% For getting the value
% dat = scl_slope(obj)
%
% For setting the value
% obj = scl_slope(obj,dat)
%__________________________________________________________________________
% Copyright (C) 2005-2017 Wellcome Trust Centre for Neuroimaging

%
% $Id: scl_slope.m 7147 2017-08-03 14:07:01Z spm $

if nargin==2
    varargout{1} = asgn(varargin{:});
elseif nargin==1
    varargout{1} = ref(varargin{:});
else
    error('Wrong number of arguments.');
end

%==========================================================================
% function dat = ref(obj)
%==========================================================================
function dat = ref(obj)
dat = obj.scl_slope;

%==========================================================================
% function obj = asgn(obj,dat)
%==========================================================================
function obj = asgn(obj,dat)
if isnumeric(dat) % && numel(dat)<=1,
    obj.scl_slope = double(dat);
else
    error('"scl_slope" must be numeric.');
end

author: spm


text:
function y = cvx_subsref( x, varargin )
temp.type = '()';
temp.subs = varargin;
y = subsref( x, temp );

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


text:
% trainScript2

imdir = '../images/all_images';
outdir = '/IUS/vmr20/dhoiem/data/mcmcdata';
ncv = 5;

%nsegments = [3 5 7 9 11 13 15 20 25 30 35 40 50 60 70 80 90 100];
nsegments = [3 5 6 6 7 8 9 10 11 12 13 14 15 17 18 20 23 27 32 45 50 75 100];
%nsegments = [3 4 5 6 7 7 8 8 9 9 10 11 11 12 12 13 13 14 14 15 16 16 17 19 20 21 23 25 28 31 37 51];

if ~exist('efeaturesdj')
    [efeaturesdj, adjlistdj] = mcmcGetAllDisjointEdgeData(spfeatures, imsegs, adjlist);
    save([outdir '/mcmcEdgeDataDj.mat'], 'efeaturesdj', 'adjlistdj');
end

if ~exist('eclassifierdj')
    eclassifierdj = mcmcTrainEdgeClassifier(efeaturesdj(cluster_images), ...
        adjlistdj(cluster_images), imsegs(cluster_images));
    save([outdir '/mcmcEdgeClassifierDj.mat'], 'eclassifierdj');
end

if ~exist('labdata')
    % gather data
    for tf = 1:numel(cv_images)

        f = cv_images(tf);

        disp([num2str(tf) ': ' imsegs(f).imname])

        [pvSP{tf}, phSP{tf}, pE{tf}] = mcmcInitialize(spfeatures{f}, efeatures{f}, ...
            adjlist{f}, imsegs(f), vclassifierSP, hclassifierSP, eclassifier, 'labels');
        pEdj{tf} =1./(1+exp(-test_boosted_dt_mc(eclassifierdj, efeaturesdj{f})));
        smaps{tf} = generateMultipleSegmentations2([pE{tf} ; pEdj{tf}], ...
            [adjlist{f} ; adjlistdj{f}], imsegs(f).nseg, nsegments);

        im = im2double(imread([imdir '/' imsegs(f).imname]));
        imdata = mcmcComputeImageData(im, imsegs(f));
%         imdata.pvSP = pvSP;
%         imdata.phSP = phSP;

        for k = 1:numel(nsegments)
            smapk = smaps{tf}(:, k);
            labdata{tf, k} = mcmcGetSegmentFeatures(imsegs(f), spfeatures{f}, imdata, smapk, 1:max(smapk));
            segdata{tf, k} = mcmcGetSegmentationFeatures(pvSP{tf}, phSP{tf}, ...
                [pE{tf} ; pEdj{tf}], [adjlist{f} ; adjlistdj{f}], imsegs(f).npixels, smapk, 1:max(smapk));
            [mclab{tf, k}, mcprc{tf, k}, allprc{tf, k}, trainw{tf, k}] = ...
                segmentation2labels(imsegs(f), smapk);
            unilabel{tf, k} = mclab{tf, k}.*(mcprc{tf, k}>0.99);
            seglabel{tf,k} =  1*(mcprc{tf, k}>0.99) + (-1)*(mcprc{tf, k}<0.90);

            if k==8
                figure(1), imagesc(label2rgb(smapk(imsegs(f).segimage))), axis image
                drawnow; pause(1);
            end
        end
    end
    save([outdir '/traindataDj.mat'], 'smaps', 'labdata', 'segdata', ...
        'mclab', 'mcprc', 'allprc', 'seglabel', 'unilabel', 'trainw', ...
        'pvSP', 'phSP', 'pE', 'pEdj');
end

for k = 1:ncv
    disp(['Iteration: ' num2str(k)]);
    testind{k} = (floor((k-1)*numel(cv_images)/ncv)+1):(floor(k*numel(cv_images)/ncv));
    trainind{k} = setdiff([1:numel(cv_images)], testind{k});
    sclassifier(k) = mcmcTrainSegmentationClassifier2(segdata(trainind{k}, :), seglabel(trainind{k}, :), trainw(trainind{k}, :));
    [vclassifier(k), hclassifier(k)] = ...
        mcmcTrainSegmentClassifier2(labdata(trainind{k}, :), unilabel(trainind{k}, :), trainw(trainind{k}, :));
    %[vregressor(k), hregressor(k)] = ...
    %    trainRegressorLR(labdata(trainind, :), allprc(trainind, :), trainw(trainind, :));
    %[sregressor(k)] = ...
    %    trainRegressorLR(labdata(trainind{k}, :), mcprc(trainind{k}, :), trainw(trainind{k}, :));
end
[vacc, hacc, vcm, hcm, pg] = testMultipleSegmentationsCV2(imsegs(cv_images), ...
    labdata, segdata, smaps, vclassifier, hclassifier, sclassifier, pvSP, phSP, ncv);

save([outdir '/resultsdj.mat'], 'vclassifier', 'hclassifier', 'sclassifier', 'vacc', 'hacc', 'vcm', 'hcm', 'pg');

author: zouchuhang

repo: LayoutNet


text:
function [E]=gramSchmidtOrtho(E)

k=size(E,1);
for i=1:1:k
    E(i,:)=E(i,:)./norm(E(i,:));
    for j=i+1:1:k
        Ei=E(i,:);
        Ej=E(j,:);
        proj_ei_ej=dot(Ei,Ej).*Ei./norm(Ei);
        E(j,:)=E(j,:)-proj_ei_ej;
    end
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:
clear all
close all
clc
load('\dades.mat')
k=4;

%%%%%%%%%%%%%%%%%%%%%
%  Funcion k-means  %
%%%%%%%%%%%%%%%%%%%%%
[centro, Xcluster, Ycluster, clustering]=kmedia(X, Y, k);

% Presentacion de resultados por pantalla.

% Creamos los colores.
colr=zeros(k,3);
for i=1:k
    colr(i,:)=rand(1,3);
end;

% Representamosla informacion.
figure;
hold on;
for i=1:k,
    n=find(clustering==i);
    p=plot(X(n), Y(n),'.');set(p,'Color',colr(i,:));title(k)
end;

clc;
fprintf('Numero de agrupaciones: %d',A);
% Borramos variables temporales.
clc;clear n;clear i;clear p;clear colr;

author: Sable

repo: mcbench-benchmarks


text:
function varargout = colorspace(Conversion,varargin)
%COLORSPACE  Transform a color image between color representations.
%   B = COLORSPACE(S,A) transforms the color representation of image A
%   where S is a string specifying the conversion.  The input array A
%   should be a real full double array of size Mx3 or MxNx3.  The output B
%   is the same size as A.
%
%   S tells the source and destination color spaces, S = 'dest<-src', or
%   alternatively, S = 'src->dest'.  Supported color spaces are
%
%     'RGB'              sRGB IEC 61966-2-1
%     'YCbCr'            Luma + Chroma ("digitized" version of Y'PbPr)
%     'JPEG-YCbCr'       Luma + Chroma space used in JFIF JPEG
%     'YDbDr'            SECAM Y'DbDr Luma + Chroma
%     'YPbPr'            Luma (ITU-R BT.601) + Chroma
%     'YUV'              NTSC PAL Y'UV Luma + Chroma
%     'YIQ'              NTSC Y'IQ Luma + Chroma
%     'HSV' or 'HSB'     Hue Saturation Value/Brightness
%     'HSL' or 'HLS'     Hue Saturation Luminance
%     'HSI'              Hue Saturation Intensity
%     'XYZ'              CIE 1931 XYZ
%     'Lab'              CIE 1976 L*a*b* (CIELAB)
%     'Luv'              CIE L*u*v* (CIELUV)
%     'LCH'              CIE L*C*H* (CIELCH)
%     'CAT02 LMS'        CIE CAT02 LMS
%
%  All conversions assume 2 degree observer and D65 illuminant.
%
%  Color space names are case insensitive and spaces are ignored.  When
%  sRGB is the source or destination, it can be omitted. For example
%  'yuv<-' is short for 'yuv<-rgb'.
%
%  For sRGB, the values should be scaled between 0 and 1.  Beware that
%  transformations generally do not constrain colors to be "in gamut."
%  Particularly, transforming from another space to sRGB may obtain
%  R'G'B' values outside of the [0,1] range.  So the result should be
%  clamped to [0,1] before displaying:
%     image(min(max(B,0),1));  % Clamp B to [0,1] and display
%
%  sRGB (Red Green Blue) is the (ITU-R BT.709 gamma-corrected) standard
%  red-green-blue representation of colors used in digital imaging.  The
%  components should be scaled between 0 and 1.  The space can be
%  visualized geometrically as a cube.
%
%  Y'PbPr, Y'CbCr, Y'DbDr, Y'UV, and Y'IQ are related to sRGB by linear
%  transformations.  These spaces separate a color into a grayscale
%  luminance component Y and two chroma components.  The valid ranges of
%  the components depends on the space.
%
%  HSV (Hue Saturation Value) is related to sRGB by
%     H = hexagonal hue angle   (0 <= H < 360),
%     S = C/V                   (0 <= S <= 1),
%     V = max(R',G',B')         (0 <= V <= 1),
%  where C = max(R',G',B') - min(R',G',B').  The hue angle H is computed on
%  a hexagon.  The space is geometrically a hexagonal cone.
%
%  HSL (Hue Saturation Lightness) is related to sRGB by
%     H = hexagonal hue angle                (0 <= H < 360),
%     S = C/(1 - |2L-1|)                     (0 <= S <= 1),
%     L = (max(R',G',B') + min(R',G',B'))/2  (0 <= L <= 1),
%  where H and C are the same as in HSV.  Geometrically, the space is a
%  double hexagonal cone.
%
%  HSI (Hue Saturation Intensity) is related to sRGB by
%     H = polar hue angle        (0 <= H < 360),
%     S = 1 - min(R',G',B')/I    (0 <= S <= 1),
%     I = (R'+G'+B')/3           (0 <= I <= 1).
%  Unlike HSV and HSL, the hue angle H is computed on a circle rather than
%  a hexagon.
%
%  CIE XYZ is related to sRGB by inverse gamma correction followed by a
%  linear transform.  Other CIE color spaces are defined relative to XYZ.
%
%  CIE L*a*b*, L*u*v*, and L*C*H* are nonlinear functions of XYZ.  The L*
%  component is designed to match closely with human perception of
%  lightness.  The other two components describe the chroma.
%
%  CIE CAT02 LMS is the linear transformation of XYZ using the MCAT02
%  chromatic adaptation matrix.  The space is designed to model the
%  response of the three types of cones in the human eye, where L, M, S,
%  correspond respectively to red ("long"), green ("medium"), and blue
%  ("short").
%
% Author::
% Pascal Getreuer 2005-2010

%%% Input parsing %%%
if nargin < 2, error('Not enough input arguments.'); end
[SrcSpace,DestSpace] = parse(Conversion);

if nargin == 2
   Image = varargin{1};
elseif nargin >= 3
   Image = cat(3,varargin{:});
else
   error('Invalid number of input arguments.');
end

FlipDims = (size(Image,3) == 1);

if FlipDims, Image = permute(Image,[1,3,2]); end
if ~isa(Image,'double'), Image = double(Image)/255; end
if size(Image,3) ~= 3, error('Invalid input size.'); end

SrcT = gettransform(SrcSpace);
DestT = gettransform(DestSpace);

if ~ischar(SrcT) && ~ischar(DestT)
   % Both source and destination transforms are affine, so they
   % can be composed into one affine operation
   T = [DestT(:,1:3)*SrcT(:,1:3),DestT(:,1:3)*SrcT(:,4)+DestT(:,4)];
   Temp = zeros(size(Image));
   Temp(:,:,1) = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3) + T(10);
   Temp(:,:,2) = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3) + T(11);
   Temp(:,:,3) = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3) + T(12);
   Image = Temp;
elseif ~ischar(DestT)
   Image = rgb(Image,SrcSpace);
   Temp = zeros(size(Image));
   Temp(:,:,1) = DestT(1)*Image(:,:,1) + DestT(4)*Image(:,:,2) + DestT(7)*Image(:,:,3) + DestT(10);
   Temp(:,:,2) = DestT(2)*Image(:,:,1) + DestT(5)*Image(:,:,2) + DestT(8)*Image(:,:,3) + DestT(11);
   Temp(:,:,3) = DestT(3)*Image(:,:,1) + DestT(6)*Image(:,:,2) + DestT(9)*Image(:,:,3) + DestT(12);
   Image = Temp;
else
   Image = feval(DestT,Image,SrcSpace);
end

%%% Output format %%%
if nargout > 1
   varargout = {Image(:,:,1),Image(:,:,2),Image(:,:,3)};
else
   if FlipDims, Image = permute(Image,[1,3,2]); end
   varargout = {Image};
end

return;

function [SrcSpace,DestSpace] = parse(Str)
% Parse conversion argument

if ischar(Str)
   Str = lower(strrep(strrep(Str,'-',''),'=',''));
   k = find(Str == '>');

   if length(k) == 1         % Interpret the form 'src->dest'
      SrcSpace = Str(1:k-1);
      DestSpace = Str(k+1:end);
   else
      k = find(Str == '<');

      if length(k) == 1      % Interpret the form 'dest<-src'
         DestSpace = Str(1:k-1);
         SrcSpace = Str(k+1:end);
      else
         error(['Invalid conversion, ''',Str,'''.']);
      end
   end

   SrcSpace = alias(SrcSpace);
   DestSpace = alias(DestSpace);
else
   SrcSpace = 1;             % No source pre-transform
   DestSpace = Conversion;
   if any(size(Conversion) ~= 3), error('Transformation matrix must be 3x3.'); end
end
return;

function Space = alias(Space)
Space = strrep(strrep(Space,'cie',''),' ','');

if isempty(Space)
   Space = 'rgb';
end

switch Space
case {'ycbcr','ycc'}
   Space = 'ycbcr';
case {'hsv','hsb'}
   Space = 'hsv';
case {'hsl','hsi','hls'}
   Space = 'hsl';
case {'rgb','yuv','yiq','ydbdr','ycbcr','jpegycbcr','xyz','lab','luv','lch'}
   return;
end
return;

function T = gettransform(Space)
% Get a colorspace transform: either a matrix describing an affine transform,
% or a string referring to a conversion subroutine
switch Space
case 'ypbpr'
   T = [0.299,0.587,0.114,0;-0.1687367,-0.331264,0.5,0;0.5,-0.418688,-0.081312,0];
case 'yuv'
   % sRGB to NTSC/PAL YUV
   % Wikipedia: http://en.wikipedia.org/wiki/YUV
   T = [0.299,0.587,0.114,0;-0.147,-0.289,0.436,0;0.615,-0.515,-0.100,0];
case 'ydbdr'
   % sRGB to SECAM YDbDr
   % Wikipedia: http://en.wikipedia.org/wiki/YDbDr
   T = [0.299,0.587,0.114,0;-0.450,-0.883,1.333,0;-1.333,1.116,0.217,0];
case 'yiq'
   % sRGB in [0,1] to NTSC YIQ in [0,1];[-0.595716,0.595716];[-0.522591,0.522591];
   % Wikipedia: http://en.wikipedia.org/wiki/YIQ
   T = [0.299,0.587,0.114,0;0.595716,-0.274453,-0.321263,0;0.211456,-0.522591,0.311135,0];
case 'ycbcr'
   % sRGB (range [0,1]) to ITU-R BRT.601 (CCIR 601) Y'CbCr
   % Wikipedia: http://en.wikipedia.org/wiki/YCbCr
   % Poynton, Equation 3, scaling of R'G'B to Y'PbPr conversion
   T = [65.481,128.553,24.966,16;-37.797,-74.203,112.0,128;112.0,-93.786,-18.214,128];
case 'jpegycbcr'
   % Wikipedia: http://en.wikipedia.org/wiki/YCbCr
   T = [0.299,0.587,0.114,0;-0.168736,-0.331264,0.5,0.5;0.5,-0.418688,-0.081312,0.5]*255;
case {'rgb','xyz','hsv','hsl','lab','luv','lch','cat02lms'}
   T = Space;
otherwise
   error(['Unknown color space, ''',Space,'''.']);
end
return;

function Image = rgb(Image,SrcSpace)
% Convert to sRGB from 'SrcSpace'
switch SrcSpace
case 'rgb'
   return;
case 'hsv'
   % Convert HSV to sRGB
   Image = huetorgb((1 - Image(:,:,2)).*Image(:,:,3),Image(:,:,3),Image(:,:,1));
case 'hsl'
   % Convert HSL to sRGB
   L = Image(:,:,3);
   Delta = Image(:,:,2).*min(L,1-L);
   Image = huetorgb(L-Delta,L+Delta,Image(:,:,1));
case {'xyz','lab','luv','lch','cat02lms'}
   % Convert to CIE XYZ
   Image = xyz(Image,SrcSpace);
   % Convert XYZ to RGB
   T = [3.2406, -1.5372, -0.4986; -0.9689, 1.8758, 0.0415; 0.0557, -0.2040, 1.057];
   R = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3);  % R
   G = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3);  % G
   B = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3);  % B
   % Desaturate and rescale to constrain resulting RGB values to [0,1]
   AddWhite = -min(min(min(R,G),B),0);
   R = R + AddWhite;
   G = G + AddWhite;
   B = B + AddWhite;
   % Apply gamma correction to convert linear RGB to sRGB
   Image(:,:,1) = gammacorrection(R);  % R'
   Image(:,:,2) = gammacorrection(G);  % G'
   Image(:,:,3) = gammacorrection(B);  % B'
otherwise  % Conversion is through an affine transform
   T = gettransform(SrcSpace);
   temp = inv(T(:,1:3));
   T = [temp,-temp*T(:,4)];
   R = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3) + T(10);
   G = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3) + T(11);
   B = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3) + T(12);
   Image(:,:,1) = R;
   Image(:,:,2) = G;
   Image(:,:,3) = B;
end

% Clip to [0,1]
Image = min(max(Image,0),1);
return;

function Image = xyz(Image,SrcSpace)
% Convert to CIE XYZ from 'SrcSpace'
WhitePoint = [0.950456,1,1.088754];

switch SrcSpace
case 'xyz'
   return;
case 'luv'
   % Convert CIE L*uv to XYZ
   WhitePointU = (4*WhitePoint(1))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
   WhitePointV = (9*WhitePoint(2))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
   L = Image(:,:,1);
   Y = (L + 16)/116;
   Y = invf(Y)*WhitePoint(2);
   U = Image(:,:,2)./(13*L + 1e-6*(L==0)) + WhitePointU;
   V = Image(:,:,3)./(13*L + 1e-6*(L==0)) + WhitePointV;
   Image(:,:,1) = -(9*Y.*U)./((U-4).*V - U.*V);                  % X
   Image(:,:,2) = Y;                                             % Y
   Image(:,:,3) = (9*Y - (15*V.*Y) - (V.*Image(:,:,1)))./(3*V);  % Z
case {'lab','lch'}
   Image = lab(Image,SrcSpace);
   % Convert CIE L*ab to XYZ
   fY = (Image(:,:,1) + 16)/116;
   fX = fY + Image(:,:,2)/500;
   fZ = fY - Image(:,:,3)/200;
   Image(:,:,1) = WhitePoint(1)*invf(fX);  % X
   Image(:,:,2) = WhitePoint(2)*invf(fY);  % Y
   Image(:,:,3) = WhitePoint(3)*invf(fZ);  % Z
case 'cat02lms'
    % Convert CAT02 LMS to XYZ
   T = inv([0.7328, 0.4296, -0.1624;-0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834]);
   L = Image(:,:,1);
   M = Image(:,:,2);
   S = Image(:,:,3);
   Image(:,:,1) = T(1)*L + T(4)*M + T(7)*S;  % X
   Image(:,:,2) = T(2)*L + T(5)*M + T(8)*S;  % Y
   Image(:,:,3) = T(3)*L + T(6)*M + T(9)*S;  % Z
otherwise   % Convert from some gamma-corrected space
   % Convert to sRGB
   Image = rgb(Image,SrcSpace);
   % Undo gamma correction
   R = invgammacorrection(Image(:,:,1));
   G = invgammacorrection(Image(:,:,2));
   B = invgammacorrection(Image(:,:,3));
   % Convert RGB to XYZ
   T = inv([3.2406, -1.5372, -0.4986; -0.9689, 1.8758, 0.0415; 0.0557, -0.2040, 1.057]);
   Image(:,:,1) = T(1)*R + T(4)*G + T(7)*B;  % X
   Image(:,:,2) = T(2)*R + T(5)*G + T(8)*B;  % Y
   Image(:,:,3) = T(3)*R + T(6)*G + T(9)*B;  % Z
end
return;

function Image = hsv(Image,SrcSpace)
% Convert to HSV
Image = rgb(Image,SrcSpace);
V = max(Image,[],3);
S = (V - min(Image,[],3))./(V + (V == 0));
Image(:,:,1) = rgbtohue(Image);
Image(:,:,2) = S;
Image(:,:,3) = V;
return;

function Image = hsl(Image,SrcSpace)
% Convert to HSL
switch SrcSpace
case 'hsv'
   % Convert HSV to HSL
   MaxVal = Image(:,:,3);
   MinVal = (1 - Image(:,:,2)).*MaxVal;
   L = 0.5*(MaxVal + MinVal);
   temp = min(L,1-L);
   Image(:,:,2) = 0.5*(MaxVal - MinVal)./(temp + (temp == 0));
   Image(:,:,3) = L;
otherwise
   Image = rgb(Image,SrcSpace);  % Convert to sRGB
   % Convert sRGB to HSL
   MinVal = min(Image,[],3);
   MaxVal = max(Image,[],3);
   L = 0.5*(MaxVal + MinVal);
   temp = min(L,1-L);
   S = 0.5*(MaxVal - MinVal)./(temp + (temp == 0));
   Image(:,:,1) = rgbtohue(Image);
   Image(:,:,2) = S;
   Image(:,:,3) = L;
end
return;

function Image = lab(Image,SrcSpace)
% Convert to CIE L*a*b* (CIELAB)
WhitePoint = [0.950456,1,1.088754];

switch SrcSpace
case 'lab'
   return;
case 'lch'
   % Convert CIE L*CH to CIE L*ab
   C = Image(:,:,2);
   Image(:,:,2) = cos(Image(:,:,3)*pi/180).*C;  % a*
   Image(:,:,3) = sin(Image(:,:,3)*pi/180).*C;  % b*
otherwise
   Image = xyz(Image,SrcSpace);  % Convert to XYZ
   % Convert XYZ to CIE L*a*b*
   X = Image(:,:,1)/WhitePoint(1);
   Y = Image(:,:,2)/WhitePoint(2);
   Z = Image(:,:,3)/WhitePoint(3);
   fX = f(X);
   fY = f(Y);
   fZ = f(Z);
   Image(:,:,1) = 116*fY - 16;    % L*
   Image(:,:,2) = 500*(fX - fY);  % a*
   Image(:,:,3) = 200*(fY - fZ);  % b*
end
return;

function Image = luv(Image,SrcSpace)
% Convert to CIE L*u*v* (CIELUV)
WhitePoint = [0.950456,1,1.088754];
WhitePointU = (4*WhitePoint(1))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
WhitePointV = (9*WhitePoint(2))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));

Image = xyz(Image,SrcSpace); % Convert to XYZ
Denom = Image(:,:,1) + 15*Image(:,:,2) + 3*Image(:,:,3);
U = (4*Image(:,:,1))./(Denom + (Denom == 0));
V = (9*Image(:,:,2))./(Denom + (Denom == 0));
Y = Image(:,:,2)/WhitePoint(2);
L = 116*f(Y) - 16;
Image(:,:,1) = L;                        % L*
Image(:,:,2) = 13*L.*(U - WhitePointU);  % u*
Image(:,:,3) = 13*L.*(V - WhitePointV);  % v*
return;

function Image = lch(Image,SrcSpace)
% Convert to CIE L*ch
Image = lab(Image,SrcSpace);  % Convert to CIE L*ab
H = atan2(Image(:,:,3),Image(:,:,2));
H = H*180/pi + 360*(H < 0);
Image(:,:,2) = sqrt(Image(:,:,2).^2 + Image(:,:,3).^2);  % C
Image(:,:,3) = H;                                        % H
return;

function Image = cat02lms(Image,SrcSpace)
% Convert to CAT02 LMS
Image = xyz(Image,SrcSpace);
T = [0.7328, 0.4296, -0.1624;-0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834];
X = Image(:,:,1);
Y = Image(:,:,2);
Z = Image(:,:,3);
Image(:,:,1) = T(1)*X + T(4)*Y + T(7)*Z;  % L
Image(:,:,2) = T(2)*X + T(5)*Y + T(8)*Z;  % M
Image(:,:,3) = T(3)*X + T(6)*Y + T(9)*Z;  % S
return;

function Image = huetorgb(m0,m2,H)
% Convert HSV or HSL hue to RGB
N = size(H);
H = min(max(H(:),0),360)/60;
m0 = m0(:);
m2 = m2(:);
F = H - round(H/2)*2;
M = [m0, m0 + (m2-m0).*abs(F), m2];
Num = length(m0);
j = [2 1 0;1 2 0;0 2 1;0 1 2;1 0 2;2 0 1;2 1 0]*Num;
k = floor(H) + 1;
Image = reshape([M(j(k,1)+(1:Num).'),M(j(k,2)+(1:Num).'),M(j(k,3)+(1:Num).')],[N,3]);
return;

function H = rgbtohue(Image)
% Convert RGB to HSV or HSL hue
[M,i] = sort(Image,3);
i = i(:,:,3);
Delta = M(:,:,3) - M(:,:,1);
Delta = Delta + (Delta == 0);
R = Image(:,:,1);
G = Image(:,:,2);
B = Image(:,:,3);
H = zeros(size(R));
k = (i == 1);
H(k) = (G(k) - B(k))./Delta(k);
k = (i == 2);
H(k) = 2 + (B(k) - R(k))./Delta(k);
k = (i == 3);
H(k) = 4 + (R(k) - G(k))./Delta(k);
H = 60*H + 360*(H < 0);
H(Delta == 0) = nan;
return;

function Rp = gammacorrection(R)
Rp = zeros(size(R));
i = (R <= 0.0031306684425005883);
Rp(i) = 12.92*R(i);
Rp(~i) = real(1.055*R(~i).^0.416666666666666667 - 0.055);
return;

function R = invgammacorrection(Rp)
R = zeros(size(Rp));
i = (Rp <= 0.0404482362771076);
R(i) = Rp(i)/12.92;
R(~i) = real(((Rp(~i) + 0.055)/1.055).^2.4);
return;

function fY = f(Y)
fY = real(Y.^(1/3));
i = (Y < 0.008856);
fY(i) = Y(i)*(841/108) + (4/29);
return;

function Y = invf(fY)
Y = fY.^3;
i = (Y < 0.008856);
Y(i) = (fY(i) - 4/29)*(108/841);
return;

author: petercorke

repo: machinevision-toolbox-matlab


text:
function model = yalmip2nonlinearsolver(model)

global newmodel

newmodel = 1;

model.dense = 0;
if ~model.equalitypresolved
    model = propagate_bounds_from_equalities(model);
end

K = model.K;
lb = model.lb;
ub = model.ub;
x0 = model.x0;
c = model.c;

% Pick out the positive conditions from cones ||Ax+b|| <= c'*x+d which will
% be treated as (Ax+b)'*(ax+b) <= (c'*x+d)^2,  c'*x+d >= 0
if any(K.q)
    aux = [];
    top = 1 + K.f + K.l;
    for i = 1:length(K.q)
        row = model.F_struc(top,:);
        if any(row(2:end))
            aux = [aux;row];
        end
        top = top + model.K.q(i);
    end
    model.F_struc = [model.F_struc(1:K.f+K.l,:);aux;model.F_struc(K.f+K.l+1:end,:)];
    model.K.l = model.K.l + size(aux,1);
end

if isempty(model.evaluation_scheme)
    model = build_recursive_scheme(model);
end
model = compress_evaluation_scheme(model);

% Do some pre-calc to be used in calls from fmincon
nonlinearindicies = union(find(model.variabletype~=0),model.evalVariables);
linearindicies    = setdiff(find(model.variabletype==0),nonlinearindicies);
model.nonlinearindicies = nonlinearindicies;
model.linearindicies    = linearindicies;

model.Anonlinineq = [];
model.bnonlinineq = [];
model.Anonlineq = [];
model.bnonlineq = [];

% Extract linear and nonlinear equality constraints
if K.f>0
    Aeq = -model.F_struc(1:1:K.f,2:end);
    beq = model.F_struc(1:1:model.K.f,1);

    nonlinear_equalities_indicies = find(any(Aeq(:,nonlinearindicies),2));
    model.Anonlineq = Aeq(nonlinear_equalities_indicies,:);
    model.bnonlineq = beq(nonlinear_equalities_indicies);

    Aeq(nonlinear_equalities_indicies,:) = [];
    beq(nonlinear_equalities_indicies,:) = [];
    Aeq(:,nonlinearindicies) = [];
    model.F_struc(1:model.K.f,:) = [];
    model.K.f = 0;
else
    Aeq = [];
    beq = [];
end

% Find nonlinear eualities implied by lower and upper bounds
if ~isempty(ub) && ~isempty(lb)
    nonlinearequality = find(lb(nonlinearindicies) == ub(nonlinearindicies));
    if ~isempty(nonlinearequality)
        for i = 1:length(nonlinearequality)
          %  model.Anonlineq = [model.Anonlineq;eyev(length(c),nonlinearindicies(nonlinearequality(i)))'];
            model.Anonlineq = [model.Anonlineq;sparse(1,nonlinearindicies(nonlinearequality(i)),1,1,length(c))];
            model.bnonlineq = [model.bnonlineq;lb(nonlinearindicies(nonlinearequality(i)))];
        end
    end
end

% Extract linear and nonlinear inequality constraints
if model.K.l>0
    A = -model.F_struc(1:model.K.l,2:end);
    b = model.F_struc(1:model.K.l,1);

    nonlinear_inequalities_indicies = find(any(A(:,nonlinearindicies),2));

    model.Anonlinineq = A(nonlinear_inequalities_indicies,:);
    model.bnonlinineq = b(nonlinear_inequalities_indicies);

    A(nonlinear_inequalities_indicies,:) = [];
    b(nonlinear_inequalities_indicies,:) = [];
    A(:,nonlinearindicies) = [];

    model.F_struc(1:model.K.l,:) = [];
    model.K.l = 0;
else
    A = [];
    b = [];
end

% This helps with robustness in bnb in some cases
x0candidate = zeros(length(c),1);
if ~isempty(lb) && ~isempty(ub)
    bounded = find(~isinf(lb) & ~isinf(ub));
    x0candidate(bounded) = (lb(bounded) + ub(bounded))/2;
    bounded_below = find(~isinf(lb) & isinf(ub));
    x0candidate(bounded_below) = lb(bounded_below) + 0.5;
    bounded_above = find(~isinf(lb) & isinf(ub));
    x0candidate(bounded_above) = lb(bounded_above) + 0.5;
end

if isempty(x0)
    x0 = x0candidate(linearindicies);
else
    if ~isempty(lb) && ~isempty(ub)
        x0((x0 < lb) | (x0 > ub)) = x0candidate((x0 < lb) | (x0 > ub));
    end
    x0 = x0(linearindicies);
end

if ~isempty(lb)
    lb = lb(linearindicies);
end
if ~isempty(ub)
    ub = ub(linearindicies);
end

lb_old = lb;
ub_old = ub;
[lb,ub,A,b] = remove_bounds_from_Ab(A,b,lb,ub);
[lb,ub,Aeq,beq] = remove_bounds_from_Aeqbeq(Aeq,beq,lb,ub);

if any(model.variabletype == 4)
    problematic = find(any(model.monomtable(:,linearindicies) < 0 ,1));
    if ~isempty(problematic)
        problematic = problematic(find(x0(problematic)==0));
        Oneisfeas = problematic(find(ub(problematic) > 1));
        x0(Oneisfeas) = 1;
    end

    problematic = find(any(model.monomtable(:,linearindicies)~=fix(model.monomtable(:,linearindicies)) ,1));
    lb(problematic) = max(lb(problematic),0);
end
x0(find(lb==ub)) = lb(find(lb==ub));

if size(A,1) == 0
    A = [];
end

if size(b,1) == 0
    b = [];
end

if size(Aeq,1) == 0
    Aeq = [];
end

if size(beq,1) == 0
    beq = [];
end

if model.presolveequalities
    if ~isempty(beq) &  (~model.equalitypresolved | ~(isequal(lb,lb_old) & isequal(ub,ub_old)))
        % This helps when there are artificial variables introduced to model
        % nonlinear operators such as log(2*x+1)
        p.F_struc = [beq -Aeq];
        p.K.f = size(beq,1);
        p.lb = lb;
        p.ub = ub;
        p.variabletype = zeros(1,length(lb));
        p.binary_variables = [];
        p.integer_variables = [];
        p = propagate_bounds_from_equalities(p);
        lb = p.lb;
        ub = p.ub;
    end
end

model.A = A;
model.b = b;
model.Aeq = Aeq;
model.beq = beq;
model.lb = lb;
model.ub = ub;
model.x0 = x0;

model = setup_fmincon_params(model);

% Check if all derivatives are available
model.derivative_available = 1;
for i = 1:length(model.evalMap)
    if isempty(model.evalMap{i}.properties.derivative)
        model.derivative_available = 0;
        break
    end
end

% Some precomputation of computational scheme for Jacobian
allA = [model.Anonlineq;model.Anonlinineq];
if any(model.K.q)
    allA = [allA;model.F_struc(1+model.K.f + model.K.f:end,2:end)];
end
requested = any(allA',2);
[i,j] = find((model.deppattern(find(requested),:)));
requested(j) = 1;
if ~isempty(model.evalMap)
    % Recursive stuff is only possible if we have evaluation-based
    % operators
    model.Crecursivederivativeprecompute = precomputeDerivative(model,requested);
end

% Some precomputation of computational scheme for gradient
requested = model.c | any(model.Q,2);
[i,j,k] = find((model.deppattern(find(requested),:)));
requested(j) = 1;
model.frecursivederivativeprecompute = precomputeDerivative(model,requested);

% Precomputed list of bilinear expressions, used in
% apply_recursive_differentiation
model = compile_bilinearslist(model);
model = compile_quadraticslist(model);

model.binary_variables  = find(ismember(linearindicies,model.binary_variables));
model.integer_variables  = find(ismember(linearindicies,model.integer_variables));
model.semicont_variables  = find(ismember(linearindicies,model.semicont_variables));

author: zarathustr

repo: LibQPEP

save_path: github-repos/MATLAB/zarathustr-LibQPEP


text:
function [y, Sigma_y] = GMR(Priors, Mu, Sigma, x, in, out)
%
% Gaussian Mixture Regression.
% This source code is the implementation of the algorithms described in
% Section 2.4, p.38 of the book "Robot Programming by Demonstration: A
% Probabilistic Approach".
%
% Author:	Sylvain Calinon, 2009
%			http://programming-by-demonstration.org
%
% This function performs Gaussian Mixture Regression (GMR), using the
% parameters of a Gaussian Mixture Model (GMM). Given partial input data,
% the algorithm computes the expected distribution for the resulting
% dimensions. By providing temporal values as inputs, it thus outputs a
% smooth generalized version of the data encoded in GMM, and associated
% constraints expressed by covariance matrices.
%
% Inputs -----------------------------------------------------------------
%   o Priors:  1 x K array representing the prior probabilities of the K GMM
%              components.
%   o Mu:      D x K array representing the centers of the K GMM components.
%   o Sigma:   D x D x K array representing the covariance matrices of the
%              K GMM components.
%   o x:       P x N array representing N datapoints of P dimensions.
%   o in:      1 x P array representing the dimensions to consider as
%              inputs.
%   o out:     1 x Q array representing the dimensions to consider as
%              outputs (D=P+Q).
% Outputs ----------------------------------------------------------------
%   o y:       Q x N array representing the retrieved N datapoints of
%              Q dimensions, i.e. expected means.
%   o Sigma_y: Q x Q x N array representing the N expected covariance
%              matrices retrieved.
%
% This source code is given for free! However, I would be grateful if you refer
% to the book (or corresponding article) in any academic publication that uses
% this code or part of it. Here are the corresponding BibTex references:
%
% @book{Calinon09book,
%   author="S. Calinon",
%   title="Robot Programming by Demonstration: A Probabilistic Approach",
%   publisher="EPFL/CRC Press",
%   year="2009",
%   note="EPFL Press ISBN 978-2-940222-31-5, CRC Press ISBN 978-1-4398-0867-2"
% }
%
% @article{Calinon07,
%   title="On Learning, Representing and Generalizing a Task in a Humanoid Robot",
%   author="S. Calinon and F. Guenter and A. Billard",
%   journal="IEEE Transactions on Systems, Man and Cybernetics, Part B",
%   year="2007",
%   volume="37",
%   number="2",
%   pages="286--298",
% }

nbData = size(x,2);
nbVar = size(Mu,1);
nbStates = size(Sigma,3);

%% Fast matrix computation (see the commented code for a version involving
%% one-by-one computation, which is easier to understand).
%%
%% Compute the influence of each GMM component, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nbStates
  Pxi(:,i) = Priors(i).* gaussPDF(x, Mu(in,i), Sigma(in,in,i));
end
beta = Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates);
%% Compute expected means y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
  y_tmp(:,:,j) = repmat(Mu(out,j),1,nbData) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x-repmat(Mu(in,j),1,nbData));
end
beta_tmp = reshape(beta,[1 size(beta)]);
y_tmp2 = repmat(beta_tmp,[length(out) 1 1]) .* y_tmp;
y = sum(y_tmp2,3);
%% Compute expected covariance matrices Sigma_y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
  Sigma_y_tmp(:,:,1,j) = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
end
beta_tmp = reshape(beta,[1 1 size(beta)]);
Sigma_y_tmp2 = repmat(beta_tmp.*beta_tmp, [length(out) length(out) 1 1]) .* repmat(Sigma_y_tmp,[1 1 nbData 1]);
Sigma_y = sum(Sigma_y_tmp2,4);

% %% Slow one-by-one computation (better suited to understand the algorithm)
% %%
% %% Compute the influence of each GMM component, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for i=1:nbStates
%   Pxi(:,i) = gaussPDF(x, Mu(in,i), Sigma(in,in,i));
% end
% beta = (Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates))';
% %% Compute expected output distribution, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% y = zeros(length(out), nbData);
% Sigma_y = zeros(length(out), length(out), nbData);
% for i=1:nbData
%   % Compute expected means y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     yj_tmp = Mu(out,j) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x(:,i)-Mu(in,j));
%     y(:,i) = y(:,i) + beta(j,i).*yj_tmp;
%   end
%   % Compute expected covariance matrices Sigma_y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     Sigmaj_y_tmp = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
%     Sigma_y(:,:,i) = Sigma_y(:,:,i) + beta(j,i)^2.* Sigmaj_y_tmp;
%   end
% end

author: epfl-lasa


text:
function hh = plot_uc(md, varargin)
%PLOT_UC   Plot generator commitment summary
%
%   PLOT_UC(MD)
%   PLOT_UC(MD, IDX)
%   PLOT_UC(MD, IDX, '<option1_name>', '<option1_value', ...)
%   PLOT_UC(MD, IDX, OPT)
%   PLOT_UC(MD, IDX, OPT, '<option1_name>', '<option1_value', ...)
%   PLOT_UC(MD, MD2)
%   PLOT_UC(MD, MD2, IDX, )
%   PLOT_UC(MD, MD2, IDX, '<option1_name>', '<option1_value', ...)
%   PLOT_UC(MD, MD2, IDX, OPT)
%   PLOT_UC(MD, MD2, IDX, OPT, '<option1_name>', '<option1_value', ...)
%   H = PLOT_UC(MD, ...)
%
%   IDX is a vector of gen indices, if empty, it includes all generators in
%   MD. Options can be specified as an OPT struct or as individual pairs of
%   'name' and 'value' arguments. The possible options include the following,
%   where the default is shown in parenthesis:
%       'title'         ('Unit Commitment - %s') title for the plot, where
%                       %s is an optional placeholder for the subtitle
%       'subtitle'      ({'First', 'Second', 'Both'}) cell array of labels
%                       to use for legend (if two schedules are provided)
%                       and to replace a placeholder in the title (based
%                       on which commitment schedule(s) is(are) provided in
%                       UC1 and/or UC2); can also be a simple string, in
%                       which case no legend will be displayed even if both
%                       UC1 and UC2 are supplied
%       'xlabel'        ('Period') label for horizontal axis
%       'ylabel'        (<empty>) label for vertical axis
%       'rowlabels'     ({'1', '2', '3', ...) labels for rows (top to bottom)
%       'saveit'        (false) flag to indicate whether to create PDF file
%       'saveall'       (false) flag to indicate whether to create a single
%                       PDF file or, if both MD and MD2 are supplied, three
%                       PFF files, one for MD, one for MD2 and one for both.
%       'savepath'      ('') path to directory to save files in
%       'savename'      ('uc-%s.pdf') name of PDF file
%                       %s is optional placeholder for storage unit index
%       'size_factor'   (1) to scale font/marker sizes in case you want to
%                       do sub-plots or something
%
%   Returns handle to current figure window.

%   MOST
%   Copyright (c) 2015-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MOST.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/most for more info.

%% define named indices into data matrices
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;

%% gather data
nt = md.idx.nt;
ng = size(md.mpc.gen, 1);
nj_max = max(md.idx.nj);
nc_max = max(max(md.idx.nc));

%% default optional args
idx = [];
md2 = [];
k = 1;
if nargin > 1                   %% 2nd arg is ...
    if isstruct(varargin{k})    %% ... md2
        md2 = varargin{k};
    else                        %% ... idx
        idx = varargin{k};
    end
    k = k + 1;
end
if isempty(idx) && nargin > 2   %% 3rd arg is ...
    if isnumeric(varargin{k})
        idx = varargin{k};      %% ... idx
        k = k + 1;
    end
end

%% default idx
if isempty(idx)
    idx = find(any(md.UC.CommitKey == 0, 2) | any(md.UC.CommitKey == 1, 2));
end
nidx = length(idx);
if nidx > 1 && size(idx, 1) == 1
    idx = idx';     %% convert row vector to column vector
end
b = md.mpc.gen(idx, GEN_BUS);

%% default options
opt = struct( ...
    'title', 'Unit Commitment - %s', ...
    'subtitle', {{'First', 'Second', 'Both'}}, ...
    'xlabel', 'Period', ...
    'ylabel', '', ...
    'rowlabels', [], ...
    'saveit', false, ...
    'saveall', false, ...
    'savepath', '', ...
    'savename', 'uc-%s.pdf', ...
    'size_factor', 1 );

%% process options
vargs = varargin(k:end); %% remaining arguments
if mod(length(vargs), 2)    %% odd number of options, first must be OPT struct
    if ~isstruct(vargs{1})
        error('plot_uc: Single OPT argument must be a struct');
    end
    myopt = vargs{1};
    k = 2;
else                        %% even number of options
    myopt = struct;
    k = 1;
end
while k < length(vargs)
    opt_name = vargs{k};
    opt_val  = vargs{k+1};
    if ~isfield(opt, opt_name)
        error('plot_uc: ''%s'' is not a valid option name', opt_name);
    end
    myopt.(opt_name) = opt_val;
    k = k + 2;
end
fields = fieldnames(myopt);
for f = 1:length(fields)
    opt.(fields{f}) = myopt.(fields{f});
end

%% check CommitKey match
uc1 = md.UC.CommitSched(idx, :);
if ~isempty(md2)
    uck2 = md2.UC.CommitKey(idx, :);
    if any(any( uck2 ~= md.UC.CommitKey(idx, :) & (uck2 == 2 | uck2 == 0) ))
        error('plot_uc: CommitKey fields in MD and MD2 do not match');
    end
    uc2 = md2.UC.CommitSched(idx, :);
else
    uc2 = [];
end

%% generator labels
m = size(uc1, 1);
opt.rowlabels = cell(m, 1);
if isfield(md.mpc, 'genfuel')
    for i = 1:m
        opt.rowlabels{i} = sprintf('Gen %d @ Bus %d, %s', idx(i), b(i), md.mpc.genfuel{idx(i)});
    end
else
    for i = 1:m
        opt.rowlabels{i} = sprintf('Gen %d @ Bus %d', idx(i), b(i));
    end
end

%% do the plots
if opt.saveall
    opt.saveit = 1;
end
if opt.saveall || isempty(uc2)
    cla;
    h = plot_uc_data(uc1, [], opt);
end
if ~isempty(uc2)
    if opt.saveall || isempty(uc1)
        cla;
        h = plot_uc_data([], uc2, opt);
    end
    if opt.saveall || ~isempty(uc1)
        cla;
        h = plot_uc_data(uc1, uc2, opt);
    end
end

if nargout
    hh = h;
end

author: MATPOWER

repo: matpower

save_path: github-repos/MATLAB/MATPOWER-matpower


text:
function M = perform_quicunx_wavelet_transform_ti(M,Jmin,options)

% perform_quicunx_wavelet_transform_ti - translation invariant quincunx wavelets
%
% Forward
%   MW = perform_quicunx_wavelet_transform_ti(M,Jmin,options);
% Backward
%   M = perform_quicunx_wavelet_transform_ti(MW,Jmin,options);
%
%   The implementation is based on lifting.
%
%   You can set the number of primal (analysis) and dual (synthesis)
%   vanishing moments
%       options.primal_vm (only 2/4 is supported).
%       options.dual_vm (only 2/4 is supported).
%
%   You can set the boundary conditions to
%       options.bound = 'per'   (periodic)
%       options.bound = 'sym'   (symmetric)
%
%   This transform (but not TI) is described in
%       Wavelet Families of Increasing Order in Arbitrary Dimensions
%       Jelena Kovacevic, Wim Sweldens
%       IEEE Trans. Image Proc, 2000.
%
%
%   Copyright (c) 2008 Gabriel Peyre

options.null = 0;
n = size(M,1);
Jmax = log2(n)-1;
% number of scales
J = (Jmax-Jmin+1)*2;
bound = getoptions(options, 'bound', 'per');
primal_vm = getoptions(options, 'primal_vm', 4);
dual_vm = getoptions(options, 'dual_vm', 4);

[dX,dY,w] = get_quincunx_filter(primal_vm);
[dX1,dY1,w1] = get_quincunx_filter(dual_vm);

dir = +1;
if size(M,3)>1
    dir=-1;
end

if dir==1
    M0 = M;
else
    M0 = M(:,:,end);
end

[Y,X] = meshgrid(1:n,1:n);

jlist = 0:J-1;
if dir==-1
    jlist = jlist(end:-1:1);
end

W_ini = repmat( reshape(w,1,1,length(w)), n,n );
W1_ini = repmat( reshape(w1,1,1,length(w1)), n,n )/2;
Xn = W_ini; Yn = W_ini;
Xn1 = W_ini; Yn1 = W_ini;

for j=jlist
    j1 = floor(j/2);
    dj = 2^j1;

    % rotate the filters
    [dXj,dYj] = rotate_quincunx(dX,dY, j);
    [dXj1,dYj1] = rotate_quincunx(dX1,dY1, j);

    % build set of indices
    for k=1:length(dX)
        Xn(:,:,k) = X+dXj(k);
        Yn(:,:,k) = Y+dYj(k);
    end
    for k=1:length(dX1)
        Xn1(:,:,k) = X+dXj1(k);
        Yn1(:,:,k) = Y+dYj1(k);
    end

    % boundary conditions
    W = W_ini; W1 = W1_ini;
    if strcmp(bound,'sym')
        I = find( Xn>n | Xn<1 |Yn>n | Yn<1 ); W(I) = 0;
        I = find( Xn1>n | Xn1<1 |Yn1>n | Yn1<1 ); W1(I) = 0;
    end
    Xn = mod(Xn-1,n)+1; Yn = mod(Yn-1,n)+1;
    Xn1 = mod(Xn1-1,n)+1; Yn1 = mod(Yn1-1,n)+1;

    % linear indexes
    In = Xn+(Yn-1)*n; In1 = Xn1+(Yn1-1)*n;

    if dir==1
        % detail coefficients
        d = sum(W,3); d(d==0) = 1;  % 0 should not happend anyway
        D = M0 - sum(M0(In).*W,3) ./ d;
        M(:,:,j+1) = D;
        % update coarse
        d = sum(W1,3); d(d==0) = 1;
        M0 = M0 + .5 * sum(D(In1).*W1,3) ./ d;
    else
        %% NB : since we are in TI mode, needs to be carefull and mix 2
        %% retrieves
        % retrieve coarse
        D = M(:,:,j+1);
        d = sum(W1,3); d(d==0) = 1;
        M0a = M0 - .5 * sum(D(In1).*W1,3) ./ d;
        % other retrieve
        d = sum(W,3); d(d==0) = 1;
        M0 = M(:,:,j+1) + sum(M0a(In).*W,3) ./ d;
        M0 = (M0+M0a)/2;
    end

end
if dir==1
    % record coarse
    M(:,:,end+1) = M0;
else
    M = M0;
end

%%%%%%%%%%%%%%%%%
function [dX,dY,w] = get_quincunx_filter(vm)

switch vm
    case 2
        dX = [0  0 1 -1];
        dY = [1 -1 0  0];
        w  = [1 1 1 1];
    case 4
        dX = [0  0 1 -1 2 2 -2 -2 1 -1 1 -1];
        dY = [1 -1 0  0 1 -1 1 -1 2  2 -2 -2];
        w  = [10 10 10 10 -1 -1 -1 -1 -1 -1 -1 -1];
    case 6
        dX = [0  0 1 -1 2 2 -2 -2 1 -1 1  -1   0 0 3 -3 ...
                3 3 -3 -3 2 2 -2 -2];
        dY = [1 -1 0  0 1 -1 1 -1 2  2 -2 -2   3 -3 0 0 ...
                2 -2 2 -2 3 -3 3 -3];
        w  = [174*ones(1,4) -27*ones(1,8) 2*ones(1,4) 3*ones(1,8)];
    otherwise
        error('Only 2/4 vanishing moments are supported.');

end
w = w/sum(w);

%%%%%%%%%%%%%%%%%
function [dXj,dYj] = rotate_quincunx(dX,dY, j)
A = [1 1;-1 1]; A = A^j;
dXj = A(1,1)*dX + A(1,2)*dY;
dYj = A(2,1)*dX + A(2,2)*dY;

author: gpeyre

repo: matlab-toolboxes


text:
classdef ShFunc_Chomog_EnforceCh_CCstar_eq < ShFunc_Chomog_EnforceCh
    properties
        component
        initial_value = 1;
    end
    methods
        function obj=ShFunc_Chomog_EnforceCh_CCstar_eq(settings,n)
            obj.init(settings);
            obj.compute_Ch_star(settings.TOL, settings.selectiveC_Cstar);
            obj.component = n;
        end

        function computeCostAndGradient(obj,x)
            obj.computePhysicalData(x);
            obj.computeCCstar(x);

            obj.value = obj.value(obj.component);
            obj.gradient = obj.gradient(:,obj.component);
            obj.value = obj.value/obj.initial_value;
            obj.gradient = obj.gradient/obj.initial_value;
            obj.passFilter();
        end
    end
end

author: SwanLab

repo: Swan

save_path: github-repos/MATLAB/SwanLab-Swan


text:
function b = zhpsl ( ap, n, ipvt, b )

%*****************************************************************************80
%
%% ZHPSL solves a complex hermitian system factored by ZHPFA.
%
%  Discussion:
%
%    A division by zero may occur if ZHPCO set RCOND to 0.0
%    or ZHPFA set INFO nonzero.
%
%    To compute
%
%      inverse ( A ) * C
%
%    where C is a matrix with P columns
%
%      call zhpfa(ap,n,ipvt,info)
%
%      if ( info == 0 )
%        do j = 1, p
%          call zhpsl(ap,n,ipvt,c(1,j))
%        end do
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2), the output from ZHPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer IPVT(N), the pivot vector from ZHPFA.
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Loop backward applying the transformations and inverse ( D ) to B.
%
  k = n;
  ik = ( n * ( n - 1 ) ) / 2;

  while ( 0 < k )

    kk = ik + k;
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

        b(1:k-1) = b(1:k-1) + b(k) * ap(ik+1:ik+k-1);

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / ap(kk);
      k = k - 1;
      ik = ik - k;

    else
%
%  2 x 2 pivot block.
%
      ikm1 = ik - ( k - 1 );

      if ( k ~= 2 )

        kp = abs ( ipvt(k) );

        if ( kp ~= k - 1 )
          t      = b(k-1);
          b(k-1) = b(kp);
          b(kp)  = t;
        end

        b(1:k-2) = b(1:k-2) + b(k)   * ap(ik+1:ik+k-2);
        b(1:k-2) = b(1:k-2) + b(k-1) * ap(ikm1+1:ikm1+k-2);

      end
%
%  Apply D inverse.
%
      km1k = ik + k - 1;
      kk = ik + k;
      ak = ap(kk) / conj ( ap(km1k) );
      km1km1 = ikm1 + k - 1;
      akm1 = ap(km1km1) / ap(km1k);
      bk = b(k) / conj ( ap(km1k) );
      bkm1 = b(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;
      ik = ik - ( k + 1 ) - k;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;
  ik = 0;

  while ( k <= n )
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k;
      k = k + 1;
%
%  2 x 2 pivot block.
%
    else

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        ikp1 = ik + k;
        b(k+1) = b(k+1) ...
          + conj ( ap(ikp1+1:ikp1+k-1) ) * transpose ( b(1:k-1) );
        kp = abs ( ipvt(k) );

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k + k + 1;
      k = k + 2;

    end

  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function modelRet = gpsimTest

% GPSIMTEST Test the gradients of the GPSIM model.
% FORMAT
% DESC runs some tests on the code in the GPSIM toolbox to
% test that it is working.
% RETURN model : a cell array of models used for testing.
%
% SEEALSO : modelTest
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% SHEFFIELDML

numGenes = 5;
numProteins = 1;
numData = 4;
times = linspace(0, 5, numData)';
y = randn(numData, numGenes);
yVar = randn(numData, numGenes);
yVar = yVar.*yVar;

numCandGenes = 3;
numCandData = 5;
timesCand = linspace(0, 5, numCandData)';
yCand = randn(numCandData, numCandGenes);
yCandVar = randn(numCandData, numCandGenes);
yCandVar = yCandVar.*yCandVar;

options = gpsimOptions;

model = gpsimCreate(numGenes, numProteins, times, y, yVar, options);

fprintf('Standard Parameter test:\n');
modelGradientCheck(model);

model = gpsimAddCandidate(model, numCandGenes, timesCand, yCand, yCandVar, options);

fprintf('Candidate Parameter test:\n');
model.type = 'gpsimCandidate';
modelGradientCheck(model);
model.type = 'gpsim';

modelRet = model;

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
% BIG PICTURE TODOS (01/09/07 ras):
% A lot of the previous things on the ToDo list have been implemented in
% 2006. Here are some things that remain.
%
%
%  * Spaces: add ability to call external matlab function to
%            get coords (e.g., for Talairach; may also be easier
%            for flattening). I think the format of any function
%            called in this way should be:
%               newCoords = myFunction(oldCoords,mr);
%
%   * Import GLM tools to work on mr objects
%
%   * Flattening: either implement the 'flatten-on-the-fly' code from
%   mrFlatMesh, or else come up with a tool to flatten using the meshes.
%   Given that we were able to use Jonas Larsson's SurfRelax code for
%   mesh inflation, maybe we could also port over his flattening routines?
%   (This would appeal a bit more to me, because having a non-disc flat map
%   makes maps easier to interpret, allowing the viewer to relate the shape
%   of the cuts to positions on the 3D mesh. May also reduce distortions?)
%


text:
%
%   simple example to show the usage of l1_ls
%

% problem data
A  = [1    0    0   0.5;...\
      0    1  0.2   0.3;...\
      0  0.1    1   0.2];
x0 = [1 0 1 0]';    % original signal
y  = A*x0;          % measurements with no noise
lambda = 0.01;      % regularization parameter
rel_tol = 0.01;     % relative target duality gap

[x,status]=l1_ls(A,y,lambda,rel_tol);

author: hiroyuki-kasai

repo: ClassifierToolbox


text:
function [intersection_nodes] = ...
                    get_unique_node_xy(parsed_osm, intersection_node_indices)
% get the x,y coordinates of unique nodes at road intersections
%
% 2010.11.20 (c) Ioannis Filippidis, jfilippidis@gmail.com

ids = parsed_osm.node.xy(:, intersection_node_indices);
xys = parsed_osm.node.xy(:, intersection_node_indices);

intersection_nodes.id = ids;
intersection_nodes.xys = xys;

author: johnyf

repo: openstreetmap


text:
function Y = squeeze(X)
%SQUEEZE Remove singleton dimensions from a tensor.
%
%   Y = SQUEEZE(X) returns a tensor Y with the same elements as
%   X but with all the singleton dimensions removed.  A singleton
%   is a dimension such that size(X,dim)==1.
%
%   If X has *only* singleton dimensions, then Y is a scalar.
%
%   Examples
%   squeeze( tenrand([2,1,3]) ) %<-- returns a 2-by-3 tensor
%   squeeze( tenrand([1 1]) ) %<-- returns a scalar
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if all(X.size > 1)
  % No singleton dimensions to squeeze
  Y = X;
else
  idx = find(X.size > 1);
  if numel(idx) == 0
    % Scalar case - only singleton dimensions
    Y = X.data;
  else
    siz = X.size(idx);
    Y = tensor(squeeze(X.data),siz);
  end
end

return;

author: andrewssobral

repo: mtt

save_path: github-repos/MATLAB/andrewssobral-mtt


text:
function vissurf = get_cube_visible_surface(cube, vp)

% cube surfaceid
% botid = 1; % 1: bottom
% frtid = 2; % 2: front
% lftid = 3; % 3: left
% rhtid = 4; % 4: right
% bckid = 5; % 5: back
% topid = 6; % 6: top

% cube junc3 id
%   5---6   1------2
%  /   /|   |      |
% 1---2 8   3------4    ....
% |   |/     \    /
% 3---4       7--8
%
% 1: front,top,left
% 2: front,top,right
% ...
% 8: back,bottom,right

%% determine visible surfaces

regionid1 = getregionid(cube.junc3(1).pt(1), cube.junc3(1).pt(2), vp);
regionid2 = getregionid(cube.junc3(2).pt(1), cube.junc3(2).pt(2), vp);
regionid3 = getregionid(cube.junc3(3).pt(1), cube.junc3(3).pt(2), vp);
regionid4 = getregionid(cube.junc3(4).pt(1), cube.junc3(4).pt(2), vp);

% vissurf = zeros(1,5); % [1234(front), 1256(top), 3478(bot), 1357(left), 2468(right)]
vissurf = zeros(1,6); % 3478(bot), 1234(front), 1357(left), 2468(right), 5678(back), 1256(top)

%bot
if (regionid3==1 || regionid3==2) && (regionid4==1 || regionid4==2)
    vissurf(1) = 1;
end

%front -- always visible
vissurf(2) = 1;

%left
if (regionid1==2 || regionid1==4) && (regionid3==2 || regionid3==4)
    vissurf(3) = 1;
end

%right
if (regionid2==1 || regionid2==3) || (regionid4==1 || regionid4==3)
    vissurf(4) = 1;
end

%back -- always invisible
vissurf(5) = 0;

%top
if (regionid1==3 || regionid1==4) && (regionid2==3 || regionid2==4)
    vissurf(6) = 1;
end

%% old
% % % regionid1 = getregionid(cube.junc3(1).pt(1), cube.junc3(1).pt(2), vp);
% % % regionid2 = getregionid(cube.junc3(2).pt(1), cube.junc3(2).pt(2), vp);
% % % regionid3 = getregionid(cube.junc3(3).pt(1), cube.junc3(3).pt(2), vp);
% % % regionid4 = getregionid(cube.junc3(4).pt(1), cube.junc3(4).pt(2), vp);
% % %
% % % vissurf = zeros(1,5); % [1234(front), 1256(top), 3478(bot), 1357(left), 2468(right)]
% % % %front -- always visible
% % % vissurf(1) = 1;
% % % %top
% % % if (regionid1==3 || regionid1==4) && (regionid2==3 || regionid2==4)
% % %     vissurf(2) = 1;
% % % end
% % % %bot
% % % if (regionid3==1 || regionid3==2) && (regionid4==1 || regionid4==2)
% % %     vissurf(3) = 1;
% % % end
% % % %left
% % % if (regionid1==2 || regionid1==4) && (regionid3==2 || regionid3==4)
% % %     vissurf(4) = 1;
% % % end
% % % %right
% % % if (regionid2==1 || regionid2==3) || (regionid4==1 || regionid4==3)
% % %     vissurf(5) = 1;
% % % end


text:
function ylag = lagmatrix( y, lags )
    n = length(y);
    N = max(lags);

    ylag = convmtx(y, N+1);
    ylag = ylag(1:n, lags+1);

end

author: BUNPC


text:
function h = draw_3d(A,b,C,d,obstacles,lb,ub)
  import iris.drawing.drawPolyFromVertices;
  import iris.thirdParty.polytopes.*;

  h = figure(2);
  cla
  hold on
  if ~isempty(obstacles)
    for j = 1:numel(obstacles)
      drawPolyFromVertices(obstacles{j},'k','FaceAlpha',0.5);
    end
  end

  if ~isempty(A)
    V = lcon2vert(A, b);
    drawPolyFromVertices(V', 'r');
  end
  th = linspace(0,2*pi,20);
  y = [cos(th);sin(th);zeros(size(th))];
  for phi = linspace(0,pi,10)
    T = makehgtform('xrotate', phi);
    R = T(1:3,1:3);
    y = [y, R * y];
  end
  x = bsxfun(@plus, C*y, d);
  drawPolyFromVertices(x, 'b', 'FaceAlpha', 1)
  xlim([lb(1),ub(1)])
  ylim([lb(2),ub(2)])
  zlim([lb(3),ub(3)])
  camtarget(0.5*(lb+ub))
  campos([lb(1)-2,lb(2)-2,ub(3)])
%   axis off;
end

author: rdeits

repo: iris-distro

save_path: github-repos/MATLAB/rdeits-iris-distro


text:
function [L, U] = factor(A, rho)
    [m, n] = size(A);
    if m >= n   % assuming this case is more serious in my application
       L = chol(A'*A + rho*eye(size(A,2)), 'lower');
    else
       L = chol(speye(m) + 1/rho*(A*A'), 'lower');
    end

    U = L';
end

author: zhangqianqianQQ

repo: MachineVisionAlgorithm


text:
function varargout = log2(varargin)

varargout{1} = log(varargin{1})/log(2);

author: yalmip

repo: YALMIP

save_path: github-repos/MATLAB/yalmip-YALMIP


text:
function R = p2p_optimum(a, n_vals, eps)
% Compute the achievable sum rate (at the symmetrical rate point) based on the point-to-point optimal code
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: the optimal rates for the blocklengths specified in n_vals.

    % Compute log binomial coefficients
    log_b = binomial_coeff(max(n_vals));

    R = zeros(length(n_vals),1);

    for i = 1:length(n_vals)
        n = n_vals(i);
        m = 0:n;  % Number of occurrence of the largest joint probability mass
        % log_Pr stores all the log probabilities in descending order
        log_Pr = m*log(a)+(n-m)*log(1/3*(1-a));

        % Compute the optimal rate
        log_sum_Pr = log_b{n+1}(n+1)+log_Pr(n+1);
        k = n;
        while exp(log_sum_Pr) < 1 - eps
            % Add up the probabilities of the strings until the total probability >= 1 - eps
            k = k - 1;
            log_sum_Pr = my_logsumexp([log_sum_Pr log_b{n+1}(k+1)+(n-k)*log(3)+log_Pr(k+1)]);
        end
        d = 1 - eps - (exp(log_sum_Pr) - exp(log_b{n+1}(k+1)+(n-k)*log(3)+log_Pr(k+1)));
        M1 = my_logsumexp(log_b{n+1}(k+2:n+1)+(n-(k+1:n))*log(3));
        M2 = log(d) - log_Pr(k+1);
        R(i) = 1/n*my_logsumexp([M1 M2]);
    end
end

author: yp-mit

repo: spectre

save_path: github-repos/MATLAB/yp-mit-spectre


text:
function nodevol=nodevolume(node,elem, evol)
%
% nodevol=nodevolume(node,elem)
%
% calculate the volumes of the cells in the barycentric dual-mesh
% (this is different from the Voronoi cells, which blong to the
% circumcentric dual mesh)
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2009/12/31
%
% input:
%    node:  node coordinates
%    elem:  element table of a mesh
%
% output:
%    nodevol:   volume values for all nodes
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

dim=4;
if(size(elem,2)==3) dim=3; end

if(nargin<3)
    evol=elemvolume(node,elem(:,1:dim));
end

elemnum=size(elem,1);
nodenum=size(node,1);
nodevol=zeros(nodenum,1);
for i=1:elemnum
      nodevol(elem(i,1:dim))=nodevol(elem(i,1:dim))+evol(i);
end
nodevol=nodevol/dim;

author: fangq


text:
function [sys,x0,str,ts] = quadrotor_dynamics(t,x,u,flag, quad, x0, groundflag)
    % Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04
    % A simulation of idealised X-4 Flyer II flight dynamics.
    % version 2.0 2005 modified to be compatible with latest version of Matlab
    % version 3.0 2006 fixed rotation matrix problem
    % version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring
    % version 5.0 8/8/11, simplified and restructured
   % version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug

    warning off MATLAB:divideByZero

    global groundFlag;

    % New in version 2:
    %   - Generalised rotor thrust model
    %   - Rotor flapping model
    %   - Frame aerodynamic drag model
    %   - Frame aerodynamic surfaces model
    %   - Internal motor model
    %   - Much coolage

    % Version 1.3
    %   - Rigid body dynamic model
    %   - Rotor gyroscopic model
    %   - External motor model

    %ARGUMENTS
    %   u       Reference inputs                1x4
    %   tele    Enable telemetry (1 or 0)       1x1
    %   crash   Enable crash detection (1 or 0) 1x1
    %   init    Initial conditions              1x12

    %INPUTS
    %   u = [N S E W]
    %   NSEW motor commands                     1x4

    %CONTINUOUS STATES
    %   z      Position                         3x1   (x,y,z)
    %   v      Velocity                         3x1   (xd,yd,zd)
    %   n      Attitude                         3x1   (Y,P,R)
    %   o      Angular velocity                 3x1   (wx,wy,wz)
    %   w      Rotor angular velocity           4x1
    %
    % Notes: z-axis downward so altitude is -z(3)

    %CONTINUOUS STATE MATRIX MAPPING
    %   x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4]

    %INITIAL CONDITIONS
    n0 = [0 0 0];               %   n0      Ang. position initial conditions    1x3
    v0 = [0 0 0];               %   v0      Velocity Initial conditions         1x3
    o0 = [0 0 0];               %   o0      Ang. velocity initial conditions    1x3
    init = [x0 n0 v0 o0];       % x0 is the passed initial position 1x3
    groundFlag = groundflag;

    %CONTINUOUS STATE EQUATIONS
    %   z` = v
    %   v` = g*e3 - (1/m)*T*R*e3
    %   I*o` = -o X I*o + G + torq
    %   R = f(n)
    %   n` = inv(W)*o

    % Dispatch the flag.
    %
    switch flag
        case 0
            [sys,x0,str,ts]=mdlInitializeSizes(init, quad); % Initialization
        case 1
            sys = mdlDerivatives(t,x,u, quad); % Calculate derivatives
        case 3
            sys = mdlOutputs(t,x, quad); % Calculate outputs
        case { 2, 4, 9 } % Unused flags
            sys = [];
        otherwise
            error(['Unhandled flag = ',num2str(flag)]); % Error handling
    end
end % End of flyer2dynamics

%==============================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the
% S-function.
%==============================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(init, quad)
    %
    % Call simsizes for a sizes structure, fill it in and convert it
    % to a sizes array.
    %
    sizes = simsizes;
    sizes.NumContStates  = 12;
    sizes.NumDiscStates  = 0;
    sizes.NumOutputs     = 12;
    sizes.NumInputs      = 4;
    sizes.DirFeedthrough = 0;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    %
    % Initialize the initial conditions.
    x0 = init;
    %
    % str is an empty matrix.
    str = [];
    %
    % Generic timesample
    ts = [0 0];

    if quad.verbose
        disp(sprintf('t\t\tz1\t\tz2\t\tz3\t\tn1\t\tn2\t\tn3\t\tv1\t\tv2\t\tv3\t\to1\t\to2\t\to3\t\tw1\t\tw2\t\tw3\t\tw4\t\tu1\t\tu2\t\tu3\t\tu4'))
    end
end % End of mdlInitializeSizes.

%==============================================================
% mdlDerivatives
% Calculate the state derivatives for the next timestep
%==============================================================
%
function sys = mdlDerivatives(t,x,u, quad)
    global a1s b1s groundFlag

    %CONSTANTS
    %Cardinal Direction Indicies
    N = 1;                      %   N       'North'                             1x1
    E = 2;                      %   S       'South'                             1x1
    S = 3;                      %   E       'East'                              1x1
    W = 4;                      %   W       'West'                              1x1

    D(:,1) = [quad.d;0;quad.h];          %   Di      Rotor hub displacements             1x3
    D(:,2) = [0;quad.d;quad.h];
    D(:,3) = [-quad.d;0;quad.h];
    D(:,4) = [0;-quad.d;quad.h];

    %Body-fixed frame references
    e1 = [1;0;0];               %   ei      Body fixed frame references         3x1
    e2 = [0;1;0];
    e3 = [0;0;1];

    %EXTRACT ROTOR SPEEDS FROM U
    w = u(1:4);

    %EXTRACT STATES FROM X
    z = x(1:3);   % position in {W}
    n = x(4:6);   % RPY angles {W}
    v = x(7:9);   % velocity in {W}
    o = x(10:12); % angular velocity in {W}

    %PREPROCESS ROTATION AND WRONSKIAN MATRICIES
    phi = n(1);    % yaw
    the = n(2);    % pitch
    psi = n(3);    % roll

    % rotz(phi)*roty(the)*rotx(psi)
    R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);   %BBF > Inertial rotation matrix
         cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);
         -sin(the)         sin(psi)*cos(the)                            cos(psi)*cos(the)];

    %Manual Construction
    %     Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1];   % RZ %Rotation mappings
    %     Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)];   % RY
    %     Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)];   % RX
    %     R = Q3*Q2*Q1    %Rotation matrix
    %
    %    RZ * RY * RX
    iW = [0        sin(psi)          cos(psi);             %inverted Wronskian
          0        cos(psi)*cos(the) -sin(psi)*cos(the);
          cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);
    if any(w == 0)
        % might need to fix this, preculudes aerobatics :(
        % mu becomes NaN due to 0/0
        error('quadrotor_dynamics: not defined for zero rotor speed');
    end

    %ROTOR MODEL
    for i=[N E S W] %for each rotor
        %Relative motion

        Vr = cross(o,D(:,i)) + v;
        mu = sqrt(sum(Vr(1:2).^2)) / (abs(w(i))*quad.r);  %Magnitude of mu, planar components
        lc = Vr(3) / (abs(w(i))*quad.r);   %Non-dimensionalised normal inflow
        li = mu; %Non-dimensionalised induced velocity approximation
        alphas = atan2(lc,mu);
        j = atan2(Vr(2),Vr(1));  %Sideslip azimuth relative to e1 (zero over nose)
        J = [cos(j) -sin(j);
            sin(j) cos(j)];  %BBF > mu sideslip rotation matrix

        %Flapping
        beta = [((8/3*quad.theta0 + 2*quad.theta1)*mu - 2*(lc)*mu)/(1-mu^2/2); %Longitudinal flapping
            0;];%sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; %Lattitudinal flapping (note sign)
        beta = J'*beta;  %Rotate the beta flapping angles to longitudinal and lateral coordinates.
        a1s(i) = beta(1) - 16/quad.gamma/abs(w(i)) * o(2);
        b1s(i) = beta(2) - 16/quad.gamma/abs(w(i)) * o(1);

        %Forces and torques
        T(:,i) = quad.Ct*quad.rho*quad.A*quad.r^2*w(i)^2 * [-cos(b1s(i))*sin(a1s(i)); sin(b1s(i));-cos(a1s(i))*cos(b1s(i))];   %Rotor thrust, linearised angle approximations
        Q(:,i) = -quad.Cq*quad.rho*quad.A*quad.r^3*w(i)*abs(w(i)) * e3;     %Rotor drag torque - note that this preserves w(i) direction sign
        tau(:,i) = cross(T(:,i),D(:,i));    %Torque due to rotor thrust
    end

    %RIGID BODY DYNAMIC MODEL
    dz = v;
    dn = iW*o;

    dv = quad.g*e3 + R*(1/quad.M)*sum(T,2);

    % vehicle can't fall below ground
    if groundFlag && (z(3) > 0)
        z(3) = 0;
        dz(3) = 0;
    end
    do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques
    sys = [dz;dn;dv;do];   %This is the state derivative vector
end % End of mdlDerivatives.

%==============================================================
% mdlOutputs
% Calculate the output vector for this timestep
%==============================================================
%
function sys = mdlOutputs(t,x, quad)

    %TELEMETRY
    if quad.verbose
        disp(sprintf('%0.3f\t',t,x))
    end

    % compute output vector as a function of state vector
    %   z      Position                         3x1   (x,y,z)
    %   v      Velocity                         3x1   (xd,yd,zd)
    %   n      Attitude                         3x1   (Y,P,R)
    %   o      Angular velocity                 3x1   (Yd,Pd,Rd)

    n = x(4:6);   % RPY angles
    phi = n(1);    % yaw
    the = n(2);    % pitch
    psi = n(3);    % roll

    % rotz(phi)*roty(the)*rotx(psi)
    R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);   %BBF > Inertial rotation matrix
         cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);
         -sin(the)         sin(psi)*cos(the)                            cos(psi)*cos(the)];

    iW = [0        sin(psi)          cos(psi);             %inverted Wronskian
          0        cos(psi)*cos(the) -sin(psi)*cos(the);
          cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);

    % return velocity in the body frame
    sys = [ x(1:6);
            inv(R)*x(7:9);   % translational velocity mapped to body frame
            iW*x(10:12)];    % RPY rates mapped to body frame
    %sys = [x(1:6); iW*x(7:9);  iW*x(10:12)];
    %sys = x;
end
% End of mdlOutputs.

author: petercorke

repo: robotics-toolbox-matlab


text:
function varargout = pcircle(center, radius, varargin)
%
% POINTS = PCIRCLE(center, radius, varargin)
% A simple function for drawing a circle pixel by pixel on a given
% region of interest (roi).
%
% INPUT:
% center:       center of the circle in [x0 y0] format
% radius:       radius of the circle (in pixels)
% varargin:     rectangular ROI defined with:
%               varagin{1} = (x1, y1) and varagin{2} = (x2, y2)
%
% OUTPUT:
% pixels:       list of circle coordinates in the format (x, y)

if nargin == 2
    roi1 = [Inf Inf];
    roi2 = [-Inf -Inf];
elseif nargin == 4
    roi1 = varargin{1};
    roi2 = varargin{2};
else
    error('Invalid number of inputs: type ''help'' PCIRCLE');
end

roil = min(roi1(1),roi2(1));
roir = max(roi1(1),roi2(1));
roib = min(roi1(2),roi2(2));
roiu = max(roi1(2),roi2(2));
x0 = center(1);
y0 = center(2);
nseg = ceil(2*pi*radius);
theta = 0:(2*pi/nseg):(2*pi);
x(:,1) = radius*cos(theta) + x0;
y(:,1) = radius*sin(theta) + y0;
pixels = round([x y]);

% delete repeated pixels
dpixels = abs(diff(pixels(:,1))) + abs(diff(pixels(:,2)));
ind = find(dpixels == 0);
pixels(ind,:) = [];
if (pixels(1,1) == pixels(end,1) & pixels(1,2) == pixels(end,2))
    pixels(end,:) = [];
end
% delete pixels out of the ROI (<=0)
rows = find(pixels(:,1) < roil | pixels(:,1) > roir);
pixels(rows,:) = [];
cols = find(pixels(:,2) < roib | pixels(:,2) > roiu);
pixels(cols,:) = [];

if nargout == 0   % draw circles
%    plot(x0, y0, 'xr', 'LineWidth',2); % also plots the center
    plot(pixels(:,1), pixels(:,2), 'r', 'LineWidth',2);
else
    varargout{1} = pixels;
end

author: Sable