text:
function Main()
% This program is a stand-alone toy example of radiation treatment planning
% (RTP) optimization for a brain tumor case.
%     The program generates a toy patient head model using scaled and shifted
% ellipsoids and p-norm sublevel sets to represent all the volumes of
% interest (VOIs), including skin, eyes, optic nerves, brain stem, a tumor
% and artificial "shell" around the tumor. These VOI sublevel sets are then
% discretized into PointClouds, by retaining the points in a discrete 3-D
% grid of volume elements (voxels) that lie in the 1-sublevel set of the
% VOI's p-norm model.
%     Then we compute candidate beam directions by firing beams at random
% points on the tumor surface from a set of about 150 nodes, located
% uniformly in a spherical region of radius 80cm from the patient's head.
%     Then we compute the dose vectors associated with each beam in each of
% the VOIs, by evaluating a dose function in a cylindrical region around each
% beam direction. The dose function is a crude model of the roll-off with
% depth, and radial diffusion or scatter. For each candidate beam, this
% rolloff/scatter function is evaluated for each voxel in each VOI. Thus
% each beam results in a column of the patient dose matrix.
% Finally, we set the min and max dosages for each VOI.
%     We then pass the dose matrices and min and max specs to one of two
% solvers (CPLEX or ADMM), which find the optimal set of beams and
% intensities (beam weights) for this patient case. This is the optimal
% "plan". The optimization formulation is based on a problem from the
% Stanford EE364b final exam from 2011, by Stephen Boyd and Eric Chu.
%     Then we visualize the optimal plan by plotting the dose levels for
% all the voxels of each VOI, and the dose volume histograms.
%     The program consists of this Main() function and several "classes":
% (1) Patient: cell array of VOIs, names, etc
% (2) VOI: p-norm model and associated point cloud and boundary, dose specs
% (3) Beams: beam heads,tails,dose vector computation + nodes + collimators
% (4) Model: p-norm models and gradients
% (5) Point Cloud & 3-D geometry functions
% (6) Optimizer functions: CPLEX wrapper and ADMM solver (ala EE364b)
%
% Reference:
% H. Hindi, "A Tutorial on Optimization Methods for Cancer Radiation
% Treatment Planning," Proc. American Contr. Conf, 2013.
%
% Written by Haitham Hindi, 2012/01/18
%
% DISCLAIMER: This code is intended for algorithm research purposes
% only!! Author makes no claims as to the realism, accuracy, or correctness
% of ANY part of this code, including (but not limited to):
% patient anatomy and dimensions; plan safety; beam weights,doses,physics;
% dose units, dose specs; plan quality, evaluation metrics. The author is
% an engineer with no medical training whatsoever! This code is supplied
% as-is, with no guarantee of correctness, and the author accepts no
% responsibility for any damage or false conclusions from the use of this
% code. Of course, we would appreciate hearing about any bugs you might find.

    clear, clc, close all
    rng('default');
    rng(2);

    %====================================================
    % data for patient and beams
    %====================================================
    % patient specs
    PatientName     = 'Toy Brain Tumor Case';
    PatientThetaxyz = [pi/2;0;0];
    PatientOffset   = [0;0;0];
    PatientAxLims   = [-15 15];

    % VOI specs (VOIs are scaled shifted p-norm sublevel sets)
    Target=1;Shell=2;BrStm=3;EyeL=4;EyeR=5;OptNrvL=6;OptNrvR=7;Skin=8;
    VoxSizeSkin      = 1;
    VoxSizeOrgans    = 0.2;%0.15%0.113;
    SampFactorSkin   = 1;
    SampFactorOrgans = 1;
    VOINames      ={'Target','Shell','BrStm','EyeL','EyeR','OptNrvL','OptNrvR','Skin'};
    dmaxs         =[50      , 25    ,20     , 20   , 20   , 20      , 20      , 30];
    dmins         =[40      , 0     , 0     ,  0   ,  0   ,  0      ,  0      ,  0];
    VOINumbers    =[ 1,       2,      3,      4,     5,     6,        7,        8];
    VOITypes      ={'v',     's',    'v',    'v',   'v',   'v',      'v',      's'};
    VOIColors     ={'k',     'r',    'g',    'b',   'b',   'c',      'c',      'y'};
    VOILineStyles ={'-',     '-',    '-',    '-',   '--',  '-',      '--',     '-'};
    VOIVoxSizes   =[VoxSizeOrgans*ones(7,1);VoxSizeSkin];
    VOISampFactors=[SampFactorOrgans*ones(7,1);SampFactorSkin];
    VOIxyzScales  =[1.2*[1;1;1],1.56*[1;1;1],[1.5;1.5;6],1.7*[1;1;1],1.7*[1;1;1],[0.7;4;0.7],[0.7;4;0.7],[11;11;11]];
    VOIpNorms     =[2;  2;  3;  2;  2;  2;  2;  3];
    VOIThetaxyz   =[[0;0;0],[0;0;0],[-pi/8;0;0],[0;0;0],[0;0;0],[pi/6;-pi/6;0],[pi/6;pi/6;0],[0;0;0]];
    VOIOffsets    =[[3;-3;0],[3;-3;0],[0;-3;-5],[-4;8;4],[4;8;4],[-2;3;2],[2;3;2],[0;0;0]];
    VOIHistBins   = 0:max(dmaxs)/50:1.4*max(dmaxs);

    % beam specs
    NPtsSphereParam = 7%8%18
    NodeLocs = GetPointsOnUnitSphere1000(NPtsSphereParam);
    NodeLocs = NodeLocs(find(NodeLocs(:,3)>=-0.6),:);% remove bottom (can't go under patient)
    NodeLocs = NodeLocs(find(NodeLocs(:,2)<= 0.6),:);% remove front  (can't go through patient)
    NodeLocs = 80*NodeLocs;% scale to 80cm radius
    NNodes = size(NodeLocs,1)
    NodesAxLims = 85*[-1 1 -1 1 -1 1];
    Collimators = [5; 7.5; 10; 12.5; 15; 20; 25];%; 30; 35; 40; 50; 60];
    Collimators = Collimators/10; % scale to cm
    NCollimators= length(Collimators);
    NBeamlets = 5;
    NPtsPerAx = 50;
    AxLength = 15;

    %====================================================
    % create patient and beams, solve optimization problem, visualize results
    %====================================================
    % get patient
    disp(['Computing patient model...'])
    Patient = PatientCreate();
    Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                          VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                          VOIVoxSizes,VOISampFactors,VOIHistBins,...
                          VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets);
    Patient = PatientRotateTranslate(Patient,PatientThetaxyz,PatientOffset);

    figure
    PatientPlot(Patient)

    % get beams
    disp(['Computing beam directions...'])
    Beams = BeamsCreate();
    Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,Patient.VOIs{Skin}.Model.f,Patient.VOIs{Target}.PointCloudBdy);

    PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators);
    PlotPatientBeamDirectionsAndNodes(Patient,Beams);
    %figure,hold on
    %PatientPlot(Patient)
    %[GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength);
    %PlotFewRandomBeamDoseMaps(Beams.Tails,Beams.Heads,Beams.Widths,3,GridPts)

    % compute dose matrices of all VOIs and set upper and lower bounds
    Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs);

    % construct the Linear Program matrices and solve for optimal beam
    % weights using CLEX or ADMM
    Patient = PatientApplyOptimization(Patient);

    % plot optimal beam weights, constraints, and dose volume histograms
    PatientEvaluate(Patient);

return

%====================================================
% Patient Class: mainly cell array of VOIs, names, axis limits
%====================================================
function Patient = PatientCreate()
    Patient.Name     = [];
    Patient.VOINames = [];
    Patient.NumVOIs  = [];
    Patient.VOIs     = [];
    Patient.AxLims   = [];
    Patient.xBeamWeights = [];
return

function Patient = PatientInit(Patient,PatientName,PatientAxLims,...
                               VOINames,VOINumbers,VOITypes,VOIColors,VOILineStyles,...
                               VOIVoxSizes,VOISampFactors,VOIHistBins,...
                               VOIxyzScales,VOIpNorms,VOIThetaxyz,VOIOffsets)
    Patient.Name = PatientName;
    Patient.VOINames = VOINames;
    Patient.NumVOIs =length(VOINames);
    Patient.VOIs = {};
    for i=1:Patient.NumVOIs
        Modi            = ModelInit(ModelCreate(),VOIxyzScales(:,i),VOIpNorms(i));
        VOIi            = VOIInit(VOICreate(),VOINames{i},VOINumbers(i),VOITypes{i},VOIColors{i},VOILineStyles{i},VOIVoxSizes(i),VOISampFactors(i),Modi,VOIHistBins);
        Patient.VOIs{i} = VOIRotateTranslate(VOIi,VOIThetaxyz(:,i),VOIOffsets(:,i));
    end
    Patient.AxLims = PatientAxLims;
return

function Patient = PatientPrepareForOptimization(Patient,Beams,dmins,dmaxs)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDoseAMatrix(Patient.VOIs{i},Beams);
        Patient.VOIs{i} = VOISetDoseMinMax(Patient.VOIs{i},dmins(i),dmaxs(i));
    end
return

function Patient = PatientApplyOptimization(Patient)
    % construct LP matrices
    A    = [];dmin = [];dmax = [];
    for i=1:Patient.NumVOIs
        A    = [A;Patient.VOIs{i}.ADose];
        dmin = [dmin;Patient.VOIs{i}.DoseMin];
        dmax = [dmax;Patient.VOIs{i}.DoseMax];
    end

    % compute optimal beam weights
    %%%Patient.xBeamWeights = OptimizeBeamsCPLEX(A,dmin,dmax);
    Patient.xBeamWeights = OptimizeBeamsADMM(A,dmin,dmax);

    % compute resulting dose on each VOI
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIGetDose(Patient.VOIs{i},Patient.xBeamWeights);
    end
return

function PatientEvaluate(Patient)
    figure,stem(Patient.xBeamWeights),title('Beam Weights')
    figure,PatientPlotConstraints(Patient);
    figure,PatientPlotDVH(Patient);
return

function Patient = PatientRotateTranslate(Patient,Thetaxyz,Offset)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIRotateTranslate(Patient.VOIs{i},Thetaxyz,Offset);
    end
return

function Patient = PatientAffineTransform(Patient,A,b)
    for i=1:Patient.NumVOIs
        Patient.VOIs{i} = VOIAffineTransform(Patient.VOIs{i},A,b);
    end
return

function PatientPlot(Patient)
    hold on
    for i=1:Patient.NumVOIs
        VOIPlot(Patient.VOIs{i},Patient.AxLims);
    end
    title(Patient.Name),legend(Patient.VOINames)
    xlabel('x'),ylabel('y'),zlabel('z')
    view(127.5,30)
    %view(3)
return

function PatientPlotConstraints(Patient)
    nPlotsPerCol = ceil(Patient.NumVOIs/2);
    for i=1:Patient.NumVOIs
        subplot(nPlotsPerCol,2,i),VOIPlotMinMaxConstraints(Patient.VOIs{i});
    end
return

function PatientPlotDVH(Patient)
    hold on,title('DVHs'),grid on, axis([0,max(Patient.VOIs{1}.DVHBins),0,1.1])
    for i=1:Patient.NumVOIs
        [DVHi,Histi] = GetDVH(Patient.VOIs{i}.Dose,Patient.VOIs{i}.DVHBins);
        plot(Patient.VOIs{i}.DVHBins,DVHi,Patient.VOIs{i}.Color);
    end
    legend(Patient.VOINames)
return

function PlotPatientBeamDirectionsAndNodes(Patient,Beams)
    figure,hold on
    PatientPlot(Patient)
    PlotNodes(Beams.NodeLocs,Beams.NodesAxLims);
    PlotBeams(Beams.Tails0,Beams.Heads,1);
    figure,hold on
    PatientPlot(Patient)
    PlotBeams(Beams.Tails,Beams.Heads,1);
return

%====================================================
% VOI Class: has name etc, point cloud model, point cloud, dose matrix, dose specs
%====================================================
function VOI = VOICreate()
    VOI.Name          = [];
    VOI.Number        = [];
    VOI.Type          = [];
    VOI.Color         = [];
    VOI.LineStyle     = [];
    VOI.VoxSize       = [];
    VOI.SubSampFactor = [];
    VOI.Model         = [];
    VOI.PointCloud    = [];
    VOI.PointCloudBdy = [];
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = [];
return

function VOI = VOIInit(VOI,Name,Number,Type,Color,LineStyle,VoxSize,SubSampFactor,Model,HistBins)
    VOI.Name          = Name;
    VOI.Number        = Number;
    VOI.Type          = Type;
    VOI.Color         = Color;
    VOI.LineStyle     = LineStyle;
    VOI.VoxSize       = VoxSize;
    VOI.SubSampFactor = SubSampFactor;
    VOI.Model         = Model;
    VOI.PointCloud    = ModelToPointCloud(Model,VoxSize,SubSampFactor);
    VOI.PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor);
    VOI.ADose         = [];
    VOI.DoseMin       = [];
    VOI.DoseMax       = [];
    VOI.Dose          = [];
    VOI.DVHBins       = HistBins;
return

function VOI = VOIAffineTransform(VOI,A,b)
    VOI.Model         = ModelAffineTransform(VOI.Model,A,b);
    VOI.PointCloud    = PointCloudAffineTransform(VOI.PointCloud,A,b);
    VOI.PointCloudBdy = PointCloudAffineTransform(VOI.PointCloudBdy,A,b);
return

function VOI = VOIRotateTranslate(VOI,Thetaxyz,Offset)
    VOI.Model         = ModelRotateTranslate(VOI.Model,Thetaxyz,Offset);
    VOI.PointCloud    = PointCloudRotateTranslate(VOI.PointCloud,Thetaxyz,Offset);
    VOI.PointCloudBdy = PointCloudRotateTranslate(VOI.PointCloudBdy,Thetaxyz,Offset);
return

function VOIPlot(VOI,AxisLimits)
    Symbol = ['.',VOI.Color];
    PlotCloud(VOI.PointCloudBdy,Symbol,AxisLimits);
return

function VOI = VOIGetDoseAMatrix(VOI,Beams)
    if VOI.Type =='v' % full volume
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloud);
    else              % boundary shell
        VOI.ADose = GetBeamDosageVectors8(Beams.Tails,Beams.Heads,Beams.Widths,VOI.PointCloudBdy);
    end
    disp(['NPts-',VOI.Name,'=',num2str(size(VOI.ADose,1))])
return

function VOI = VOISetDoseMinMax(VOI,dmin,dmax)
    if VOI.Type =='v' % full volume
        NPtsVOI = size(VOI.PointCloud,1);
    else              % boundary shell
        NPtsVOI = size(VOI.PointCloudBdy,1);
    end
    VOI.DoseMin = dmin*ones(NPtsVOI,1);
    VOI.DoseMax = dmax*ones(NPtsVOI,1);
return

function VOI = VOIGetDose(VOI,xBeamWeights)
    VOI.Dose = VOI.ADose*xBeamWeights;
return

function VOIPlotMinMaxConstraints(VOI)
    nVox = length(VOI.DoseMin);
    vox  = 1:nVox;
    plot(vox,VOI.Dose,'*b',...
         vox,VOI.DoseMin,'--r',...
         vox,VOI.DoseMax,'--r')
    axis([0 nVox  0  1.2*max(VOI.DoseMax) ])
    legend(VOI.Name)
    xlabel('voxels'),ylabel('dose')
return

function [DVH,Hist] = GetDVH(DosageProfile,Bins);
    Hist = hist(DosageProfile,Bins);
    Hist = Hist/sum(Hist);
    HistCum = cumsum(Hist);
    DVH = 1-HistCum;
    DVH = [1;DVH(1:(length(DVH)-1))'];
return

%====================================================
% Beam Class: has beams, nodes, collimators, dose computation
%====================================================
function Beams = BeamsCreate()
    Beams.NodeLocs    = [];
    Beams.NodesAxLims = [];
    Beams.Collimators = [];
    Beams.NBeamlets   = [];
    Beams.Tails0= [];
    Beams.Tails = [];
    Beams.Heads = [];
    Beams.Widths= [];
    Beams.Nodes = [];
return

function Beams = BeamsInit(Beams,NodeLocs,NodesAxLims,Collimators,NBeamlets,SkinModelf,BdyTarget)
    Beams.NodeLocs    = NodeLocs;
    Beams.NodesAxLims = NodesAxLims;
    Beams.Collimators = Collimators;
    Beams.NBeamlets   = NBeamlets;
    [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets);
    Beams.Tails0= BeamTails0;
    Beams.Tails = BeamTails;
    Beams.Heads = BeamHeads;
    Beams.Widths= BeamWidths;
    Beams.Nodes = BeamNodes;
return

function [BeamTails,BeamHeads,BeamTails0,BeamWidths,BeamNodes] = GetBeamDirections3(SkinModelf,BdyTarget,NodeLocs,Collimators,NBeamlets)
% compute candidate beam directions by firing from each Node, NBeamlets for each collimator size, onto random points on the target boundary
% also calculate the point of entry into the patient (skin) boundary
    NNodes = size(NodeLocs,1);
    NColls = length(Collimators);
    NBdyTar = size(BdyTarget,1);
    NBisections = 10;
    BeamTails0= zeros(NColls*NNodes*NBeamlets,3);
    BeamTails = zeros(NColls*NNodes*NBeamlets,3);
    BeamHeads = zeros(NColls*NNodes*NBeamlets,3);
    BeamWidths= zeros(NColls*NNodes*NBeamlets,1);
    BeamNodes = zeros(NColls*NNodes*NBeamlets,1);
    % for each node
    for i=1:NNodes
        % make that node the tail for all collimators and beamlets
        ii     = (i-1)*NColls*NBeamlets;
        iiPlus = ii +  NColls*NBeamlets;
        BeamTails0(ii+1:iiPlus,:) = repmat(NodeLocs(i,:),NColls*NBeamlets,1);
        BeamNodes(ii+1:iiPlus,:)  = repmat(i,NColls*NBeamlets,1);
        % for each collimator size
        for j=1:NColls
            % get NBeamlets random heads on target boundary
            jj     = (j-1)*NBeamlets;
            jjPlus = jj  + NBeamlets;
            BeamHeads(ii+jj+1:ii+jjPlus,:) = BdyTarget(GetRandomIndecesNoRepetition(NBeamlets,NBdyTar),:);
            BeamWidths(ii+jj+1:ii+jjPlus,:)= repmat(Collimators(j),NBeamlets,1);
            % for each beamlet
            for k=1:NBeamlets
                % compute intersection with patient surface
                BeamTails(ii+jj+k,:) = IntersectionSegmentLevelset(SkinModelf,BeamTails0(ii+jj+k,:),BeamHeads(ii+jj+k,:),NBisections);
            end
        end
    end
return

function DosageVecs = GetBeamDosageVectors8(Tails,Heads,Widths,PointCloud)%,Collimators)
% compute beam dose matrix on the given PointCloud, and quantizes it to accuracy 1e-3
    NBeams     = size(Tails,1);
    NPts       = size(PointCloud,1);
    DosageVecs = zeros(NPts,NBeams);
    disp(['Computing fake beam dosage vectors...'])
    tic
    for j=1:NBeams
        BeamRadius = Widths(j)/2;
        [DistOnRay,DistToRay]   = ComputeDistancesOnAxisAndToAxis(PointCloud,Tails(j,:)',Heads(j,:)');
        [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay);
        DosageVecs(SelectIdx,j) = ComputeFakeBeamRolloffAndDiffusion(DistOnRaySelect,DistToRaySelect,BeamRadius);
    end
    DosageVecs = 1e-3 * round(1e3*DosageVecs); % quantize to 1e-3 accuracy, anything smaller gets set to zero
    toc
return

function DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius,BeamDiffusionSigma)
% for single beam, compute fake dose vector as:
% - on-axis build-up then roll-off
% - radial diffusion that broadens a bit with distance along beam direction
    if nargin < 4, BeamDiffusionSigma = 0.2;end
    % [build up then roll off] .* [convolve(rect_aperture,gaussian) = difference of erf's]
    DoseVector =  (DistOnRay+0.02).^(1/8) .* exp(-(DistOnRay+0.02)/15).*...
                  (  erf( (DistToRay+BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) ) - ...
                     erf( (DistToRay-BeamRadius)./(BeamDiffusionSigma*(1+0.025*DistOnRay)) )   );
return

function [DistOnRay,DistToRay] = ComputeDistancesOnAxisAndToAxis(PointCloud,tail,head)
% vectorized computation of cylindrical coordinates along beam direction, for all points in VOI PointCloud,
% used for calculation of VOI dose vector
    NPts        = size(PointCloud,1);
    x0              = tail;
    u0              = ComputeUnitVector(head,tail);
    PointCloudShift = PointCloud-repmat(x0',NPts,1);
    DistOnRay       = PointCloudShift*u0;
    DistToRay       = sqrt( sum(  (PointCloudShift - DistOnRay*u0').^2  ,2)  );
return

function [DistOnRaySelect,DistToRaySelect,SelectIdx] = SelectPositiveOnAxisFiniteOffAxis(DistOnRay,DistToRay)
% keep only points in the positive direction of the beam from the tail0 and
% with distance no more than 5cm from the axis (since largest collimator is 6cm diameter = 3cm radius)
    SelectIdx       = find( (DistOnRay > 0) & (DistToRay <=5) );
    DistOnRaySelect = DistOnRay(SelectIdx);
    DistToRaySelect = DistToRay(SelectIdx);
return

function u = ComputeUnitVector(head,tail)
    u = (head-tail);
    u = u/norm(u);
return

function IndexRand = GetRandomIndecesNoRepetition(NumSamples,NumTotal)
% gets NumSamples random indexes from index set {1,...,NumTotal}, with no repetition
    IndexPerm = randperm(NumTotal);
    IndexRand = IndexPerm(1:NumSamples);
return

function PointOfInt = IntersectionSegmentLevelset(LevelSetf,tail,head,NBisections)
% finds approximate intersection of segment with levelset of f using bisection
% assumes head is inside level set and tail is outside; crashes if not.
    if LevelSetf(tail(:)) <=1, error('tail already inside sublevel set!'),end
    if LevelSetf(head(:)) > 1, error('head is not inside sublevel set!'),end
    for r=1:NBisections
        PointOfInt = 0.5*(tail+head);
        % if PointOfInt inside the 1-level set
        if LevelSetf(PointOfInt(:)) <=1
            head = PointOfInt; % move away from target
        else
            tail = PointOfInt; % move toward target
        end
    end
return

function PlotDoseMapAxialRolloffAndRadialDiffusion(Collimators)
% plot first the roll off of beam intensity along the center of a beam
% then plot a matrix of plots showing the roll off from the axis for all collimators
    BeamRadius  = 0.75/2;
    DistOnRay=0:0.1:50;
    DistToRay=0;
    DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay,DistToRay,BeamRadius);
    figure
    plot(DistOnRay,DoseVector),grid
    title('On axis beam roll off')
    xlabel('on axis distance [cm]'),ylabel('normalized intensity')

    DistOnRay=0:5:40;
    DistToRay=0:0.1:8;
    NColl = length(Collimators);
    figure,
    for j=1:NColl
        Collj = Collimators(j);
        subplot(4,3,j)
        hold on
        for i=1:length(DistOnRay)
            DoseVector = ComputeFakeBeamRolloffAndDiffusion(DistOnRay(i),DistToRay,Collj/2);
            plot(DistToRay,DoseVector), grid on
        end
        title(['Radial roll off ',num2str(Collj/2),'cm radius beam'])
        xlabel('off axis distance [cm]'),ylabel('normalized intensity')
    end
return

function PlotBeams(BeamTails,BeamHeads,LineWidth)
% plots dotted lines as the directions of the beams (not the full dose vectors)
    if nargin<3, LineWidth=4; end
    for i=1:size(BeamTails,1)
        h = plot3([BeamTails(i,1);BeamHeads(i,1)],...
              [BeamTails(i,2);BeamHeads(i,2)],...
              [BeamTails(i,3);BeamHeads(i,3)]);
        %set(h,'LineWidth',4);
        set(h,'LineWidth',LineWidth,'LineStyle',':');
    end
return

function PlotFewRandomBeamDoseMaps(Tails,Heads,Widths,nBeamsToPlot,GridPts)
% plot few random beams over the points GridPts - rarely do more than 3-5
% beams because can be really slow for a 100x100x100 grid
    NBeams      = length(Widths);
    Select      = GetRandomIndecesNoRepetition(nBeamsToPlot,NBeams);
    TailsSelect = Tails(Select,:);
    HeadsSelect = Heads(Select,:);
    WidthsSelect= Widths(Select,:)
    DosageVecs  = GetBeamDosageVectors8(TailsSelect,HeadsSelect,WidthsSelect,GridPts);
    DosageVecsThresh = 0.01;
    PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh);
return

function PlotDosageVecs2(DosageVecs,GridPts,DosageVecsThresh)
% plot the DosageVecs on the grid GridPoints, plotting only values greater
% than DosageVecsThresh
    if nargin < 3, DosageVecsThresh = 0;end
    NDosageVecs = size(DosageVecs,2);
    disp(['Plotting beam dosage vectors...'])
    for j=1:NDosageVecs
        iThresh = find(DosageVecs(:,j) > DosageVecsThresh);
        scatter3(GridPts(iThresh,1),GridPts(iThresh,2),GridPts(iThresh,3),3,DosageVecs(iThresh,j),'filled');
    end
return

function PlotNodes(NodeLocs,NodesAxLims)
    hold on
    plot3(NodeLocs(:,1),NodeLocs(:,2),NodeLocs(:,3),'r.'), grid on
    xlabel('x'),ylabel('y'),zlabel('z')
    axis(NodesAxLims)
    %view(3)
return

%====================================================
% Model Class: for generating point clouds as sublevel sets of scaled/shifted p-norms (1 < p < Inf)
%====================================================
function Model = ModelCreate()
    Model.f = [];
    Model.fGrad    = [];
    Model.AxLims = [];
return

function Model = ModelInit(Model,xyzScale,p)
    A            = diag(1./xyzScale);
    Model.f      = @(x) norm(A*x,p); % scaled p-norm (where 1 < p < Inf)
    Model.fGrad  = @(x) A*(sum((A*x).^p))^((1/p)-1)*(A*x).^(p-1); % grad of scaled p-norm
    Model.AxLims = [xyzScale(1)*[-1 1] xyzScale(2)*[-1 1] xyzScale(3)*[-1 1]]; % range for plotting
return

function PointCloud = ModelToPointCloud(Model,VoxSize,SubSampFactor)
    PointCloud = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
return

function PointCloudBdy = ModelToPointCloudBdy(Model,VoxSize,SubSampFactor)
    PointCloud    = PointCloudFromf(Model.f,Model.AxLims,VoxSize);
    PointCloudBdy = PointCloudBdyFromfAndfGrad(Model.f,Model.fGrad,PointCloud,VoxSize,SubSampFactor);
return

function Model = ModelRotateTranslate(Model,Thetaxyz,Offset)
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Model  = ModelAffineTransform(Model,RotMtx,Offset);
return

function Model = ModelAffineTransform(Model,A,b)
    Model.f      = fAffineTransform(Model.f,A,b);
    Model.fGrad  = fGradAffineTransform(Model.fGrad,A,b);
    Model.AxLims = AxLimsAffineTransform(Model.AxLims,A,b);
return

%====================================================
% Point Cloud 3D geometry stuff
%====================================================
function [GridPts,AxPts] = GetGridPts(NPtsPerAx,AxLength)
    %AxPts = linspace(0,AxLength,NPtsPerAx);
    AxPts = linspace(-AxLength,AxLength,NPtsPerAx);
    [X,Y,Z] = meshgrid(AxPts,AxPts,AxPts);
    GridPts = [X(:),Y(:),Z(:)];
return

function [Cloud,nPtsCloud] = PointCloudFromf(f,AxLim,VoxSize)
% returns point cloud of 1-sublevel set of f, sampled over a box specified
% by AxLim, with granularity VoxSize
    xMin=AxLim(1);xMax=AxLim(2);yMin=AxLim(3);yMax=AxLim(4);zMin=AxLim(5);zMax=AxLim(6);
    [X,Y,Z] = meshgrid([xMin:VoxSize:xMax],[yMin:VoxSize:yMax],[zMin:VoxSize:zMax]);
    GridPts = [X(:),Y(:),Z(:)];
    nPts    = size(GridPts,1);
    InOrOut = zeros(nPts,1);
    for i=1:nPts
        InOrOut(i) = ( f(GridPts(i,:)') <= 1);
    end
    IndexCloudPts = find(InOrOut > 0);
    Cloud         = GridPts(IndexCloudPts,:);
    nPtsCloud     = length(IndexCloudPts);
return

function [CloudBdy,nPtsCloudBdy] = PointCloudBdyFromfAndfGrad(f,fGrad,Cloud,VoxSize,SubSampFactor)
% returns boundary points of point cloud of 1-sublevel set of f,
% assumed to be differentiable with gradient fGrad, also assumed
% be computed with granularity VoxSize, and will be downsampled by factor SubSampFactor
    if nargin < 4, VoxSize = 0.1;end
    if nargin < 5, SubSampFactor = 1; end
    n       = size(Cloud,1);
    InOrOut = zeros(n,1);
    for i=1:SubSampFactor:n
        xi = Cloud(i,:)';
        InOrOut(i) = ( abs(f(xi)-1)  <=  norm(fGrad(xi))*VoxSize );% point on shell
    end
    IndexBdyPts = find( InOrOut > 0 );
    CloudBdy    = Cloud(IndexBdyPts,:);
    nPtsCloudBdy= size(CloudBdy,1);
return

function f = fAffineTransform(f,A,b)
% update f so that its 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    f = @(x) f(AInv*(x-b));
return

function fGrad = fGradAffineTransform(fGrad,A,b)
% update fGrad to correspond to the f whose 1-sublevel set S is transformed to A*S+b
    AInv = inv(A);
    fGrad = @(x) AInv'*fGrad(AInv*(x-b));
return

function AxLims = AxLimsAffineTransform(AxLims,A,b)
    Vertices = AxLimsToVertices(AxLims);
    Vertices = PointCloudAffineTransform(Vertices,A,b);
    AxLims   = AxLimsFromVertices(Vertices);
return

function AxLims = AxLimsFromVertices(Vertices)
% computes box around set of Vertices sorted row-wise [x1';x2';...;xN'];
    Dimensions = size(Vertices,2);
    AxLims = zeros(1,2*Dimensions);
    for i=1:Dimensions
        AxLims(2*i)  = max(Vertices(:,i));
        AxLims(2*i-1)= min(Vertices(:,i));
    end
return

function Vertices = AxLimsToVertices(AxLims)
    Vertices = GenerateVertices([],0,AxLims);
return

function V = GenerateVertices(V,i,AxLims)
% computes Vertices sorted row-wise [x1';x2';...;xN'] from Matlab axis limits AxLims,
% via recursively doubling up the coordinates matrix, backwards from N to 1,
% adding new coordinate column each step.
    n = length(AxLims)/2;
    i = i+1;
    if i <= n
        V = [AxLims(2*(n-i+1))  *ones(2^(i-1),1),V;
             AxLims(2*(n-i+1)-1)*ones(2^(i-1),1),V];
        V = GenerateVertices(V,i,AxLims);
    else
        V = V;
    end
return

function Cloud = PointCloudAffineTransform(Cloud,A,b)
% Apply A*x+b to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    nPtsCloud = size(Cloud,1);
    Cloud     = Cloud*A' + repmat(b(:)',nPtsCloud,1);
return

function Cloud = PointCloudRotateTranslate(Cloud,Thetaxyz,Offset)
% Apply rotation (Thetaxyz) and Offset to each point in Cloud
% Assumes points stored in Cloud row-wise [x1';x2';...;xN'];
    RotMtx = AnglesToRotMtx(Thetaxyz);
    Cloud = PointCloudAffineTransform(Cloud,RotMtx,Offset(:));
return

function [RotMtx,Rx,Ry,Rz] = AnglesToRotMtx(Thetaxyz)
% Rotation about x, y, and z axes - in that order!
    cx = cos(Thetaxyz(1)); sx = sin(Thetaxyz(1));
    cy = cos(Thetaxyz(2)); sy = sin(Thetaxyz(2));
    cz = cos(Thetaxyz(3)); sz = sin(Thetaxyz(3));
    Rx  = [1    0   0;
           0    cx -sx;
           0    sx  cx];
    Ry  = [cy   0   sy;
           0    1   0;
          -sy   0   cy];
    Rz  = [cz  -sz  0;
           sz   cz  0;
           0    0   1];
    RotMtx = Rz*Ry*Rx;
return

function PlotCloud(CloudGridPts,Symbol,AxPts)
    plot3(CloudGridPts(:,1),CloudGridPts(:,2),CloudGridPts(:,3),Symbol);
    AxMin = min(AxPts); AxMax = max(AxPts);
    axis([AxMin,AxMax,AxMin,AxMax,AxMin,AxMax])
    grid on
return

function Points = GetPointsOnUnitSphere1000(N)
% get evenly distributed 1000 points on UNIT sphere
% taken off the web
    if nargin <1
        N=14;
    end
    Beta = 0.5*pi/N;
    Points = [];
    % line segment length
    A = 2*sin(Beta/2);
    % endcap
    Points = [Points;[0 0 1]];
    Points = [Points;[0 0 -1]];
    % rings
    for i=1:N
      R = sin(i*Beta);
      Z = cos(i*Beta);
      M = round(R*2*pi/A);
      for j=0:M-1
          Alpha = j/M * 2 * pi;
          X = cos(Alpha)*R;
          Y = sin(Alpha)*R;
          Points = [Points;[X Y Z]];
          if i~=N
              Points = [Points;[X Y -Z]];
          end
      end
    end
return

%====================================================
% Optimization functions: CPLEX and ADMM
%====================================================
function xBmWtsStar = OptimizeBeamsCPLEX(AAll,dMin,dMax);
% solve using CPLEX
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    INVoxAll = speye(NVoxAll);
    ZNVoxAll = sparse(NVoxAll,NVoxAll);
    ZNVoxAllNBeams = sparse(NVoxAll,NBeams);
    AEq = [AAll, -INVoxAll, ZNVoxAll];
    AEq = sparse(AEq);
    bEq = [zeros(NVoxAll,1)];
    AIneq = [ZNVoxAllNBeams, -INVoxAll, -INVoxAll;
             ZNVoxAllNBeams,  INVoxAll, -INVoxAll];
    AIneq = sparse(AIneq);
    bIneq = [-dMin;dMax];
    lb = zeros(NBeams+NVoxAll+NVoxAll,1);
    ub = [300*ones(NBeams,1);1e6*ones(2*NVoxAll,1)];
    cLP = [zeros(NBeams,1);zeros(NVoxAll,1);ones(NVoxAll,1)];
    disp('Solving optimization with CPLEX ...')
    tic
    options = cplexoptimset('Simplex','on');
    [xStar,Obj,exitflag,output,lambda] = ...
    cplexlp(cLP',AIneq,bIneq,AEq,bEq,lb,ub,[],options); %, x0);
    exitflag = exitflag
    xBmWtsStar = xStar(1:NBeams);
    fstar = Obj
    toc
return

function xBmWtsStar = OptimizeBeamsADMM(AAll,dMin,dMax);
% solve using ADMM following Stanford EE364b final exam problem (Boyd&Chu)
%        minimize   sum pos(d - dmax)  + sum pos(dmin - d)
%        such that  A x =  d
%                     x >= 0
% where pos(x) is the positive part of x, taken componentwise
%         pos(x) = max(x,0)
%
    tic
    disp(' ')
    NVoxAll  = size(AAll,1)
    NBeams   = size(AAll,2)
    disp('doing ADMM as in EE364b 2011 Final (Stephen Boyd, Eric Chu)')
    disp('computing pseudo-inverse of [A;I]...')
    tic
    AAllI    = [AAll;eye(NBeams)];
    %AAllIPinv = pinv(AAllI);
    Delta = pinv(speye(NBeams)+AAll'*AAll);
    save('Delta','Delta');
    %load Delta
    toc
    ud  = zeros(NVoxAll,1);
    ux  = zeros(NBeams,1);
    dose = zeros(NVoxAll,1);
    xBm  = zeros(NBeams,1);
    z    = zeros(NBeams,1);
    zOld = z;
    disp('doing ADMM iterations')
    MaxIters = 300
    rho     = 1.25;%0.5
    mu      = 10;%1.5
    tauIncr = 2;%1.1
    tauDecr = 1/tauIncr;
    rNorm = zeros(MaxIters,1);
    sNorm = zeros(MaxIters,1);
    rhos  = zeros(MaxIters,1);
    for i=1:MaxIters

        % do ADMM update
        AAllzOld = AAll*zOld;
        dose = DoubleHingeProx(dMin,dMax,rho,AAllzOld-(ud/rho));
        xBm  = min(max(z-(ux/rho),0),300);
        z    = Delta*(  AAll'*(dose+(ud/rho))  +  (xBm+(ux/rho))  );
        AAllz= AAll*z;
        ux   = ux + rho*(xBm-z);
        ud   = ud + rho*(dose-AAllz);

        % compute primal and dual residuals for stopping criterion and rho updating
        r        = [dose;xBm] - [AAllz;z];
        s        = rho*([AAllz-AAllzOld;z-zOld]);
        rNorm(i) = norm(r,2);
        sNorm(i) = norm(s,2);

        % update z
        zOld = z;

        % "adaptive" rho update (doesn't always work ==> safer to switch off)
        rhos(i) = rho;
%         if     (rNorm(i) > mu*sNorm(i))
%             rho = tauIncr*rho
%         elseif (rNorm(i) < (1/mu)*sNorm(i))
%             rho = tauDecr*rho
%         end

    end
    disp('done!')
    xBmWtsStar = xBm;
    toc

    figure
    subplot(3,1,1),plot(1:MaxIters,log10(rNorm)),grid,ylabel('log10(rNorm)')
    subplot(3,1,2),plot(1:MaxIters,log10(sNorm)),grid,ylabel('log10(sNorm)')
    subplot(3,1,3),plot(1:MaxIters,log10(rhos)),grid,ylabel('log10(rho)')
return

function xProx = DoubleHingeProx(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xProx = 1/rho + x - max(x-(aa-1/rho),0) + max(x-aa,0) - max(x-bb,0) + max(x-(bb+1/rho),0);
return

function xProx = DoubleHingeProx2(aa,bb,rho,x)
    if min(bb-aa)<0, error('xMin > xMax!'),end
    xTemp = SoftThresh(1/2/rho,x-(aa-1/2/rho)) + aa;
    xProx = SoftThresh(1/2/rho,xTemp-(bb+1/2/rho))+bb;
return

function xST = SoftThresh(k,x)
    if min(k) <= 0, error('k <=0'),end
    xST = max(x-k,0) - max(-x-k,0);
return

function y = DoubleHinge(xMin,xMax,x)
    if min(xMax-xMin)<0, error('xMin > xMax!'),end
    y = max(xMin-x,0) + max(x-xMax,0);
return


text:
function ng = triangle_grid_count ( n )

%*****************************************************************************80
%
%% TRIANGLE_GRID_COUNT counts the grid points inside a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Output, integer NG, the number of grid points inside the triangle.
%
  ng = ( ( n + 1 ) * ( n + 2 ) ) / 2;

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function [ g ] = gsp_design_regular(G, param)
%GSP_DESIGN_REGULAR Create a regular filterbank
%   Usage: g = gsp_design_regular( G );
%          g = gsp_design_regular( G, param );
%
%   Inputs parameters:
%       G       : Graph structure or lmax
%       param   : Structure of optional parameters
%
%   Outputs parameters:
%       g       : filterbank
%
%   This function creates a parseval filterbank of $2$ filters. The low-pass
%   filter is defined by a function $f_l(x)$ between $0$ and $2$. For
%   $d = 0$.
%
%   ..    f_l(x) = sin(pi/4*x)
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} x \right)
%
%   For $d = 1$
%
%   ..    f_l(x) = sin( pi/4 * (1+sin(pi/2*(x-1))) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2}(x-1)\right) \right) \right)
%
%   For $d = 2$
%
%   ..    f_l(x) = sin( pi/4 * ( 1 + sin( pi/2 * sin(pi/2*(x-1) ) ) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2} \sin\left(\frac{\pi}{2}(x-1)\right)\right) \right) \right)
%
%   And so on for the other degrees $d$.
%
%   The high pass filter is adapted to obtain a tight frame.
%
%   This function will compute the maximum eigenvalue of the laplacian. To
%   be more efficient, you can precompute it using::
%
%       G = gsp_estimate_lmax(G);
%
%   Example:::
%
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_regular(G);
%         gsp_plot_filter(G,g);
%         [A,B] = gsp_filterbank_bounds(G,g)
%
%   *param* is an optional structure containing the following fields
%
%   * *param.verbose*: verbosity level. 0 no log - 1 display warnings.
%     (default 1)
%   * *param.d*: Degree. See equation for mor informations. (default 3)
%

% Author: Nathanael Perraudin, David Shuman
% Date  : 21 June 2014
% Testing: test_filter

if nargin < 2
    param = struct;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'d'), param.d = 3; end

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_REGULAR has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

d = param.d;

g = cell(2,1);
g{1} = @(x) regular(x*(2/lmax),d);
g{2} = @(x) real(sqrt(1-(regular(x*(2/lmax),d)).^2));

end

function y = regular(val,d)

if d==0
    y = sin(pi/4*val);
else
    output = sin(pi*(val-1)/2);
    for k=2:d
        output = sin(pi*output/2);
    end
    y = sin(pi/4*(1+output));
end

end

repo: gspbox


text:
% Sliding Window Gauss Newton Optimization
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc
clear
close all
load('../KITTI Trials/SWF_2011_09_26_drive_0036_sync_KLT.mat');
k1 = kStart;
k2 = k1+kappa;
K = k2 - k1;  %There are K + 1 total states, since x0 is the k1th state
% v_var = 0.1*ones(3,1);
% w_var = 0.1*ones(3,1);
% y_var = 0.5^2*ones(2,1);                 % pixel coord var
% JcostThresh = 0.1e-2;

%% Setup
if useMonoCamera
    pixMeasDim = 2;
else
    pixMeasDim = 4;
end

initialStateStruct = {};

% Extract noise values
Q = diag([v_var; w_var]);
if useMonoCamera
    R = diag(y_var);
    %R = diag(y_var(1:2));
else
    R = 1*eye(4);
    %R = diag(y_var);
end

%% First create the initial guess using dead reackoning

%Use ground truth for the first state
firstState.C_vi = Cfrompsi(theta_vk_i(:,k1));
if isnan(firstState.C_vi(1,1))
    firstState.C_vi = eye(3);
end
firstState.r_vi_i = r_i_vk_i(:,k1);
firstState.k = k1;
initialStateStruct{1} = firstState;

%There are K + 1 states (there is a '0' state)
for kIdx = 1:K
    k = kIdx + k1;
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);
    %Propagate the state forward
    initialStateStruct{kIdx+1} = propagateState(initialStateStruct{kIdx}, imuMeasurement, deltaT);
end

%% IMU Only
imuOnlyStateStruct{1} = firstState;
%There are K + 1 states (there is a '0' state)
for k = kStart+1:kEnd+1
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);
    %Propagate the state forward
    imuOnlyStateStruct{k} = propagateState(imuOnlyStateStruct{k-1}, imuMeasurement, deltaT);
end

%%

%Slide the window along
stateVecHistStruct = {};
stateVecHistStruct{1} = firstState;

%stateSigmaHistMat = [];
figure
plot(-firstState.r_vi_i(2),firstState.r_vi_i(1), '*b')
hold on;
grid on;
for k1 = kStart:kEnd
k2 = k1+kappa;

% %How many exteroceptive measurements do we have?
% %NOTE: k1 is the 0th state
% totalLandmarkObs = 0;
% observedBinaryFlags = zeros(numLandmarks, 1);
% lmObsVec = zeros(1, K);
%
% for k = (k1+1):k2
%     validObs = squeeze(y_k_j(1, k, :) > -1);
%     lmObsVec(k-k1) = sum(validObs);
%     observedBinaryFlags(validObs') = ones(1, sum(validObs==1));
%     totalLandmarkObs = totalLandmarkObs + sum(y_k_j(1, k, :) > -1);
% end
% totalUniqueObservedLandmarks = sum(observedBinaryFlags);
% observedLandmarkIds = find(observedBinaryFlags);

%To initialize G-N, we propagate all the states for the first window
%and then only propagate the most recent state, re-using the rest
if k1 == kStart
        currentStateStruct = initialStateStruct;
        rho_i_pj_i_est = NaN(3, numLandmarks);
else
        currentStateStruct = currentStateStruct(2:end);

        %Extract the measurement
        imuMeasurement.omega = w_vk_vk_i(:, k2-1);
        imuMeasurement.v = v_vk_vk_i(:, k2-1);
        deltaT = t(k2) - t(k2-1);

        %Propagate the state forward
        currentStateStruct{end+1} = propagateState(currentStateStruct{end}, imuMeasurement, deltaT);
end

% Initialize the landmark positions (start fresh each window)
rho_i_pj_i_est = NaN(3, numLandmarks);
observedLandmarkStructs = {};
for l_i = 1:numLandmarks
       observedLandmarkStructs{l_i}.camStates = {};
       observedLandmarkStructs{l_i}.observations = [];
end
for kIdx = 1:K
        k = kIdx + k1;
        validLmObsId = find(y_k_j(1, k, :) > -1);
        kState = currentStateStruct{kIdx+1};
        T_vi = [kState.C_vi -kState.C_vi*kState.r_vi_i; 0 0 0 1];
        T_ci = T_cv*T_vi;
        T_ic = inv(T_ci);

        % camStates{k}.q_CG        4x1 Global to camera rotation quaternion
        % camStates{k}.p_C_G       3x1 Camera Position in the Global frame
        % camStates{k}.trackedFeatureIds  1xM List of feature ids that are currently being tracked from that camera state

        for lmId = validLmObsId'

            yMeas = y_k_j(:, k, lmId);
            camState = {};
            camState.C_CG = T_ci(1:3,1:3);
            camState.p_C_G = T_ic(1:3,4);
            observedLandmarkStructs{lmId}.camStates{end+1} = camState;
            observedLandmarkStructs{lmId}.observations(:, end+1) = [(yMeas(1) - calibParams.c_u)/calibParams.f_u; (yMeas(2) - calibParams.c_v)/calibParams.f_v];

            %Find the ground truth position of the observed landmark
            %rho_pi_i_check = rho_i_pj_i(:, lmId);

%             if (isnan(rho_i_pj_i_est(1, lmId)))
%                 %Use triangulation to find the position of the landmark
%                  rho_pc_c = triangulate(yMeas, calibParams);
%
%                  rho_pi_i = kState.C_vi'*(vehicleCamTransform.C_cv'*rho_pc_c + vehicleCamTransform.rho_cv_v) +  kState.r_vi_i;
%                  rho_i_pj_i_est(:, lmId) = rho_pi_i;
%
%                  %Use ground truth for now
%                  %rho_i_pj_i_est(:, lmId) = rho_i_pj_i(:, lmId);
%             end
        end
end

%Triangulate all landmarks and keep track of which ones we are
%triangulating
totalLandmarkObs = 0;
observedLandmarkIds = [];
totalUniqueObservedLandmarks = 0;
for lmId = 1:length(observedLandmarkStructs)
    if length(observedLandmarkStructs{lmId}.camStates) > 1
        camStates = observedLandmarkStructs{lmId}.camStates;
        observations = observedLandmarkStructs{lmId}.observations;
        [rho_pi_i, Jcost, RCOND] = calcGNPosEst(camStates, observations, noiseParams);
        if Jcost < JcostThresh*length(camStates)^2
            rho_i_pj_i_est(:, lmId) = rho_pi_i;
            totalLandmarkObs = totalLandmarkObs + length(observedLandmarkStructs{lmId}.camStates);
            totalUniqueObservedLandmarks = totalUniqueObservedLandmarks + 1;
            observedLandmarkIds(end+1) = lmId;
        end
    end
end

%Define the optimal state
optimalStateStruct = currentStateStruct;
Jbest = Inf;
dx = Inf;

for optIdx = 1:maxOptIter+1

%Error Vector
errorVector = NaN(6*K+pixMeasDim*totalLandmarkObs, 1);
%This helper index will keep track of where we need to insert our next
%errors
errorVectorHelperIdx = 1;

%H and T
H = sparse(6*K+pixMeasDim*totalLandmarkObs, 6*(K+1) + 3*totalUniqueObservedLandmarks);
T = sparse(6*K+pixMeasDim*totalLandmarkObs, 6*K+pixMeasDim*totalLandmarkObs);

%Helper indices that keep track of the row number of the last block entry
%into H and T
HHelperIdx = 1;
THelperIdx = 1;

for kIdx = 1:K
    k = kIdx + k1;
    imuMeasurement.omega = w_vk_vk_i(:, k-1);
    imuMeasurement.v = v_vk_vk_i(:, k-1);
    deltaT = t(k) - t(k-1);

    %==== Build the interoceptive error and Jacobians=====%
    %Note that there are K+1 states (the 0th state is the 1st element)
    kState = currentStateStruct{kIdx+1};
    kMinus1State = currentStateStruct{kIdx};

    intErrorVec = imuError(kState, kMinus1State, imuMeasurement, deltaT);
    H_x_k =  H_xfn(kMinus1State, imuMeasurement, deltaT );
    H_w_k = H_wfn(kMinus1State);

    %==== Build the exteroceptive error and Jacobians=====%
    validLmObsId = intersect(find(y_k_j(1, k, :) > -1), observedLandmarkIds);

    if kIdx == 1 && optIdx == 1
        fprintf('Tracking %d features. \n', length(validLmObsId));
    end

    if ~isempty(validLmObsId)

        extErrorVec = NaN(pixMeasDim*length(validLmObsId), 1);
        G_x_k = NaN(pixMeasDim*length(validLmObsId),6);
        %Jacobians wrt feature position
        G_x_f_k = NaN(pixMeasDim*length(validLmObsId), 3);

        idx = 1;
        for lmId = validLmObsId'

            yMeas = y_k_j(:, k, lmId);

            rho_pi_i = rho_i_pj_i_est(:,lmId);

            stereoError = stereoCamError(yMeas, kState, vehicleCamTransform, rho_pi_i, calibParams);

            [G_x_k_state, G_x_k_feat] = G_xfn(kState, vehicleCamTransform, rho_pi_i, calibParams, useMonoCamera);

            %Use stereo or monocular errors
            if useMonoCamera
                extErrorVec(idx:idx+1, 1) = stereoError(1:2);

                G_x_k(idx:idx+1, :) = G_x_k_state;
                G_x_f_k(idx:idx+1, :) = G_x_k_feat;
                idx = idx + 2;
            else
                extErrorVec(idx:idx+3, 1) = stereoError;

                G_x_k(idx:idx+3, :) = G_x_k_state;
                G_x_f_k(idx:idx+3, :) = G_x_k_feat;
                idx = idx + 4;
            end
        end
    else
        extErrorVec = [];
    end

    %Update matrices
    %==== Error vector =====
    combinedErrorVec = [intErrorVec; extErrorVec];
    errorVector(errorVectorHelperIdx:(errorVectorHelperIdx + length(combinedErrorVec) - 1) ,1) = combinedErrorVec;
    errorVectorHelperIdx = errorVectorHelperIdx + length(combinedErrorVec);

    %==== H matrix =====
    Hblock = zeros(6+pixMeasDim*length(validLmObsId), 12);
    Hblock(1:6,1:6) = -H_x_k;
    Hblock(1:6,7:12) = eye(6);

    if ~isempty(validLmObsId)
        Hblock(7:(7+pixMeasDim*length(validLmObsId) - 1), 7:12) = -G_x_k;
    end
    Hblockrows = size(Hblock, 1);

    H(HHelperIdx:(HHelperIdx + Hblockrows - 1), 1+6*(kIdx-1):12+6*(kIdx-1) ) = Hblock;

    %Add the feature Jacobians
    lmNum = 1;
    for lmId = validLmObsId'
        rowIdx = pixMeasDim*(lmNum-1)+1;

        colLmId = find(observedLandmarkIds == lmId);
        colIdx = 6*(K+1)+3*colLmId-2;

        H(HHelperIdx+5+rowIdx:HHelperIdx+rowIdx+5+(pixMeasDim-1), colIdx:colIdx+2) = -G_x_f_k(rowIdx:rowIdx+pixMeasDim-1, :);
        lmNum = lmNum + 1;
    end

    HHelperIdx = HHelperIdx + Hblockrows;

    %==== T matrix =====
    T_k = zeros(6+pixMeasDim*length(validLmObsId), 6+pixMeasDim*length(validLmObsId));
    T_k(1:6, 1:6) = H_w_k*Q*deltaT^2*H_w_k';
    %Here, G_n_k is identity, so we can just repeat the variances along the
    %diagonal
    obsVar = diag(R);
    T_k(7:end, 7:end) = diag(repmat(obsVar, [length(validLmObsId),1]));
    Tksize = size(T_k, 1);
    T(THelperIdx:(THelperIdx + Tksize - 1), THelperIdx:(THelperIdx + Tksize - 1)) = T_k;
    THelperIdx = THelperIdx + Tksize;
end

    H = H(:, 7:end);

    %Calculate scalar objective
    Jnew = 0.5*errorVector'*(T\errorVector);

    if Jnew < Jbest
        optimalStateStruct = currentStateStruct;
        Jbest = Jnew;
    end

    %Check for convergence
    if norm(dx) < 1e-3
        disp('Converged!')
        break;
    end

    % Solve for the optimal step size!
    if optIdx <= maxOptIter
        dx = (H'*(T\H) + LMLambda*diag(diag(H'*(T\H))))\(-H'*(T\errorVector));
        [currentStateStruct, rho_i_pj_i_est] = updateStateStruct(currentStateStruct, observedLandmarkIds, rho_i_pj_i_est,  lineLambda*dx);
    end

end %End optimization iterations

% error = 0;
% for i=1:numLandmarks
%     if ~isnan(rho_i_pj_i_est(1,i))
%         error = error + norm(rho_i_pj_i_est(:,i) - rho_i_pj_i(:,i));
%     end
% end
% error = error/numLandmarks
currentStateStruct = optimalStateStruct;

if optIdx == maxOptIter
    fprintf('Warning: Failed to converge! \n');
end

fprintf('%d done. J = %.5f. %d iterations. \n', k1, Jbest, optIdx)

%Extract variance of states
stateCov = inv(H'*(T\H) + LMLambda*eye(size(H,2)));
stateVar = diag(stateCov);

%Keep track of the first state in the window
% if ~all(stateVar > 0)
%     warning('Variances not positive');
% end
stateVecHistStruct{end+1} = currentStateStruct{2};
plot(-currentStateStruct{2}.r_vi_i(2),currentStateStruct{2}.r_vi_i(1), '*b')
plot(-imuOnlyStateStruct{k1}.r_vi_i(2),imuOnlyStateStruct{k1}.r_vi_i(1), '*r')
plot(-r_i_vk_i(2,k1),r_i_vk_i(1,k1), '*k');

drawnow;
stateSigmaHistMat(:,k1 - kStart + 1) = sqrt(abs(stateVar(1:6)));

end %End Sliding window

toc

sigma_x = (stateSigmaHistMat(1,:));
sigma_y = (stateSigmaHistMat(2,:));
sigma_z = (stateSigmaHistMat(3,:));
sigma_th1 = (stateSigmaHistMat(4,:));
sigma_th2 = (stateSigmaHistMat(5,:));
sigma_th3 = (stateSigmaHistMat(6,:));

%% Plot error and variances
%addpath('/Users/valentinp/Research/MATLAB/export_fig'); %Use Oliver Woodford's awesome export_fig package to get trimmed PDFs
addpath('../msckf/utils')
rotErrVec = zeros(3, length(stateVecHistStruct));
transErrVec = zeros(3, length(stateVecHistStruct));
transErrVecImu = zeros(3, length(stateVecHistStruct));
rotErrVecImu = zeros(3, length(stateVecHistStruct));

translation = zeros(3, length(stateVecHistStruct));
translation_imuonly = zeros(3, length(stateVecHistStruct));

for stIdx = 1:length(stateVecHistStruct)
    translation(:, stIdx) = stateVecHistStruct{stIdx}.r_vi_i;
    translation_imuonly(:,stIdx) = imuOnlyStateStruct{stIdx}.r_vi_i;
    transErrVec(:,stIdx) = stateVecHistStruct{stIdx}.r_vi_i - r_i_vk_i(:,stIdx);
    transErrVecImu(:,stIdx) = imuOnlyStateStruct{stIdx}.r_vi_i - r_i_vk_i(:,stIdx);
    eRotMat = eye(3) - stateVecHistStruct{stIdx}.C_vi*axisAngleToRotMat(theta_vk_i(:,stIdx))';
    rotErrVec(:, stIdx) = [eRotMat(3,2); eRotMat(1,3); eRotMat(2,1)];

    eRotMat = eye(3) - imuOnlyStateStruct{stIdx}.C_vi*axisAngleToRotMat(theta_vk_i(:,stIdx))';
    rotErrVecImu(:, stIdx) = [eRotMat(3,2); eRotMat(1,3); eRotMat(2,1)];
end

figure
plot3(-translation(2,:),translation(1,:),translation(3,:), '-b');
hold on
plot3(-translation_imuonly(2,:),translation_imuonly(1,:),translation_imuonly(3,:), '-r');
plot3(-r_i_vk_i(2,1:length(stateVecHistStruct)),r_i_vk_i(1,1:length(stateVecHistStruct)),r_i_vk_i(3,1:length(stateVecHistStruct)), '-k');
legend('Opt', 'IMU', 'Ground Truth')
armse_imu = mean(sqrt(sum(transErrVecImu.^2,1)/3))
armse_imu_rot = mean(sqrt(sum(rotErrVecImu(:,2:end).^2,1)/3))

armse = mean(sqrt(sum(transErrVec.^2,1)/3))
armse_rot = mean(sqrt(sum(rotErrVec(:,2:end).^2,1)/3))

norm(transErrVecImu(:,end))
norm(transErrVec(:,end))

save(sprintf('../KITTI Trials/SWF_RERUN_%s', fileName));
%%
% numFeat = NaN(1, 100);
% for f_i = 1:100
%    numFeat(f_i) = sum(y_k_j(1,f_i,:) > -1);
% end
% plot(numFeat)
% Save estimates
% swf_trans_err = transErrVec;
% swf_rot_err = rotErrVec;
% save(sprintf('swf_%d_%d_%d_%s.mat',kStart,kEnd, kappa, fileName), 'swf_trans_err', 'swf_rot_err', 'stateSigmaHistMat');

% transLim = 0.5;
% rotLim = 0.5;
% recycleStates = 'Yes';
%
% figure
% subplot(3,1,1)
% plot(t(kStart:kEnd), transErrVec(1,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_x, '--r')
% plot(t(kStart:kEnd), -3*sigma_x, '--r')
% ylim([-transLim transLim])
% title(sprintf('Translational Error (\\kappa = %d, Recycle States? %s)', kappa, recycleStates))
% ylabel('\delta r_x')
%
%
% subplot(3,1,2)
% plot(t(kStart:kEnd), transErrVec(2,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_y, '--r')
% plot(t(kStart:kEnd), -3*sigma_y, '--r')
% ylim([-transLim transLim])
% ylabel('\delta r_y')
%
% subplot(3,1,3)
% plot(t(kStart:kEnd), transErrVec(3,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_z, '--r')
% plot(t(kStart:kEnd), -3*sigma_z, '--r')
% ylim([-transLim transLim])
% ylabel('\delta r_z')
% xlabel('t_k')
% %set(gca,'FontSize',12)
% %set(findall(gcf,'type','text'),'FontSize',12)
%
% figure
% subplot(3,1,1)
% plot(t(kStart:kEnd), rotErrVec(1,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th1, '--r')
% plot(t(kStart:kEnd), -3*sigma_th1, '--r')
% ylim([-rotLim rotLim])
% title(sprintf('Rotational Error (\\kappa = %d, Recycle States? %s)', kappa, recycleStates))
% ylabel('\delta\theta_x')
%
%
% subplot(3,1,2)
% plot(t(kStart:kEnd), rotErrVec(2,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th2, '--r')
% plot(t(kStart:kEnd), -3*sigma_th2, '--r')
% ylim([-rotLim rotLim])
% ylabel('\delta\theta_y')
%
% subplot(3,1,3)
% plot(t(kStart:kEnd), rotErrVec(3,:), 'LineWidth', 1.2)
% hold on
% plot(t(kStart:kEnd), 3*sigma_th3, '--r')
% plot(t(kStart:kEnd), -3*sigma_th3, '--r')
% ylim([-rotLim rotLim])
% ylabel('\delta\theta_z')
% xlabel('t_k')

author: utiasSTARS

repo: msckf-swf-comparison


text:
function [] = idtf2u3d(idtffile, u3dfile)
%IDTF2U3D   Convert IDTF to U3D file.
%
% usage
%   IDTF2U3D
%   IDTF2U3D(IDTF_filename)
%   IDTF2U3D(IDTF_filename, U3D_filename)
%
% optional input
%   idtffile = filename string for IDTF file (default = 'matfig.idtf')
%   u3dfile = filename string for U3D file (default = 'matfig.u3d')
%
% output
%   Conerts the IDTF file into a U3D file which is saved to the disk.
%
% note
%   If only IDTF file name is provided without extension, then the '.idtf'
%   file extension is appended and the U3D file uses the same name with the
%   '.u3d' file extension appended.
%
%   If only the IDTF file name is provided with the extension '.idtf', then
%   the U3D file uses the same name with the '.idtf' extension replaced by
%   the extension '.u3d'.
%
%   If both file names are provided, any one without the appropriate
%   extension gets appended with that extension ('.idtf' and '.u3d',
%   respectively).
%
% reference
%   IDTF (Intermediate Data Text File) Format Description, Version 100,
%   Intel Corporation, 2005, available at:
%       http://u3d.svn.sourceforge.net/viewvc/u3d/releases/Gold12Update/Docs/IntermediateFormat/IDTF%20Format%20Description.pdf
%
% See also FIG2U3D, FIG2PDF3D, FIG2IDTF.
%
% File:      idtf2u3d.m
% Author:    Ioannis Filippidis, jfilippidis@gmail.com
% Date:      2011.02.17 - 2012.06.21
% Language:  MATLAB R2012a
% Purpose:   convert IDTF file to U3D file format
%
% Based on MESH_TO_LATEX.m by Alexandre Gramfort, which is part of
% "Matlab mesh to PDF with 3D interactive object"
% which is Copyright (c) by Alexandre Gramfort under the BSD License
% The link on the MATLAB Central File Exchange is:
% http://www.mathworks.com/matlabcentral/fileexchange/25383-matlab-mesh-to-pdf-with-3d-interactive-object

% depends
%   clear_file_extension, check_file_extension
%   IDTFConverter executables in ./bin directory

%% input
if nargin < 1
    idtffile = 'matfig.idtf';
    u3dfile = 'matfig.u3d';
end

if nargin < 2
    u3dfile = clear_file_extension(idtffile, '.idtf');
end

%% filenames & extensions

% fname extensions ok ?
idtffile = full_fname_with_extension(idtffile, 'idtf');
u3dfile = full_fname_with_extension(u3dfile, 'u3d');

%% prepare command
mfiledir = fileparts(mfilename('fullpath') );
curpath = pwd;

% Intel Mac
if ismac
    idtf_executable_path = [mfiledir, '/bin/maci/'];
    cd(idtf_executable_path)

    IDTFcmd = './IDTFConverter';

    %temp = [getenv('DYLD_LIBRARY_PATH'), ':"', mfiledir, '/bin/maci/"'];
    %setenv('DYLD_LIBRARY_PATH', temp)
    %IDTFcmd = ['"', mfiledir, '/bin/maci/', IDTFcmd, '"'];
end

% Linux
if isunix && ~ismac
    idtf_executable_path = [mfiledir, '/bin/glx/'];
    cd(idtf_executable_path)

    IDTFcmd = './IDTFConverter.sh';

    %temp = [getenv('LD_LIBRARY_PATH'), ':"', mfiledir, '/bin/glx/"'];
    %setenv('LD_LIBRARY_PATH', temp)
    %IDTFcmd = ['"', mfiledir, '/bin/glx/', IDTFcmd, '.sh"'];
end

% windows
if ispc
    win_mfiledir = strrep(mfiledir, '\', '\\');
    IDTFcmd = ['"', win_mfiledir, '\\bin\\w32\\IDTFConverter.exe"'];
end

%% idtf -> u3d conversion
s = [IDTFcmd, ' -input "%s" -output "%s"'];
idtf2u3dcmd = sprintf(s, idtffile, u3dfile);
disp(idtf2u3dcmd)
[status, result] = system(idtf2u3dcmd);
cd(curpath) % go back

disp(result)
if status ~= 0
    error('idtf2u3d:conversion',...
          'IDTFConverter executable returned with error.')
end

function [fname] = full_fname_with_extension(fname, extension)
fname = check_file_extension(fname, extension);
fname  = fullfile(cd, fname);

author: Sable

repo: mcbench-benchmarks


text:
function output = bnb_solvelower(lowersolver,relaxed_p,upper,lower,x_min,allSolutions)

if all(relaxed_p.lb==relaxed_p.ub)
    x = relaxed_p.lb;
    if checkfeasiblefast(relaxed_p,relaxed_p.lb,relaxed_p.options.bnb.feastol)
        output.problem = 0;
    else
        output.problem = 24;
    end
    output.Primal = x;
    return
end

if ~(relaxed_p.all_integers && all(relaxed_p.c == fix(relaxed_p.c)) && nnz(relaxed_p.Q)==0)
     % Objective contains floating-point numbers, so add some margin
     % if we add an upper bound cut
     upper = upper + 1e-4;
end

p = relaxed_p;
p.solver.tag = p.solver.lower.tag;

if ~isinf(upper) && nnz(p.Q)==0 && isequal(p.K.m,0) && ~any(p.variabletype)
    if p.all_integers && all(p.c == fix(p.c))
        % All integer objective coefficients and all integer
        % variables, we must find a solution which is at least
        % 1 better than current upper bound
        if upper == lower + 1
            p = addEquality(p,[upper-1-p.f -p.c']);
        else
            p = addInequality(p,[upper-1-p.f -p.c']);
        end
    end
end

if ~isinf(upper) && p.all_integers && all(p.ub <= 0) && all(p.lb >= -1)
    % Exclusion cuts for negated binaries based on some optimal solutions
    % kept for historical reason on malformed model
    for i = 1:min(size(allSolutions,2),10)
        [b,a] = exclusionCut(allSolutions(:,end-i+1),-1);
        p = addInequality(p,[b a]);
    end
elseif ~isinf(upper) && p.all_integers && all(p.ub <= 1) && all(p.lb >= 0)
    % Normal exclusion on binary
     for i = 1:min(size(allSolutions,2),10)
         [b,a] = exclusionCut(allSolutions(:,end-i+1),1);
         p = addInequality(p,[b a]);
     end
end

% for i = 1:length(p.cardinalityvariables)
%     used = p.cardinalityvariables{i};
%     L = p.lb(used);
%     if sum(L) == p.cardinalitysize{i}
%         p.ub(used(p.lb(used) < p.ub(used) )) = 0;
%     end
% end

removethese = p.lb==p.ub;
[~,map] = ismember(find(~p.lb==p.ub),1:length(p.c));
if nnz(removethese)>0 && all(p.variabletype == 0) && isempty(p.evalMap) && p.options.allowsmashing

    % Fixed variables, so let us try to presolve as muh as possible
    % (needed often in SDPs etc where solvers are less robust to weird
    % models having no interior etc)
    p = smashFixed(p,'delete');
    p = smashQPOjective(p,removethese);
    idx = find(removethese);

    % FIX: should be updated
    % safe fix now where we just remove
    p.cardinalityvariables = [];
    p.cardinalitygroups = [];
    %p.atmost.groups = [];
    %p = smashAtmost(p,idx);

    p.lb(idx)=[];
    p.ub(idx)=[];
    if ~isempty(p.x0)
        p.x0(idx)=[];
    end
    p.monomtable(:,idx)=[];
    p.monomtable(idx,:)=[];
    p.variabletype(idx) = [];

    [p,infeasible] = detectRedundantInfeasibleSOCPRows(p);
    if infeasible
        output = createOutputStructure(24);
        return
    end

    [p,infeasible] = detectRedundantInfeasibleSDPRows(p);
    if infeasible
        output = createOutputStructure(24);
        return
    end

    [p,infeasible] = detectRedundantInfeasibleEXPRows(p);
    if infeasible
        output = createOutputStructure(24);
        return
    end

    [p,infeasible] = detectRedundantInfeasiblePOWRows(p);
    if infeasible
        output = createOutputStructure(24);
        return
    end

    % We do this last, as the SOCP/EXP/SDP presolve might add trivial
    % equalities from presolving 0 >= norm(z) etc
    p = removeEmptyLPRows(p);
    [p,infeasible] = detectRedundantInfeasibleLPRows(p);
    if infeasible
        output = createOutputStructure(24);
        return
    end

    % Derive bounds from this presolved model, and if we detect new fixed
    % variables, apply recursively
    if ~isempty(p.F_struc)
        [lb,ub] = find_lp_bounds(p.F_struc,p.K,p.lb,p.ub,1);
        p.ub = min(ub,p.ub);
        p.lb = max(lb,p.lb);
    end

    if any(p.lb > p.ub)
        output = createOutputStructure(24);
        return
    end

    if any(p.lb == p.ub)
        % Recurse!
        output = bnb_solvelower(lowersolver,p,inf,lower,x_min,[]);
    elseif any(p.lb > p.ub-p.options.bnb.feastol)
        % Infeasible
        output = createOutputStructure(24);
        return
    else
        % Solve relaxation
        p.solver.version = p.solver.lower.version;
        p.solver.subversion = p.solver.lower.subversion;
        output = feval(lowersolver,p);
    end
    if output.problem == 1 || output.problem == 24
        output.Primal = [];
        return
    end
    % Recover
    x=relaxed_p.c*0;
    x(removethese)=relaxed_p.lb(removethese);
    x(~removethese)=output.Primal;
    output.Primal=x;
else
    p.solver = p.solver.lower;
    output = feval(lowersolver,p);
end

author: yalmip

repo: YALMIP

save_path: github-repos/MATLAB/yalmip-YALMIP


text:
function optFlagC = checkInputStructsROE(modelC)
% Identify optional input structures

optFlagC = cell(1,length(modelC));
for modelNum = 1:length(modelC)
    if isstruct(modelC{modelNum}.parameters.structures)
        structC = fieldnames(modelC{modelNum}.parameters.structures);
        flagV = false(1,length(structC));
        for strNum = 1:length(structC)
            strParS = modelC{modelNum}.parameters.structures.(structC{strNum});
            if isfield(strParS,'optional') && strcmpi(strParS.optional,'yes')
                flagV(strNum) = true;
            end
        end
        optFlagC{modelNum} = flagV;
    else
        optFlagC{modelNum} = false;
    end
end

end

author: cerr

repo: CERR

save_path: github-repos/MATLAB/cerr-CERR


text:
function S = nameval2struct(C)
%NAMEVAL2STRUCT Converts a cell array of name-value pairs to a struct.
% Usage:
%   struct = nameval2struct(nameval_cell)
%
% See also: struct2nameval

C = C(:);

% Check input
validateattributes(C, {'cell'}, {'vector', 'nonempty'})
assert(iseven(length(C)), ...
    'Cell arrays of name-value pairs must have an even number of elements.')
assert(all(cellfun(@ischar, C(1:2:end))), ...
    ['Cell arrays of name-value pairs must be in the format: ' ...
    '{''Name1'', Val1, ..., ''NameN'', ValN}'])

% Fix non-array inputs
N = cellfun(@numel, C(2:2:end));
if numel(unique(N)) ~= 1
    C(2:2:end) = cf(@(x) {x}, C(2:2:end));
end

% Convert to structure
S = struct(C{:});

end

author: talmo

repo: leap

save_path: github-repos/MATLAB/talmo-leap


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 [match_accepted, consistent_group] = ...
    acceptMatchIfConsistent(match, query_frame_i, query_robot_i, ...
    decentr_state, params)

this.frame_i = query_frame_i;
this.match = match;

robot_state = decentr_state{query_robot_i};
consistent_group = robot_state.consistent_groups{match.robot_i};

% Remove out-of-range matches from consistent group.
in_range = cellfun(@(x) areFramesWithinRange(robot_state, ...
            x.frame_i, query_frame_i, ...
            params.robust_relpose_consistency_range), ...
            consistent_group.members);
consistent_group.members = consistent_group.members(in_range);

% If all members were removed, group is forcibly floating again.
if (all(~in_range))
    consistent_group.floating = true;
end

% If consistent (or group empty), add current match.
consistent = cellfun(@(x) areMatchesConsistent(decentr_state, ...
    query_robot_i, x, this, params.robust_relpose_position_tolerance), ...
    consistent_group.members);
if (all(consistent))  % all([]) = true according to spec
    consistent_group.members = [consistent_group.members; this];
elseif (consistent_group.floating)
    % If not consistent and group was floating, replace group with this.
    consistent_group.members = {this};
    match_accepted = false;
    return;
else
    match_accepted = false;
    return;
end

% We only get here if the match has been added to the consitent group.

if ~consistent_group.floating
    match_accepted = true;
else
    if numel(consistent_group.members) > params.robust_relpose_min_group_size
        consistent_group.floating = false;
        match_accepted = true;
    else
        match_accepted = false;
    end
end

end

author: uzh-rpg


text:
function z=smooth_lf(x,y,varargin)

% must (unlike R smooth.lf() function) give x and y.
% also R's direct=T is automatic.
%

xev = x;
if (k>1)
  if (strcmp(varargin{1},'xev'))
    xev = varargin{2};
    varargin(1:2) = [];
  end;
end;
fit = locfit(x,y,varargin{:},'ev',xev,'module','simple');
z = lfknots(fit);
fv = invlink(z(:,1),fit.fit_points.family_link);

z = { xev, fv };

return;

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


text:
classdef STL10 < datasets.Dataset
%STL10 STL-10 dataset
%   Encapsulates the STL-10 dataset for training.
%
%   D = datasets.STL10('/data/stl') loads the STL-10 dataset from the
%   directory '/data/stl'. The dataset can be downloaded from:
%   https://cs.stanford.edu/~acoates/stl10/
%
%   D.train() returns a cell array with mini-batches, from the shuffled
%   training set. Each mini-batch consists of a set of indexes.
%
%   D.val() returns a cell array with mini-batches, from the validation set
%   (without shuffling). Each mini-batch consists of a set of indexes.
%
%   [IMAGES, LABELS] = D.get(BATCH) returns a tensor of images and the
%   corresponding labels for the given mini-batch BATCH. The images are
%   always mean-centered (by subtracting D.dataMean).
%
%   datasets.STL10(...,'option', value, ...) sets the following properties:
%
%   `batchSize`:: 128
%     The batch size.
%
%   `partialBatches`:: false
%     Whether partial batches are returned (which can happen for the last
%     batch in a set, if the batch size does not divide the set size).
%
%   `small`:: false
%     Loads smaller images with size 48x48, instead of the original 96x96.

% Copyright (C) 2018 Joao F. Henriques, Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

  properties
    images  % images tensor
    dataMean  % image mean
    labels  % labels (1 to 10)
    labelNames  % string name associated with each numeric label
    sets  % set identities (1 for training, 3 for test)
    folds  % identities of original training folds as defined in the dataset

    small = false  % downsample images 2x
  end

  methods
    function o = STL10(dataDir, varargin)
      % parse generic Dataset arguments
      varargin = o.parseGenericArgs(varargin) ;

      % parse arguments specific to this dataset
      vl_parseprop(o, varargin, {'small'}) ;

      % load object from memory cache if possible
      persistent s
      if ~isempty(s) && isequal(o.small, s.small)
        [o.images, o.dataMean, o.labels, o.labelNames, o.sets, o.folds] = ...
          deal(s.images, s.dataMean, s.labels, s.labelNames, s.sets, s.folds);

      else
        % otherwise, create from scratch
        o.loadRawData(dataDir);

        % save to cache
        [s.images, s.dataMean, s.labels, s.labelNames, s.sets, s.folds, s.small] = ...
          deal(o.images, o.dataMean, o.labels, o.labelNames, o.sets, o.folds, o.small);
      end
    end

    function [images, labels] = get(o, idx)
      % return a single batch (may be wrapped in a cell)
      if iscell(idx) && isscalar(idx)
        idx = idx{1} ;
      end
      images = (single(o.images(:,:,:,idx)) - o.dataMean) / 255;
      labels = o.labels(idx);
    end
  end

  methods (Access = protected)
    function loadRawData(o, data_dir)
      % labeled training data
      s = load([data_dir '/train.mat']);
      data = convert(s.X);
      labels = s.y(:);
      classes = s.class_names;
      folds = s.fold_indices;

      % unlabeled training data
      s = load([data_dir '/unlabeled.mat']);
      data = cat(4, data, convert(s.X));
      labels = [labels; zeros(size(s.X, 1), 1)];

      set = ones(size(labels));

      % labeled test data
      s = load([data_dir '/test.mat']);
      data = cat(4, data, convert(s.X));
      labels = [labels; s.y(:)];
      set = [set; 3 * ones(size(s.X, 1), 1)];

      % resize by 1/2 with bilinear interpolation
      if o.small
        data = uint16(data);  % avoid overflow
        data = (data(1:2:end,1:2:end,:,:) + data(2:2:end,1:2:end,:,:) + ...
                data(1:2:end,2:2:end,:,:) + data(2:2:end,2:2:end,:,:)) / 4;
        data = uint8(data);
      end

      o.images = data;
      o.labels = labels;  %#ok<*PROPLC>
      o.labelNames = classes;
      o.sets = set;
      o.folds = folds;

      o.dataMean = single(mean(data(:,:,:,set == 1), 4));

      % train and validation sample indexes
      o.trainSet = find(set == 1);
      o.valSet = find(set == 3);
    end
  end
end

% simple helper function to convert STL-10 array to MatConvNet format
function X = convert(X)
  X = permute(reshape(X, [], 96, 96, 3), [2:4, 1]);
end

author: vlfeat

repo: autonn

save_path: github-repos/MATLAB/vlfeat-autonn


text:
function edge_im = draw_line_image2(edge_im,linesnnorm , grpno);

if grpno==1
     color='r';
elseif grpno==2
    color='g';
else
    color='b';

end

    for i=1:size(linesnnorm,2)
        x1=lines(1,i);
        x2=lines(2,i);
        y1=lines(3,i);
        y2=lines(4,i);

        line([y1 y2],[x1 x2],'Color',color);

    end

    return

img=imread([dirname d(1).name]);
grayIm=rgb2gray(img);
[lines] = APPgetLargeConnectedEdges(grayIm, 20);
imsize=size(img);

[v2, sigma, p, hpos] = APPestimateVp(lines, imsize, DO_DISPLAY);
[tmp, bestv] = max(p, [], 2);

figure(1001);
imshow(img)
tempind=find(bestv==1);
hold on, plot(lines(tempind, [1 2])', lines(tempind, [3 4])','r')
tempind=find(bestv==2);
hold on, plot(lines(tempind, [1 2])', lines(tempind, [3 4])','g')
tempind=find(bestv==3);
hold on, plot(lines(tempind, [1 2])', lines(tempind, [3 4])','b')
v2=v2.*imsize(1);

imagesc(img);
hold on; plot(v2(1,1)/v2(1,3),v2(1,2)/v2(1,3),'r*');
plot(v2(2,1)/v2(2,3),v2(2,2)/v2(2,3),'g*');
plot(v2(3,1)/v2(3,3),v2(3,2)/v2(3,3),'b*');

author: zouchuhang

repo: LayoutNet


text:
function varargout = modschaffer4(X)
% modified Schaffer function, #4
%
%   MODSCHAFFER4([x1, x2]) returns the value of the 4th Schaffer
%   function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -100 < x_i < 100
%
%   The global minimum is
%
%               f(x1, x2) = f(0, 1.25313) = 0.292579.

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-100, -100]; % LB
        varargout{3} = [+100, +100]; % UB
        varargout{4} = [0, 1.253131828927371e+000]; % solution
        varargout{5} = 2.925786320359805e-001; % function value at solution

    % otherwise, output function value
    else

        % keep values within the search interval
        X(X < -100) = inf;      X(X > 100) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = 0.5  + (cos(sin(abs(x1.^2 - x2.^2))).^2 - 0.5) ./ (1+0.001*(x1.^2 + x2.^2)).^2;

    end

end


text:
function y = iterapp(op,afun,atype,afcnstr,x,varargin)
%ITERAPP   Apply matrix operator to vector and error gracefully.
%   ITERAPP(OP,AFUN,ATYPE,AFCNSTR,X) applies matrix operator AFUN to vector
%   X. If ATYPE is 'matrix, then AFUN is a matrix and the OP is applied
%   directly. OP is either 'mtimes' or 'mldivide'.
%   ATYPE and AFCNSTR are used in case of error.
%   ITERAPP(OP,AFUN,ATYPE,AFCNSTR,X,P1,P2,...) allows extra arguments to
%   AFUN(X,P1,P2,...) although this usage is now discouraged in favor of
%   using anonymous functions.
%   AFUN(X,P1,P2,...,PN,TFLAG) should accept a TFLAG as its final input if
%   the calling function is BICG, LSQR or QMR. TFLAG is either 'transp' or
%   'notransp' depending on whether A' OP X or A OP X is required.
%   ITERAPP is designed for use by iterative methods like PCG which
%   require matrix operators AFUN representing matrices A to operate on
%   vectors X and return A*X and may also have operators MFUN representing
%   preconditioning matrices M operate on vectors X and return M\X.
%
%   See also BICG, BICGSTAB, CGS, GMRES, LSQR, MINRES, PCG, QMR, SYMMLQ.

%   Copyright 1984-2013 The MathWorks, Inc.

if strcmp(atype,'matrix')
    switch lower(op)
        case 'mtimes'
            if (nargin >= 6) && isequal(varargin{end},'transp')
                y = afun' * x;
            else
                y = afun * x;
            end
        case 'mldivide'
            if (nargin >= 6) && isequal(varargin{end},'transp')
                y = afun' \ x;
            else
                y = afun \ x;
            end
        otherwise
            error(message('MATLAB:iterapp:InvalidOp'))
    end
else
%     try
        if (nargin >= 6) && isequal(varargin{end},'notransp')
            % A request for A*x coming from BICG, LSQR and QMR
            try
                % New syntax: we now request afun(x,P1,P2,...,PN,'notransp')
                y = afun(x,varargin{:});
            catch
                % Old syntax: we used to accept afun(x,P1,P2,...,PN)
                y = afun(x,varargin{1:end-1});
            end
        else
            % A request for A*x
            % coming from BICGSTAB, CGS, GMRES, MINRES, PCG or SYMMLQ
            % with the call always afun(P1,P2,...,PN)
            % or a request for A'*x coming from
            % BICG, LSQR and QMR in the afun(x,P1,P2,...,PN,'transp') case
            y = afun(x,varargin{:});
        end
%     catch ME
%         error(message('MATLAB:iterapp:InvalidInput', atype,afcnstr, ME.message));
%     end

%     if ~iscolumn(y)
%         error(message('MATLAB:iterapp:MustReturnColumn', atype, afcnstr));
%     end
end

repo: CFWCR


text:
function [subj_descriptives, event_descriptives] = get_descriptives(D, varargin)
% Returns descriptive stats for every variable, or specified variables
%
% :Usage:
% ::
%
%    print_summary(D, [optional inputs])
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2013 Tor Wager
%
%     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/>.
% ..
%
%
% :Inputs:
%
%   **D:**
%        a canlab_dataset object
%
% :Optional Inputs:
%
%   **subj:**
%        followed by a cell array of subject level var names, to only see those vars
%
%   **event:**
%        followed by a cell array of event level var names, to only see those vars
%
%   if either varargin is unspecified, all variables will be printed
%
% Examples:
%
% % Get descriptive stats in tables all variables in canalab_dataset object D
% [subj_descriptives, event_descriptives] = get_descriptives(D, 'event', 'temp')
%
% Get descriptive stats in tables for the Event_Level variable 'temp'
% [subj_descriptives, event_descriptives] = get_descriptives(D, 'event', 'temp')
%
% Get descriptive stats in tables for the Event_Level variables 'temp' and 'ratings'
%[subj_descriptives, event_descriptives] = get_descriptives(D, 'event', {'temp' 'ratings'})

subj_descriptives = table;
event_descriptives = table;

% Default: All variables
subj_varnames = D.Subj_Level.names;
if iscolumn(subj_varnames), subj_varnames = subj_varnames'; end % need row

event_varnames = D.Event_Level.names;
if iscolumn(event_varnames), event_varnames = event_varnames'; end % need row

svars = find(strcmp('subj', varargin));
evars = find(strcmp('event', varargin));

if ~isempty(svars) || ~isempty(evars)
    % Replace default lists with input variables only
    [subj_varnames, event_varnames] = deal({});
end

% Subject level
% -------------------------------------------------------------------------

if ~isempty(svars), subj_varnames = varargin{svars+1}; end
if ~isempty(svars) && ~iscell(subj_varnames), subj_varnames = {subj_varnames}; end

Var_name = subj_varnames';
clear Min_value Max_value Mean_value St_Dev IQR NaN_count

for i=1:length(subj_varnames)

    vname = subj_varnames{i};

    [Min_value(i, 1) Max_value(i, 1) Mean_value(i, 1) St_Dev(i, 1) IQR(i, 1) NaN_count(i, 1), text_vals] = get_descriptives_var(D, vname);

end

if length(subj_varnames) > 0
    subj_descriptives = table(Var_name, Min_value, Max_value, Mean_value, St_Dev, IQR, NaN_count);
end

% Event level
% -------------------------------------------------------------------------
if ~isempty(evars), event_varnames = varargin{evars+1}; end
if ~isempty(evars) && ~iscell(event_varnames), event_varnames = {event_varnames}; end

Var_name = event_varnames';
clear Min_value Max_value Mean_value St_Dev IQR NaN_count

for i=1:length(event_varnames)

    vname = event_varnames{i};

    [Min_value(i, 1) Max_value(i, 1) Mean_value(i, 1) St_Dev(i, 1) IQR(i, 1) NaN_count(i, 1), text_vals] = get_descriptives_var(D, vname);

end

if length(event_varnames) > 0
    event_descriptives = table(Var_name, Min_value, Max_value, Mean_value, St_Dev, IQR, NaN_count);
end

end % main function

function [Min_value, Max_value, Mean_value, St_Dev, myIQR, NaN_count, text_vals] = get_descriptives_var(D, vname)

% intialize
[Min_value, Max_value, Mean_value, St_Dev, myIQR, NaN_count] = deal(NaN);
text_vals = [];

[var, varcell, ~, descrip] = get_var(D, vname);

if isempty(var)
    return
end

if ismatrix(var)
    % Happens when this is Event_Level var with same number of events for
    % all subjects; or padded with NaNs.
    var = var(:);
end

if ischar(var)
    % This happens for event-level variables with different numbers of
    % observations for different cells. This issue may be removed in the
    % future, however.

    if iscell(varcell{1})
        fprintf('%s (%s): Cell array. Length of first cell: %d\t\n', ...
        vname, descrip, length(varcell{1}));

    else

        var = double(cat(1, varcell{:}));

    end
end

if iscell(var)  % SHOULD BE DEPRECATED - need to handle text differently
    % istext

    % check for some (partial) empty vars
    is_empty = cellfun(@isempty, var);
    num_empty = sum(is_empty);

    if num_empty > 0 && num_empty < length(var)
        warning(sprintf('Some values of %s are missing.', vname))
    end

    fprintf('%s (%s): Text. Unique values: %d\t\n', ...
        vname, descrip, length(unique(var(~is_empty))));

else
    % Standard Subj-level and Event-level vars with same no. of obs.
    % isnumeric
    Min_value = min(var);
    Max_value = max(var);
    Mean_value = nanmean(var);
    St_Dev = nanstd(var);
    myIQR = iqr(var);
    NaN_count = sum(isnan(var));
end

end

author: canlab

repo: CanlabCore

save_path: github-repos/MATLAB/canlab-CanlabCore


text:
function f=GA_demo(x)
f1=4*x(1).^3+4*x(1)*x(2)+2*x(2).^2-42*x(1)-14;
f2=4*x(2).^3+4*x(1)*x(2)+2*x(1).^2-26*x(1)-22;
f=f1.^2+f2.^2;

author: HuangCongQing


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.

function y = FSABR2x(ulimit, f)
% F(X>ulimit) based on FSABR2
    y = 1- FSABR2(ulimit,f);
end


text:
function d = spm_shoot_defaults
% Defaults file
% FORMAT d = spm_shoot_defaults
% This file contains settings that are intended to be customised
% according to taste.  Some of them will influence the speed/accuracy
% tradeoff, whereas others are various regularisation settings
% (registration and template blurring)...

%_______________________________________________________________________
% Copyright (C) Wellcome Trust Centre for Neuroimaging (2009)

% John Ashburner
% $Id: spm_shoot_defaults.m 6489 2015-06-26 11:46:29Z john $

%_______________________________________________________________________
% The following settings are intended to be customised according to
% taste.  Some of them will influence the speed/accuracy tradeoff,
% whereas others are various regularisation settings (registration
% and template blurring)...

d.tname   = 'Template'; % Base file name for templates
d.issym   = false;      % Use a symmetric template?

d.cyc_its = [2 2];      % No. multigrid cycles and iterations

nits      = 24;         % No. iterations of Gauss-Newton

% Schedule for coarse to fine
lam     = 0.5;          % Decay of coarse to fine schedule
inter   = 32;           % Scaling of parameters at first iteration
d.sched = (inter-1)*exp(-lam*((1:(nits+1))-1))+1;
d.sched = d.sched/d.sched(end);

maxoil    = 8;                          % Maximum number of time steps for integration
d.eul_its = round((0:(nits-1))*(maxoil-0.5001)/(nits-1)+1); % Start with fewer steps

d.rparam  = [1e-4 0.001 0.2 0.05 0.2];  % Regularisation parameters for deformation
d.sparam  = [0.0001 0.08 0.8];          % Regularisation parameters for blurring
d.smits   = 16;                         % No. smoothing iterations
%d.smits   = 0; d.sparam = [];
d.scale   = 0.8;                        % Fraction of Gauss-Newton update step to use

d.bs_args = [2 2 2  1 1 1];             % B-spline settings for interpolation
%_______________________________________________________________________

author: spm


text:
function [ y2, m2, d2, f2 ] = ymdf_inc_english ( y1, m1, d1, f1, days )

%*****************************************************************************80
%
%% YMDF_INC_ENGLISH increments an English YMDF date by DAYS days.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 April 2103
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the YMDF date.
%
%    Input, real DAYS, the number of days to advance the date.
%
%    Output, integer Y2, M2, D2, real F2, the
%    incremented YMDF date.
%

%
%  Copy the parameters.
%
  y2 = y1;
  m2 = m1;
  d2 = d1;
  f2 = f1 + days;
%
%  Check the parameters.
%
  [ y2, m2, d2, f2, ierror ] = ymdf_check_english ( y2, m2, d2, f2 );

  return
end


text:
% GRAVLOAD Joint loads due to gravity
%
% Uses either the rne MEX file, from RTB, or the grav function, from
% pHRIWARE, depending on being able to use the MEX file and if the
% Jacobian is requested to be returned
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file requires file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Syntax:
%  (1) tauB = robot.gravload(q)
%  (2) [tauB, J] = robot.grav(q)
%  (3) ... = robot.grav(q, grav)
%
%  (2) is as per (1) but also returns world-frame Jacobian - pHRIWARE
%  (3) is as per others but with explicit gravity vector
%
% Outputs:
%  tauB : Generalised joint force/torques
%  J    : Jacobian in world frame
%
% Inputs:
%  q     : Joint row vector, or trajectory matrix of joint row vectors
%  grav  : Gravity _reaction_ vector (as RTB default is [0 0 +9.81]')
%
% See also grav, rne

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE 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.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license

function varargout = gravload(robot, varargin)

if robot.fast && ~robot.issym() && nargout <= 1
   qz = zeros(size(varargin{1}));
   rne_input = {varargin(1), qz, qz, varargin(2:end)};
   varargout = robot.frne(rne_input{:});
else
   pHRIWARE('c');
   varargout = robot.grav(varargin{:});
end

end

author: Allopart

repo: rbpf-gmapping


text:
%ISUNTRAINED Test on untrained mapping
%
%    I = ISUNTRAINED(W)
%    ISUNTRAINED(W)
%
% True if the mapping type of W is 'untrained' (see HELP MAPPINGS).
% If called without an output argument ISUNTRAINED generates
% an error if the mapping type of W is not 'untrained'.

% $Id: isuntrained.m,v 1.1 2009/03/18 16:12:41 duin Exp $

function i = isuntrained(w)

  if iscell(w)
		i = strcmp(w{1}.mapping_type,'untrained');
		for j=2:length(w)
			if strcmp(w{j}.mapping_type,'untrained') ~= i
				error('Cell array of classifiers cannot be partially trained')
			end
		end
	else
		i = strcmp(w.mapping_type,'untrained');
	end

	if (nargout == 0) & (i == 0)
		error([newline '---- Untrained mapping expected ----'])
	end
return

author: marianux

repo: ecg-kit

save_path: github-repos/MATLAB/marianux-ecg-kit


text:
function [C, sigma] = dataset3Params(X, y, Xval, yval)
%DATASET3PARAMS returns your choice of C and sigma for Part 3 of the exercise
%where you select the optimal (C, sigma) learning parameters to use for SVM
%with RBF kernel
%   [C, sigma] = DATASET3PARAMS(X, y, Xval, yval) returns your choice of C and
%   sigma. You should complete this function to return the optimal C and
%   sigma based on a cross-validation set.
%

% You need to return the following variables correctly.
C = 1;
sigma = 0.3;

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return the optimal C and sigma
%               learning parameters found using the cross validation set.
%               You can use svmPredict to predict the labels on the cross
%               validation set. For example,
%                   predictions = svmPredict(model, Xval);
%               will return the predictions on the cross validation set.
%
%  Note: You can compute the prediction error using
%        mean(double(predictions ~= yval))
%

error = 1;
for C_temp = [0.01, 0.03, 0.1, 0.3, 1, 3, 10, 30]
    for sigma_temp = [0.01, 0.03, 0.1, 0.3, 1, 3, 10, 30]
        model= svmTrain(X, y, C_temp, @(x1, x2) gaussianKernel(x1, x2, sigma_temp));
        predictions = svmPredict(model, Xval);
        error_temp = mean(double(predictions ~= yval));
        if  error_temp < error
            error = error_temp;
            C = C_temp;
            sigma = sigma_temp;
        end
    end
end

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

end

author: imLogM


text:
% the MWF beamformer
% r1MWF:    h = PhiN^-1 * PhiX * refVec / (mu + trace(PhiN^-1 * PhiX)
% SDWMWF:   h = (PhiX + mu * PhiN)^-1 * PhiX * refVec
% input:    PhiN, (Nch, Nch, Nbin) the noise covariance matrix
%           PhiX, (Nch, Nch, Nbin) the speech covariance matrix
%           mu, the speech distortion/noise reduction trade-off parameter
%           refMic, the reference microphone, default = 1
% output:   h, (Nch, Nbin)  the beamformer coefficients
% % Ziteng Wang @ 201812

function h = MWF(PhiX, PhiN, mu, refMic, choice)
if nargin < 3
    mu = 1;                 % typical value {0, 1}
elseif nargin < 4
    refMic = 1;
elseif nargin < 5
    choice = 'r1MWF';       % collected from 'r1MWF' 'SDWMWF'
end

[Nch, ~, Nbin] = size(PhiX);
refVec = zeros(Nch, 1);
refVec(refMic) = 1;
lambda = zeros(Nbin, 1);
h = zeros(Nch, Nbin);

for bin = 1:Nbin
    if rcond(PhiN(:,:,bin)) < eps
        disp(['bin ' num2str(bin) ': Noise covariance ill-conditioned.']);
        PhiN(:,:,bin) = PhiN(:,:,bin) + 1e-10 * eye(Nch);
    end
    if strcmp(choice, 'r1MWF')
        tmp = PhiN(:,:,bin) \ PhiX(:,:,bin);
        lambda(bin) = trace(tmp);
        h(:,bin) = tmp * refVec / (mu + lambda(bin));
        %%% Gain factor to normalize the steering vector. The results are
        %%% better. Needs further checking
        %gain_factor = norm(PhiX(:,refMic,bin) / PhiX(refMic,refMic,bin));
        %h(:,bin) = gain_factor * h(:,bin);
    elseif strcmp(choice, 'SDWMWF') % SDW-MWF is more sensitive to estimation errors.
        tmp = (PhiX(:,:,bin) + mu * PhiN(:,:,bin)) \ PhiX(:,:,bin);
        h(:,bin) = tmp * refVec;
    end
end

author: ZitengWang

repo: MASP

save_path: github-repos/MATLAB/ZitengWang-MASP


text:
function [G1,Gd1,Wd,Wn,Juu,Jud]=randcase(ny,nu,nd)
% RANDCASE  A random case for self-optimizing control
%
%   [G,Gd,Wd,Wn,Juu,Jud] = randcase(ny,nu,nd) produces matrices of a random
%   case for self-optimizing control corresponding to the following
%   optimizing control problem:
%
%   min J(y,u,d)
%   s.t.  y = G*u + Gd*Wd*d + Wn*e
%
%   with Juu = \partial^2 J/\partial u \partial u
%        Jud = \partial^2 J/\partial u \partial d
%
%  These matrices can then be used to test the b3wc program.
%
%  See also b3wc, pb3wc, b3av, pb3av

%  By Yi Cao at Cranfield University, 9th January 2009; 23rd February 2010.
%

% Example.
%{
ny=30;
nu=15;
nd=5;
[G,Gd,Wd,Wn,Juu,Jud] = randcase(ny,nu,nd);
[B,sset,ops,ctime] = b3wc(G,Gd,Wd,Wn,Juu,Jud);
[B1,sset1,ops1,ctime1] = pb3wc(G,Gd,Wd,Wn,Juu,Jud,20);
%}
G1=randn(ny,nu);
Gd1=rand(ny,nd);
Wd=diag(rand(nd,1));
Wn=diag(rand(ny,1));
x=randn(nu,nu+2*ny+nd);
Juu=diag(sum(x.*x,2));
Jud=rand(nu,nd);


text:
function CPD = update_tied_ess(CPD, domain, engine, evidence, ns, cnodes)

if ~adjustable_CPD(CPD), return; end
nCPDs = size(domain, 2);
fmarginal = cell(1, nCPDs);
for l=1:nCPDs
  fmarginal{l} = marginal_family(engine, nodes(l));
end

[ss cpsz dpsz] = size(CPD.weights);
if const_evidence_pattern(engine)
  dom = domain(:,1);
  dnodes = mysetdiff(1:length(ns), cnodes);
  ddom = myintersect(dom, dnodes);
  cdom = myintersect(dom, cnodes);
  odom = dom(~isemptycell(evidence(dom)));
  hdom = dom(isemptycell(evidence(dom)));
  % If all hidden nodes are discrete and all cts nodes are observed
  % (e.g., HMM with Gaussian output)
  % we can add the observed evidence in parallel
  if mysubset(ddom, hdom) & mysubset(cdom, odom)
    [mu, Sigma, T] = add_cts_ev_to_marginals(fmarginal, evidence, ns, cnodes);
  else
    mu = zeros(ss, dpsz, nCPDs);
    Sigma = zeros(ss, ss, dpsz, nCPDs);
    T = zeros(dpsz, nCPDs);
    for l=1:nCPDs
      [mu(:,:,l), Sigma(:,:,:,l), T(:,l)] = add_ev_to_marginals(fmarginal{l}, evidence, ns, cnodes);
    end
  end
end
CPD.nsamples = CPD.nsamples + nCPDs;

if dpsz == 1 % no discrete parents
  w = 1;
else
  w = fullm.T(:);
end
CPD.Wsum = CPD.Wsum + w;
% Let X be the cts parent (if any), Y be the cts child (self).
xi = 1:cpsz;
yi = (cpsz+1):(cpsz+ss);
for i=1:dpsz
  muY = fullm.mu(yi, i);
  SYY = fullm.Sigma(yi, yi, i);
  CPD.WYsum(:,i) = CPD.WYsum(:,i) + w(i)*muY;
  CPD.WYYsum(:,:,i) = CPD.WYYsum(:,:,i) + w(i)*(SYY + muY*muY'); % E[X Y] = Cov[X,Y] + E[X] E[Y]
  if cpsz > 0
    muX = fullm.mu(xi, i);
    SXX = fullm.Sigma(xi, xi, i);
    SXY = fullm.Sigma(xi, yi, i);
    CPD.WXsum(:,i) = CPD.WXsum(:,i) + w(i)*muX;
    CPD.WXYsum(:,:,i) = CPD.WXYsum(:,:,i) + w(i)*(SXY + muX*muY');
    CPD.WXXsum(:,:,i) = CPD.WXXsum(:,:,i) + w(i)*(SXX + muX*muX');
  end
end

%%%%%%%%%%%%%

function fullm = add_evidence_to_marginal(fmarginal, evidence, ns, cnodes)

dom = fmarginal.domain;

% Find out which values of the discrete parents (if any) are compatible with
% the discrete evidence (if any).
dnodes = mysetdiff(1:length(ns), cnodes);
ddom = myintersect(dom, dnodes);
cdom = myintersect(dom, cnodes);
odom = dom(~isemptycell(evidence(dom)));
hdom = dom(isemptycell(evidence(dom)));

dobs = myintersect(ddom, odom);
dvals = cat(1, evidence{dobs});
ens = ns; % effective node sizes
ens(dobs) = 1;
S = prod(ens(ddom));
subs = ind2subv(ens(ddom), 1:S);
mask = find_equiv_posns(dobs, ddom);
subs(mask) = dvals;
supportedQs = subv2ind(ns(ddom), subs);

if isempty(ddom)
  Qarity = 1;
else
  Qarity = prod(ns(ddom));
end
fullm.T = zeros(Qarity, 1);
fullm.T(supportedQs) = fmarginal.T(:);

% Now put the hidden cts parts into their right blocks,
% leaving the observed cts parts as 0.
cobs = myintersect(cdom, odom);
chid = myintersect(cdom, hdom);
cvals = cat(1, evidence{cobs});
n = sum(ns(cdom));
fullm.mu = zeros(n,Qarity);
fullm.Sigma = zeros(n,n,Qarity);

if ~isempty(chid)
  chid_blocks = block(find_equiv_posns(chid, cdom), ns(cdom));
end
if ~isempty(cobs)
  cobs_blocks = block(find_equiv_posns(cobs, cdom), ns(cdom));
end

for i=1:length(supportedQs)
  Q = supportedQs(i);
  if ~isempty(chid)
    fullm.mu(chid_blocks, Q) = fmarginal.mu(:, i);
    fullm.Sigma(chid_blocks, chid_blocks, Q) = fmarginal.Sigma(:,:,i);
  end
  if ~isempty(cobs)
    fullm.mu(cobs_blocks, Q) = cvals(:);
  end
end

author: bayesnet

repo: bnt

save_path: github-repos/MATLAB/bayesnet-bnt


text:
function d =  generate(a)

 if ~isa(a.child,'cell') r=[]; r{1}=a.child; a.child=r; end;

 ps=a.prior;
 for j=1:length(ps)
   ps(j)=sum(ps(1:j));
   a.child{j}.l=1;
 end
 ps=[0 ps];

 k=length(a.child);

 xs=[]; ys=[];
 for i=1:a.l
   p=rand; j=max(find(p>ps)); %% choose class
   x=get_x(test(a.child{j})); %% generate data with that class
   y=-ones(1,k); y(j)=1;
   xs=[xs ; x]; ys=[ys ; y];
 end

 if size(ys,2)==2 ys=ys(:,1); end;

 d=data(get_name(a),xs,ys);