function [image_information_metric, rniirs, diagnostic] = RGIQE(filename, varargin)
% RGIQE Radar generalized image quality equation
%    [image_information_metric, rniirs] = RGIQE(bandwidth, nesz_db, graze_degrees)
%    [image_information_metric, rniirs] = RGIQE(filename, 'PropertyName', PropertyValue, ...)
%
% An implementation of a generalized image quality equation to provide a
% quantitative image quality metric and an RNIIRS prediction.
%
% RNIIRS (Radar National Imagery Interpretation Rating Scale) is an image
% quality scale from 1 to 9.  This "quality" refers to local image fidelity
% and a users ability to perform certain detection and recognition tasks
% with an image.  The "quality" that RNIIRS attempts to capture does NOT
% refer to image size, geopositioning accuracy, radiometric accuracy, or
% any other host of characterists that may improve an image's utility.
% Although RNIIRS is subjective, generally assessed by a trained analyst,
% an image quality equation (IQE) may be used to predict RNIIRS for a given
% SAR collect.
%
% The General Image Quality Equation (GIQE) provided here uses generic,
% fundamental, sensor-indepenedent parameters of radar collection to
% measure local image fidelity by quantifying the information density in
% the ground in terms of bits\m^2, using Shannon's channel capacity
% theorem.  Unlike RNIIRS, this value is quantitative and not subjective or
% based on analysts' assessments.  We can however compute an RNIIRS
% prediction based off of it by fitting the information metric to analysts
% ratings.  Both the information theory metric and the RNIIRS prediction
% are computed in this function.
%
%    [image_information_metric, rniirs] = RGIQE(bandwidth, nesz_db, graze_degrees)
% INPUTS:
%
%    bandwidth               Transmitted (and processed) bandwidth in Hz.
%    nesz_db                 Noise equivalent sigma-0 in dB.  Should
%                               capture all noise, including multiplicative
%                               sources.
%    graze_degrees           Grazing angle in degrees.
%
%
%    [image_information_metric, rniirs] = RGIQE(filename, 'PropertyName', PropertyValue, ...)
% INPUTS:
%
%    filename                Must either be a complex image in a format
%                               recognized by the MATLAB SAR Toolbox or an
%                               XML file with SICD-formatted XML.  If empty
%                               or not passed, the user will be prompted to
%                               select a file with a dialog box.
%
%    Property name           Description
%    multi_noise             Multiplicative noise.  This value is equal to:
%
%                               MNR * sigma_0_background
%
%                               where MNR is the multiplicative noise ratio
%                               and sigma_0_background is the assumed power
%                               level of the background and ambiguous
%                               areas.
%                               MNR can further be broken down into:
%
%                               MNR = ISLR + QNR + AMBR
%
%                               where ISLR is the integrated sidelobe
%                               ratio, QNR is the quantization noise ratio,
%                               and AMBR is the ambiguity ratio.
%                               (Default = 0)
%    interactive             "AUTO", "NOISE", "SIGNAL", "BOTH", or "NONE".
%                               Enable interactive measurement of noise and
%                               signal levels.  "AUTO" only runs
%                               interactive measurment if metadata from
%                               source file does not provide it. (Default
%                               is "AUTO").
%    frames                  If a file contains more than one complex
%                               dataset, this value specifies for which of
%                               those datasets to compute metrics. (Default
%                               is all datasets in the file.)
%    signal_sigma_override   Allows caller to override signal level used
%                               for computation (and INTERACTIVE input
%                               parameter).  In general, this is not
%                               recommended, as RNIIRS estimation function
%                               was trained using the default values. If
%                               the dataset provided is radiometrically
%                               calibrated, this value will be treated as
%                               calibrated signal level in the ground plane
%                               (sigma-0).  If the dataset is not
%                               calibrated, this value will be assume to be
%                               average pixel power. (Default is 1 square
%                               meter of radar return per square meter on
%                               the ground for co-pol, 0.25 for cross-pol.)
%
% Note that the selection of signal level and noise level are fundamentally
% different in this model.  Noise is measureable, quantifiable, and likely
% fairy accuractely predictable (at least for additive noise).  There is a
% right answer for what the noise level is for any given dataset. On the
% other hand, signal level is somewhat arbitrarily chosen, selected to be
% the power level of the scene content that one may be most interested in
% examining.  It is for this reason that this function allows for a signal
% level override, but not a direct noise level override (although noise can
% be adjusted with the MULTI_NOISE input parameter.)
%
% Author: Wade Schwartzkopf, NGA/Research

% Of the lines in this file, >95% is file and input parameter parsing and
% documention, and <5% actually computing the image quality metrics...

%% Bandwidth formulation
% Assumes azimuth bandwidth to match range in ground plane
% NESZ provided should capture all noise, including multiplicative sources
if nargin == 3 && isnumeric(filename) && isnumeric(varargin{1}) && isnumeric(varargin{2})
    bandwidth = filename;  % Hz
    nesz = varargin{1};  % in dB
    graze = varargin{2};  % in degrees
    bandwidth = bandwidth*2/SPEED_OF_LIGHT;  % Convert units from Hz to cycles/m
    bandwidth = bandwidth*cosd(graze);  % Convert from slant plane to ground plane
    bandwidth = bandwidth.^2;  % Convert from 1D bandwidth to 2D bandwidth area (assumes azimuth bandwidth same as range)
    image_information_metric = bandwidth.*log2(1+1/(10^(nesz/10)));
    rniirs = estimate_rniirs(image_information_metric);
    diagnostic = struct();  % No diagnostics for this version
    return;
end

%% SICD/SLC version.
%% Open input file
if ((nargin<1)||isempty(filename)) % If no filename was give, use dialog box to ask for one
    % Recall last interactively selected path used
    if ispref('matlab_sar_toolbox','last_used_directory')
        pathname = getpref('matlab_sar_toolbox','last_used_directory');
        if ~ischar(pathname)||~exist(pathname,'dir')
            pathname = pwd;
        end
    else
        pathname = pwd;
    end
    [filename,pathname]=uigetfile(sar_file_extensions(),...
        'Open SAR Data File',pathname);
    setpref('matlab_sar_toolbox','last_used_directory',pathname);
else % Path was already passed in with filenames
    pathname='';
end
if(filename)
    fullfilename=[pathname filename];
else % filename=0.  Cancel was pressed, instead of a file being chosen.
    return;
end
if ~isempty(pathname)
    setpref('matlab_sar_toolbox','last_used_directory',pathname); %store path
end
try
    ro = open_reader(fullfilename);
    % Check for multiple images
    if ~iscell(ro)
        ro = {ro};
    end
    meta = cell(numel(ro),1);
    for i = 1:numel(ro)
        meta{i} = ro{i}.get_meta();
    end
catch
    try
        fid = fopen(fullfilename);
        meta = {sicdxml2struct(xmlread( java.io.StringBufferInputStream( char(fread(fid)'))))};
        fclose(fid);
    catch
        error('RGIQE:InvalidFile','Invalid File.');
    end
end

%% Parse other input parameters
p = inputParser;
p.addParameter('multi_noise', 0, @(x) isnumeric(x) && isscalar(x));
p.addParameter('interactive', 'AUTO', @(x) ismember(upper(x), ...
    {'AUTO', 'NOISE', 'SIGNAL', 'BOTH', 'NONE'}));
p.addParameter('frames', 1:numel(meta), @(x) all(ismember(x, 1:numel(meta))));
p.addParameter('signal_sigma_override', [], @(x) isnumeric(x) && isscalar(x));
parse(p,varargin{:})

%% Iterate through datasets provided
meta = meta(p.Results.frames);
image_information_metric = zeros(numel(meta),1);
rniirs = zeros(numel(meta),1);
for i = 1:numel(meta)
    meta{i} = derived_sicd_fields(meta{i});  % Assure all radiometric fields are derived if not there

    %% Get noise level
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric,'NoiseLevel') && ...
            isfield(meta{i}.Radiometric.NoiseLevel,'NoiseLevelType') && ...
            strcmpi(meta{i}.Radiometric.NoiseLevel.NoiseLevelType,'ABSOLUTE') && ...
            isfield(meta{i}.Radiometric.NoiseLevel,'NoisePoly') && ...
            ~ismember(upper(p.Results.interactive), {'NOISE', 'BOTH'})
        % SICD metadata can only tell us additive noise.  Estimation of
        % multiplicative noise must come from elsewhere.
        noise_sigma = 10^(meta{i}.Radiometric.NoiseLevel.NoisePoly(1)/10) + p.Results.multi_noise;
    elseif ismember(upper(p.Results.interactive), {'AUTO', 'NOISE', 'BOTH'})
        % Measured noise contains both additive and multiplicative noise
        uiwait(msgbox('Please select a noise region for measurement.','Noise measurement'));
        aoi = mitm_viewer(fullfilename,'mode','aoi','closeAfterSelect',true,'initialFrame',i);
        if isempty(aoi), return; end  % Cancel was selected
        cdata = double(ro{i}.read_chip(...
            [aoi(1) aoi(1)+aoi(3)-1],[aoi(2) aoi(2)+aoi(4)-1]));
        noise_sigma = mean(abs(cdata(:)).^2);
    else
        noise_sigma = NaN;
    end
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
        % Scale to be equal to noise equivalent sigma zero
        noise_sigma = meta{i}.Radiometric.SigmaZeroSFPoly(1) * noise_sigma;
    end

    %% Get signal level
    % One issue with using the Shannon-Hartley channel capacity formulation
    % is that it requires a signal level.  Certainly the amount of
    % information a SAR image conveys is related to its signal level, so
    % this is how we hope it would behave.  A target with brighter returns
    % is always easier to interpret than a target that is in the noise.
    % However, it presents a problem to image quality prediction, since the
    % signal level can't be known prior to measurement.  This constraint is
    % implicitly true for traditional RNIIRS assessments as well, which
    % assumes specific types of targets.
    % One approach for setting the signal level used in the channel
    % capacity equation would be to assume a fixed radiometric signal level
    % that is representative of the sorts of scene content we are generally
    % interested in and use that same signal level across all images for
    % which we compute this metric.  This allows for an even
    % apples-to-apples comparison across images of varying content and is
    % consistent with the traditional RNIIRS viewpoint, which assumes
    % specific types of tasks and scene content.
    % Another approach would be to use the signal level in the actual scene
    % by measuring the average pixel power across a full image or AOI. This
    % approach may be more accurate at measuring the true information in
    % any given image area.  However, it may be less consistent with the
    % traditional RNIIRS viewpoint, and this approach can only be used
    % after the collection has been made, not prior to it for planning
    % collection and predicting image quality.
    % We also allow the caller of the function to pass their own signal
    % level. Passing a non-default signal level is not recommended for
    % RNIIRS estimation, since the RNIIRS function was trained on the
    % default values.  However, we provide this flexibility in case it
    % might be educational to see how it affects the information theory
    % metric.
    if ~ismember('signal_sigma_override', p.UsingDefaults)
        signal_sigma = p.Results.signal_sigma_override;
    elseif isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly') && ...
            ~ismember(upper(p.Results.interactive), {'SIGNAL', 'BOTH'})
        % Default value: 1 square meter of return per square meter on
        % the ground (roughly similar to a large vehicle over its area).
        % Ulaby plots show cross-pol generally 5-10dB lower than
        % co-pol.  We pick 1/4 as simple round number.
        if isfield(meta{i},'ImageFormation') && ...
                isfield(meta{i}.ImageFormation,'TxRcvPolarizationProc') && ...
                numel(unique(split(meta{i}.ImageFormation.TxRcvPolarizationProc,':')))>1
            signal_sigma = 0.25;
        else
            signal_sigma = 1;
        end
    elseif ismember(upper(p.Results.interactive), {'AUTO', 'SIGNAL', 'BOTH'})
        ButtonName = questdlg(['Do you want to use average signal '...
            'power across entire image or select an AOI?'], ...
            'Signal level estimation', ...
            'Image', 'AOI', 'Image');
        if isempty(ButtonName), return; end %window was closed CANCEL
        if strcmpi(ButtonName,'AOI')
            aoi = mitm_viewer(fullfilename,'mode','aoi','closeAfterSelect',true,'initialFrame',i);
            if isempty(aoi), return; end  % Cancel was selected
            cdata = double(ro{i}.read_chip(...
                [aoi(1) aoi(1)+aoi(3)-1],[aoi(2) aoi(2)+aoi(4)-1]));
            signal_sigma = mean(abs(cdata(:)).^2) - noise_sigma;
        else
            samplesize=[1000 1000]; % Exract 1000x1000 array of samples to estimate mean
            subsample=ceil(double([meta{i}.ImageData.NumCols meta{i}.ImageData.NumRows])./samplesize);
            data = abs(single(ro{i}.read_chip([1 meta{i}.ImageData.NumCols],...
                    [1 meta{i}.ImageData.NumRows],subsample)));
            signal_sigma = mean(abs(data(:)).^2) - noise_sigma;
        end
        if isfield(meta{i}, 'Radiometric') && ...
                isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
            % Scale to be equal to noise equivalent sigma zero
            signal_sigma = meta{i}.Radiometric.SigmaZeroSFPoly * signal_sigma;
        end
        warning('RGIQE:SignalLevelMeasured',['More uncertainty with ' ...
            'RNIIRS estimates exists when signal level is sampled, ' ...
            'rather than when using a radiometric constant.']);
    else
        signal_sigma = NaN;
    end

    %% Save some diagnostic statistics
    if isfield(meta{i}, 'Radiometric') && ...
            isfield(meta{i}.Radiometric, 'SigmaZeroSFPoly')
        diagnostic.nesz(i) = 10*log10(noise_sigma);
    end
    diagnostic.snr(i) = signal_sigma/noise_sigma;
    diagnostic.sp_resolution(i) = sqrt(meta{i}.Grid.Row.ImpRespWid * ...
        meta{i}.Grid.Col.ImpRespWid);
    diagnostic.ellipicity(i) = max(meta{i}.Grid.Row.ImpRespWid, ...
        meta{i}.Grid.Col.ImpRespWid)/min(meta{i}.Grid.Row.ImpRespWid, ...
        meta{i}.Grid.Col.ImpRespWid);
    if numel(meta)>1
        diagnostic.meta{i} = meta{i};
    else
        diagnostic.meta = meta{i};
    end

    %% Compute bandwidth area
    % The cosine of the slope angle is proper scale factor to project the
    % bandwidth area into the ground plane.
    diagnostic.bandwidth_area(i) = ...
        meta{i}.Grid.Col.ImpRespBW * meta{i}.Grid.Row.ImpRespBW * ...
        cosd(meta{i}.SCPCOA.SlopeAng);
    % Computing area from vertices is much more work, but it may help
    % visualize.
    % Project slant plane spatial frequency bounds to ground plane
    % coords_slant_x = meta{i}.Grid.Col.KCtr + ...
    %     meta{i}.Grid.Col.ImpRespBW * [-1 1 1 -1 -1]/2;
    % coords_slant_y = meta{i}.Grid.Row.KCtr + ...
    %     meta{i}.Grid.Row.ImpRespBW * [1 1 -1 -1 1]/2;
    % ruvect = [meta{i}.Grid.Row.UVectECF.X meta{i}.Grid.Row.UVectECF.Y meta{i}.Grid.Row.UVectECF.Z];
    % cuvect = [meta{i}.Grid.Col.UVectECF.X meta{i}.Grid.Col.UVectECF.Y meta{i}.Grid.Col.UVectECF.Z];
    % coords_slant_3d = ruvect'*coords_slant_y + cuvect'*coords_slant_x;
    % Compute basis vectors for ground plane
    % gpn = wgs_84_norm([meta{i}.GeoData.SCP.ECF.X meta{i}.GeoData.SCP.ECF.Y meta{i}.GeoData.SCP.ECF.Z])';
    % gruvect = project_ground(ruvect, gpn); % Project range vector to ground plane
    % gruvect = gruvect/norm(gruvect);
    % gcuvect = cross(gruvect,gpn);
    % gcuvect = gcuvect/norm(gcuvect);
    % Project spatial frequencies to ground plane
    % coords_ground_3d = coords_slant_3d;
    % coords_ground_x = zeros(size(coords_slant_x));
    % coords_ground_y = zeros(size(coords_slant_y));
    % for j=1:numel(coords_slant_x)
    %     % Planes in ECF
    %     coords_ground_3d(:,j) = project_ground(coords_slant_3d(:,j)',gpn)';
    %     % Convert to 2D basis vectors in ground plane
    %     coords_ground_x(j) = dot(coords_ground_3d(:,j),gcuvect);
    %     coords_ground_y(j) = dot(coords_ground_3d(:,j),gruvect);
    % end
    % This is a way to compute the area of a any polygon from its vertices.
    % diagnostic.bandwidth_area = sum((coords_ground_x(1:(end-1)).*coords_ground_y(2:end)) - ...
    %     (coords_ground_y(1:(end-1)).*coords_ground_x(2:end)))/2;
    % Visualize spatial frequency projections in slant and ground planes
    % figure;
    % title('Spatial frequency bounds 3D');
    % g_plane = gruvect'*[100 100 -100 -100 100] + gcuvect'*[-100 100 100 -100 -100];
    % plot3(coords_slant_3d(1,:),coords_slant_3d(2,:),coords_slant_3d(3,:),...
    %     coords_ground_3d(1,:),coords_ground_3d(2,:),coords_ground_3d(3,:),...
    %     g_plane(1,:),g_plane(2,:),g_plane(3,:));
    % legend('Slant plane bounds', 'Ground plane bounds', 'Ground plane')
    % figure;
    % plot(coords_slant_x, coords_slant_y, coords_ground_x, coords_ground_y);
    % title('Spatial frequency bounds');
    % legend('Slant plane bounds', 'Ground plane bounds')

    %% After 300+ line of setup code and comments, we finally compute quality metric...
    % Shannon-Hartley theorem of channel capacity
    % C = B * log2(1 + (S/N))
    % We propose a 2-dimension information rate based on a 2-dimensional
    % bandwidth.  This 2D bandwidth is an area in a space where each axis
    % is cycles/m, as opposed to 1D distance in Hz (cycles per second).
    % Therefore the units of this metric are bits/m^2.
    image_information_metric(i) = diagnostic.bandwidth_area(i) * log2(1 + diagnostic.snr(i));

    %% Map bits/m^2 into RNIIRS
    if ismember('signal_sigma_override', p.UsingDefaults)
        rniirs(i) = estimate_rniirs(image_information_metric(i));
    else  % If assumed signal is not default, this RNIIRS mapping is not valid
        rniirs(i) = NaN;
    end
end
try
    for i = 1:numel(meta)
        ro{i}.close();
    end
end

end

function rniirs = estimate_rniirs(image_information_metric)
    % The coefficients here were derived by fitting hundreds of SAR
    % datasets with varying parameters to analysts ratings.  This
    % author prefers the information metric but also provides this as
    % well for comparison to this legacy scale.
    % coeffs = [0.3960 3.7555];  % Probably more digits of precision than we really have
    coeffs = [0.4357 3.4761];  % Coefficients were updated by NGA NIQU (Feb 1, 2022) to more accruately represent RNIIRS tables
    rniirs = polyval(coeffs,log2(image_information_metric));
    % For completeness, we handle the very low RNIIRS case where the
    % logarithmic equation would have been negative.  Negative is not
    % allowed in the RNIIRS scale. We compute where a line tangent to
    % the bits/m^2-to-RNIIRS function intersects with the origin.  We
    % will use this tangent line as the mapping for very low RNIIRS,
    % since it approaches zero as information approaches zero and
    % remains non-negative. This way the entire mapping function
    % between bits/m^2 and RNIIRS is continuous and has a continuous
    % derivative.  The function will be logarithmic with respect to the
    % information metric above this point (presumably nearly all data
    % will fit in here) and linear with respect to the metric below
    % this point (extremely low RNIIRS).  Note that since NGA has no
    % precedent or analysts ratings for RNIIRS<1, we are really free to
    % define this however we want.
    linlog_transition = exp(1 - coeffs(2)*log(2)/coeffs(1));
    % Derivative of linear portion (as well as intersection with log portion)
    d_rniirs = coeffs(1)/(log(2)*linlog_transition);
    % This is what the two lines would look line at very low RNIIRS:
    % x=linspace(0,.02,1000);
    % figure; plot(x, polyval(coeffs,log2(x)), x, x*d_rniirs);
    % That was a lot of explanation for a case that will likely rarely
    % be used, wasn't it?
    if image_information_metric<linlog_transition
        rniirs = d_rniirs * image_information_metric;
    end
end

function out = project_ground(point, gpn)
    % Projection of a point along a given direction to a plane is
    % just the intersection of the line defined by that point (l0)
    % and direction (l) and the plane defined by a point in the
    % plane (p0) and the normal (p):
    % l0 - ((l0 - p0).p/(l.p))*l
    % where . represents the dot product.
    % In this specific case, the origin is in the plane, and the projection
    % direction is the same as the plane normal, so this equation is much
    % simplified:
    out = point - dot(point,gpn)*gpn;
end


function  cplexProblem = buildCplexProblemFromCOBRAStruct(Problem)
% Build a cplex object from the given LP problem in COBRA Format
% USAGE:
%    cplexProblem = buildCplexProblemFromCOBRAStruct(LPproblem)
%
% INPUT:
%    LPproblem:         A COBRA style Problem with the following fields:
%                        * .A - The equality and in equality matrix
%                        * .b - The right hand side values of the constraints
%                        * .ub - the upper bounds of the variables
%                        * .lb - the lower bounds of the variables
%                        * .osense - The objective sense (-1 for max, 1 for min)
%                        * .c - the objective coefficient vector for the linear part
%                       OPTIONAL:
%                        * .F - The objective coefficient matrix for the quadratic part.
%                        * .varType - The variable types for mixed integer problems ('I', integer, 'C', continous,'B' binary)
%                        * .b_L - left hand sides of the constraints (will only be used if csense is empty)
%                        * .csense - The constraint senses, Default assumption is all 'E'
%                        * .x0 - Basis to use
%

try
    cplexProblem = Cplex();
    %cplexProblem = Cplex('COBRAProblem');
catch ME
    error('CPLEX not installed or licence server not up')
end
if (~isempty(Problem.csense))
    % build the rhs/lhs of the problem.
    boolE = Problem.csense == 'E';
    b_L(boolE) = Problem.b(boolE);
    b_U(boolE) = Problem.b(boolE);
    boolG = Problem.csense == 'G';
    b_L(boolG) = Problem.b(boolG);
    b_U(boolG) = inf;
    boolL = Problem.csense == 'L';
    b_L(boolL) = -inf;
    b_U(boolL) = Problem.b(boolL);
elseif isfield(Problem.csense, 'b_L') && isfield(Problem, 'b_U')
    % or extract them
    b_L = Problem.b_L;
    b_U = Problem.b_U;
else
    % or simply use the equality assumption.
    b_L = Problem.b;
    b_U = Problem.b;
end

cplexProblem.Model.A = Problem.A;
cplexProblem.Model.rhs = columnVector(b_U);
cplexProblem.Model.lhs = columnVector(b_L);
cplexProblem.Model.ub = Problem.ub;
cplexProblem.Model.lb = Problem.lb;

if isfield(Problem,'F')
    f = diag(Problem.F);
    bool0 = f==0;
    if any(bool0)
        Fbar = Problem.F - diag(f);
        if ~any(Fbar,'all')
            feasTol = getCobraSolverParams('LP', 'feasTol');
            f(bool0)=feasTol/10;
            f(~bool0)=0;
            Problem.F = Problem.F + spdiags(f,0,size(Problem.F,1),size(Problem.F,1));
            %fprintf('%s\n',['buildCplexProblemFromCOBRAStruct: Replacing zeros on the diagonal of QP problem.F with regularisation of ' num2str(feasTol/10)])
        end
    end
    cplexProblem.Model.Q = Problem.F;
end

if isfield(Problem,'c')
    cplexProblem.Model.obj = Problem.c;
end
if isfield(Problem,'osense')
    if Problem.osense == 1
        cplexProblem.Model.sense = 'minimize';
    else
        cplexProblem.Model.sense = 'maximize';
    end
else
    cplexProblem.Model.sense = 'minimize';
end

if isfield(Problem,'vartype')
    cplexProblem.Model.ctype = columnVector(Problem.vartype)';
end

if isfield(Problem,'x0')
    cplexProblem.Start.x = Problem.x0;
end


function varargout=bss_decomp_tvfilt(varargin)

% decompose an estimated source into target/interference/noise/artefacts components, assuming the admissible distortion is a time-varying filter.
%
% Usage:
%
% [s_target,e_interf[,e_noise],e_artif]=bss_decomp_tvfilt(se,index,S[,N],tvshape,tvstep,L)
%
% Input:
%   - se: row vector of length T containing the estimated source,
%   - index: points which component of S se has to be compared to,
%   - S: n x T matrix containing the original sources,
%   - N: m x T matrix containing the noise on the obseravtions (if any).
%   - tvshape : row vector of length V at most T containing the shape of the elementary
%     allowed time variations of the filter coefficients
%   - tvstep  : hop size (in number of samples) between two consecutive
%     variations of the filter coefficients
%   - L: the number of lags
%
% Output:
%   - s_target: row vector of length T containing the target source(s)
%   contribution,
%   - e_interf: row vector of length T containing the interferences
%   contribution,
%   - e_noise: row vector of length T containing the noise contribution (if
%   any),
%   - e_artif: row vector of length T containing the artifacts
%   contribution.
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

se=varargin{1}; index=varargin{2}; S=varargin{3};

switch nargin
    case 6
        N=[]; tvshape = varargin{4}; tvstep = varargin{5}; L = varargin{6};
    case 7
        N=varargin{4}; tvshape = varargin{5}; tvstep = varargin{6}; L = varargin{7};
    otherwise
        disp('Wrong number of arguments.')
end

[ne,Te]=size(se);
[n,T]=size(S);

%%%%%%%%%% WARNINGS %%%%%%%%%%%%%
switch isempty(N)
    case 1
        if n>T || ne>Te, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: se and S have different lengths.'), return; end
    case 0
        [m,Tm]=size(N);
        if n>T || ne>Te || m>Tm, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: S and Se have different lengths.'), return; end
        if T~=Tm, disp('Watch out: N, S and Se have different lengths.'), return; end
end

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

% Create the space of target source(s)
target_space = bss_make_lags(S(index,:),L);
% Create the space of sources
sources_space= bss_make_lags(S,L);
% Create the noise space
noise_space  = bss_make_lags(N,L);

%%% Target source(s) contribution %%%
s_target = bss_tvproj(se,target_space,tvshape,tvstep);

%%% Interferences contribution %%%
P_S_se = bss_tvproj(se,sources_space,tvshape,tvstep);
e_interf = P_S_se - s_target;

if isempty(noise_space) % No noise
    %%% Artifacts contribution %%%
    e_artif= se - P_S_se;

    %%% Output %%%
    varargout{1}=s_target;
    varargout{2}=e_interf;
    varargout{3}=e_artif;

else % Noise
    %%% Noise contribution %%%
    P_SN_se= bss_tvproj(se,[sources_space;noise_space],tvshape,tvstep);
    e_noise=P_SN_se-P_S_se;

    %%% Artifacts contribution %%%
    e_artif=se-P_SN_se;

    %%% Output %%%
    varargout{1}=s_target;
    varargout{2}=e_interf;
    varargout{3}=e_noise;
    varargout{4}=e_artif;
end


% UPDATE_POSTERIOR updates GPML posterior given a new observation.
%
% This function provides a fast update for a GPML posterior structure
% given a single new observation, avoiding O(N^3) retraining time.
% This implementation assumes exact inference (inf = @infExact) and a
% Gaussian likelihood (lik = @likGauss). It will not work correctly
% for other combinations of likelihood and inference!
%
% Usage:
%
%   new_posterior = update_posterior(hyperparameters, mean_function, ...
%           covariance_function, x, posterior, x_star, y_star)
%
% Inputs:
%
%       hyperparameters: a GPML hyperparameter struct
%         mean_function: a GPML mean function
%   covariance_function: a GPML covariance function
%                     x: the previous observations (N x D)
%             posterior: a GPML posterior struct
%                x_star: the new observation location (1 x D)
%                y_star: the new observation value
%
% Output:
%
%   new_posterior: the updated GPML posterior struct for the GP
%                  conditioned on ([x; x_star], [y; y_star]).
%
% See also INFMETHODS.

% Copyright (c) 2013--2014 Roman Garnett.

function new_posterior = update_posterior(hyperparameters, mean_function, ...
          covariance_function, x, posterior, x_star, y_star, y_sd)

  % check input
  if (numel(y_star) > 1)
    error('gpml_extensions:not_supported', ...
          'update_posterior only supports rank-one updates!');
  end

  % Need to decide how to deal with noise
  if nargin < 8; y_sd = []; end

  noise_variance = exp(2 * hyperparameters.lik);

  k      = feval(covariance_function{:}, hyperparameters.cov, x_star);
  k_star = feval(covariance_function{:}, hyperparameters.cov, x, x_star);

  [m_star, v_star] = gp(hyperparameters, [], mean_function, ...
                        covariance_function, [], x, posterior, x_star);

  % update posterior.L; handle both high- and low-noise
  % parameterizations of posterior
  if (is_chol(posterior.L))
    % high-noise parameterization: posterior.L contains chol(K / sigma^2 + I)

    alpha_update = solve_chol(posterior.L, k_star) * (1 / noise_variance);

    new_L_column = linsolve(posterior.L, k_star, ...
                            struct('UT', true, 'TRANSA', true)) * (1 / noise_variance);
    new_posterior.L = [posterior.L, new_L_column; ...
                       zeros(1, size(posterior.L, 1)), ...
                       sqrt(1 + k / noise_variance - new_L_column' * new_L_column)];
  else
    % low-noise parameterization: posterior.L contains -inv(K + \sigma^2 I)

    alpha_update = -posterior.L * k_star;

    v = -alpha_update / v_star;
    new_posterior.L = [posterior.L + v * alpha_update', -v; -v', -1 / v_star];
  end

  % alpha_update now contains (K + \sigma^2 I) \ k*
  new_posterior.alpha = ...
      [posterior.alpha; 0] + ...
      (m_star - y_star) / v_star * [alpha_update; -1];

  % noise vector is constant; just add one more entry
  new_posterior.sW = posterior.sW([1; (1:end)']);

end

%--------------------------------------------------------------------------
function result = is_chol(L)

  result = (ismatrix(L) && ...
            (size(L, 1) == size(L, 2)) && ...
            isreal(diag(L))  && ...
            all(diag(L) > 0) && ...
            isequal(L, triu(L)));

end


function    g = lfmGradientUpsilon(gamma,sigma2,gradThetaGamma,t1, t2, mode)

% LFMGRADIENTUPSILON Gradient of the function \upsilon(z) with respect to
% one of the hyperparameters of the kernel: m_k, C_k, D_k, m_r, C_r or D_r.
% FORMAT
% DESC Computes the gradient of the function \upsilon(z) with respect to
% one of the parameters of the system (mass, spring or damper).
% ARG gamma : Gamma value of the system.
% ARG sigma2 : length scale of latent process.
% ARG gradThetaGamma : Vector with the gradient of gamma1 and gamma2 with
% respect to the desired parameter.
% ARG Tt1 : first time input (number of time points 1 x number of time points 2).
% ARG Tt2 : second time input (number of time points 1 x number of time points 2).
% ARG mode : indicates in which way the variables t1 and t2 must be
% transposed
% RETURN g : Gradient of the kernel with respect to the desired
% parameter.
%
% COPYRIGHT : David Luengo, Mauricio Alvarez 2008
%
% SEEALSO : lfmKernGradient, lfmXlfmKernGradient, lfmGradientH

% KERN

%Parameters of the function

sigma = sqrt(sigma2);

% Initialization of vectors and matrices

switch mode
    case 1
        % In this mode the Tt1 is actually the Tt1 and Tt2 is actually Tt2
        % Z1 is a matrix with order given by Tt1 and z2 is a vector with
        % order given by Tt2
        Tt11 = repmat(t1, 1, size(t2, 1));
        Tt22 = repmat(t2', size(t1, 1), 1);
        Z1 = (Tt11-Tt22)/sigma - sigma*gamma/2;
        z2 = t2/sigma + sigma*gamma/2;
        Z2 = repmat(z2.', size(t1, 1), 1);
        [preFactorZ1, gradPreFactorZ1] = quadrantWofzGrad(Z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        Tt1minusTt2 = Tt11 - Tt22;
        preFactor1 = exp(sigma2*(gamma^2)/4 - gamma*(Tt1minusTt2)...
            + log(sigma2*gamma - 2*(Tt1minusTt2)) + log(gradThetaGamma));
        preFactor2 = exp(-(Tt1minusTt2).*(Tt1minusTt2)/sigma2 ...
            + log(gradThetaGamma) + log(gradPreFactorZ1));
        preFactor3 = exp(repmat((-t2.*t2/sigma2).',size(t1, 1), 1) ...
            - gamma*Tt11 + log(gradThetaGamma)...
            + log(Tt11.*repmat(preFactorz2.',size(t1, 1), 1) ...
            + repmat((gradPreFactorz2).',size(t1, 1), 1)));
    case 2

        % In this mode the Tt1 is actually the Tt2 and Tt2 is actually Tt1
        % Z1 is a matrix with the order given by Tt2 and z2 is a vector
        % with the order given by Tt1

        Tt11 = repmat(t2', size(t1, 1), 1);
        Tt22 = repmat(t1, 1, size(t2, 1));
        Z1 = (Tt11-Tt22)/sigma - sigma*gamma/2;
        z2 = t1/sigma + sigma*gamma/2;
        Z2 = repmat(z2,  1, size(t2, 1));
        [preFactorZ1, gradPreFactorZ1] = quadrantWofzGrad(Z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        Tt1minusTt2 = Tt11 - Tt22;
        preFactor1 = exp(sigma2*(gamma^2)/4 - gamma*(Tt1minusTt2)...
            + log(sigma2*gamma - 2*(Tt1minusTt2)) + log(gradThetaGamma));
        preFactor2 = exp(-(Tt1minusTt2).*(Tt1minusTt2)/sigma2 ...
            + log(gradThetaGamma) + log(gradPreFactorZ1));
        preFactor3 = exp( repmat(-t1.*t1/sigma2,1, size(t2, 1))...
            - gamma*Tt11 + log(gradThetaGamma) ...
            + log(Tt11.*repmat(preFactorz2,1, size(t2, 1)) + repmat(gradPreFactorz2,1, size(t2, 1))));

    case {3,4}

        % In this mode Tt1 is actually Tt1 and Tt2 is zero.
        % Z1 is a vector and Z2 is a scalar
        z1 = t1/sigma - sigma*gamma/2;
        z2 = sigma*gamma/2;
        Z1 = repmat(z1, 1, size(t2,1));
        Z2 = repmat(z2, size(t1, 1), size(t2,1));
        [preFactorz1, gradPreFactorz1] = quadrantWofzGrad(z1, sigma,1);
        [preFactorz2, gradPreFactorz2] = quadrantWofzGrad(z2, sigma,1);
        preFactor1 = repmat(exp(sigma2*(gamma^2)/4 - gamma*(t1) ...
            + log(sigma2*gamma - 2*t1) + log(gradThetaGamma)), 1, size(t2, 1));
        preFactor2 = repmat(exp(-t1.*t1/sigma2 + log(gradThetaGamma)...
            + log(gradPreFactorz1)), 1, size(t2, 1));
        preFactor3 = repmat(exp(- gamma*t1 + log(gradThetaGamma) ...
            + log(t1*preFactorz2 + gradPreFactorz2)),1, size(t2, 1));

end

g = zeros(size(preFactor1));
% Evaluation of Upsilon when real(Z1)>=0 and real(Z2)>=0
ind = (real(Z1)>=0) & (real(Z2)>=0);
if any(any(ind))
    g(ind) = preFactor1(ind) - preFactor2(ind) + preFactor3(ind);
end;
% Evaluation of Upsilon when real(Z1)<0 and real(Z2)>=0
ind = (real(Z1)<0) & (real(Z2)>=0);
if any(any(ind))
    g(ind) = preFactor2(ind) + preFactor3(ind);
end
% Evaluation of Upsilon when real(Z1)>=0 and real(Z2)<0
ind = (real(Z1)>=0) & (real(Z2)<0);
if any(any(ind))
    g(ind) = - preFactor2(ind) - preFactor3(ind);
end;
% Evaluation of Upsilon when real(Z1)<0 and real(Z2)<0
ind = (real(Z1)<0) & (real(Z2)<0);
if any(any(ind))
    g(ind) = -preFactor1(ind) + preFactor2(ind) - preFactor3(ind);
end;

if mode==4
    g = g.';
end


function X=diag(X,k)
%DIAG (overloaded)

n = X.dim(1);
m = X.dim(2);

if nargin == 1
    k = 0;
end

if min([n m])==1
    Y = X;
	diagX = diag(X.basis(:,1),k);
	Y.basis = diagX(:);
	for i = 1:length(X.lmi_variables)
		diagX = diag(X.basis(:,i+1),k);
		Y.basis(:,i+1) = diagX(:);
    end
	Y.lmi_variables = X.lmi_variables;
	Y.dim(1) = size(diagX,1);
	Y.dim(2) = size(diagX,2);
    X=Y;
else
    index = diag(reshape(1:n*m,n,m),k);
    X.basis = X.basis(index,:);
	X.dim(1) = length(index);
	X.dim(2) = 1;
    X = clean(X);
end
% Reset info about conic terms
if isa(X,'sdpvar')
    X.conicinfo = [0 0];
end


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

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

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

%-- Skew coefficient:
alpha_c = 0.000000000000000;

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

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

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

%-- Skew coefficient uncertainty:
alpha_c_error = 0.000000000000000;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


function [M, TF] = padcat(varargin)
% PADCAT - concatenate vectors with different lengths by padding with NaN
%
%   M = PADCAT(V1, V2, V3, ..., VN) concatenates the vectors V1 through VN
%   into one large matrix. All vectors should have the same orientation,
%   that is, they are all row or column vectors. The vectors do not need to
%   have the same lengths, and shorter vectors are padded with NaNs.
%   The size of M is determined by the length of the longest vector. For
%   row vectors, M will be a N-by-MaxL matrix and for column vectors, M
%   will be a MaxL-by-N matrix, where MaxL is the length of the longest
%   vector.
%
%   Examples:
%      a = 1:5 ; b = 1:3 ; c = [] ; d = 1:4 ;
%      padcat(a,b,c,d) % row vectors
%         % ->   1     2     3     4     5
%         %      1     2     3   NaN   NaN
%         %    NaN   NaN   NaN   NaN   NaN
%         %      1     2     3     4   NaN
%      CC = {d.' a.' c.' b.' d.'} ;
%      padcat(CC{:}) % column vectors
%         %      1     1   NaN     1     1
%         %      2     2   NaN     2     2
%         %      3     3   NaN     3     3
%         %      4     4   NaN   NaN     4
%         %    NaN     5   NaN   NaN   NaN
%
%   [M, TF] = PADCAT(..) will also return a logical matrix TF with the same
%   size as R having true values for those positions that originate from an
%   input vector. This may be useful if any of the vectors contain NaNs.
%
%   Example:
%       a = 1:3 ; b = [] ; c = [1 NaN] ;
%       [M,tf] = padcat(a,b,c)
%       % find the original NaN
%       [Vev,Pos] = find(tf & isnan(M))
%       % -> Vec = 3 , Pos = 2
%
%   This second output can also be used to change the padding value into
%   something else than NaN.
%
%       [M, tf] = padcat(1:3,1,1:4)
%       M(~tf) = 99 % change the padding value into 99
%
%   Scalars will be concatenated into a single column vector.
%
%   See also CAT, RESHAPE, STRVCAT, CHAR, HORZCAT, VERTCAT, ISEMPTY
%            NONES, GROUP2CELL (Matlab File Exchange)

% for Matlab 2008 and up (tested in R2011a)
% version 1.2 (oct 2011)
% (c) Jos van der Geest
% email: jos@jasen.nl

% History
% 1.0 (feb 2009) created
% 1.1 (feb 2011) improved comments
% 1.2 (oct 2011) added help on changing the padding value into something
%     else than NaN

% Acknowledgements:
% Inspired by padadd.m (feb 2000) Fex ID 209 by Dave Johnson

error(nargchk(1,Inf,nargin)) ;

% check the inputs
SZ = cellfun(@size,varargin,'UniformOutput',false) ; % sizes
Ndim = cellfun(@ndims,varargin) ; %

if ~all(Ndim==2)
    error([mfilename ':WrongInputDimension'], ...
        'Input should be vectors.') ;
end

TF = [] ; % default second output so we do not have to check all the time

% for 2D matrices (including vectors) the size is a 1-by-2 vector
SZ = cat(1,SZ{:}) ;
maxSZ = max(SZ) ;    % probable size of the longest vector
% maxSZ equals :
%  - [1 1] for all scalars input
%  - [X 1] for column vectors
%  - [1 X] for all row vectors
%  - [X Y] otherwise (so padcat will not work!)

if ~any(maxSZ == 1),  % hmm, not all elements are 1-by-N or N-by-1
    % 2 options ...
    if any(maxSZ==0),
        % 1) all inputs are empty
        M  = [] ;
        return
    else
        % 2) wrong input
        % Either not all vectors have the same orientation (row and column
        % vectors are being mixed) or an input is a matrix.
        error([mfilename ':WrongInputSize'], ...
            'Inputs should be all row vectors or all column vectors.') ;
    end
end

if nargin == 1,
    % single input, nothing to concatenate ..
    M = varargin{1} ;
else
    % Concatenate row vectors in a row, and column vectors in a column.
    dim = (maxSZ(1)==1) + 1 ;      % Find out the dimension to work on
    X = cat(dim, varargin{:}) ;    % make one big list

    % we will use linear indexing, which operates along columns. We apply a
    % transpose at the end if the input were row vectors.

    if maxSZ(dim) == 1,
        % if all inputs are scalars, ...
        M = X ;   % copy the list
    elseif all(SZ(:,dim)==SZ(1,dim)),
        % all vectors have the same length
        M = reshape(X,SZ(1,dim),[]) ;% copy the list and reshape
    else
        % We do have vectors of different lengths.
        % Pre-allocate the final output array as a column oriented array. We
        % make it one larger to accommodate the largest vector as well.
        M = zeros([maxSZ(dim)+1 nargin]) ;
        % where do the fillers begin in each column
        M(sub2ind(size(M), SZ(:,dim).'+1, 1:nargin)) = 1 ;
        % Fillers should be put in after that position as well, so applying
        % cumsum on the columns
        % Note that we remove the last row; the largest vector will fill an
        % entire column.
        M = cumsum(M(1:end-1,:),1) ; % remove last row

        % If we need to return position of the non-fillers we will get them
        % now. We cannot do it afterwards, since NaNs may be present in the
        % inputs.
        if nargout>1,
            TF = ~M ;
            % and make use of this logical array
            M(~TF) = NaN ; % put the fillers in
            M(TF)  = X ;   % put the values in
        else
            M(M==1) = NaN ; % put the fillers in
            M(M==0) = X ;   % put the values in
        end
    end

    if dim == 2,
        % the inputs were row vectors, so transpose
        M = M.' ;
        TF = TF.' ; % was initialized as empty if not requested
    end
end % nargin == 1

if nargout > 1 && isempty(TF),
    % in this case, the inputs were all empty, all scalars, or all had the
    % same size.
    TF = true(size(M)) ;
end


function [cc] = in32cc(in3)
% Convert volume from cubic inches to cubic centimeters*.
% * Not to be confused with cubic cubits.
% Chad Greene 2012
cc = in3*16.387064;


function w = ymdf_to_weekday_republican ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_REPUBLICAN returns the weekday of a Republican YMDF date.
%
%  Discussion:
%
%    The Republican calendar used a 10 day week.
%    There was a final "month" of 5 or 6 days.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, the day of the week of the date.
%    The days are numbered from Sunday through Saturday, 1 through 7.
%
  w = i4_wrap ( d, 1, 10 );

  return
end


function varargout = spm_resels_vol(varargin)
% computes the number of resels in a volume - a compiled routine
% FORMAT R = spm_resels_vol(V,W)
% V      -  is a memory mapped image volume.
%           Finite and non-zero values are considered to be part of
%           the search volume.
% W      -  smoothness of the component fields {FWHM in voxels}.
% R      - Resel counts, where:
%          R(1) - Euler Characteristic of the volume (number of connected
%                 components - number of holes).
%          R(2) - Resel Diameter (average over all rotations of the
%                 distance between two parallel planes tangent to the
%                 volume in resel space).
%          R(3) - Resel Surface Area (half the surface area of the
%                 volume in resel space).
%          R(4) - Resel Volume (the volume in resel space).
%_______________________________________________________________________
%
% Reference : Worsley KJ et al 1996, Hum Brain Mapp. 4:58-73
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_resels_vol.m 1143 2008-02-07 19:33:33Z spm $

%-This is merely the help file for the compiled routine
error('spm_resels_vol.c not compiled - see Makefile')


%fsqfindewq(lambda,mu,c,m)
%   This function finds the average waiting time in the queue
%   (ewq) for a machine repair problem (finite source queue)

function out = fsqfindewq(lambda,mu,c,m)

ld = fsqfindld(lambda,mu,c,m);
lu = fsqfindlu(lambda,mu,c,m);

ewq = ld/(lambda*lu);

out = ewq;


function [flips,scorepath,covmats_unflipped] = findflip(data,T,options)
% Finds an optimal permutation of the channels, where goodness is measured
% as the mean lagged partial  cross-correlation across pair of channels and lags.
% In other words, it finds a permutation where the signs of the lagged
% partial correlations agree as much as possible across subjects.
%
% INPUTS
% data          observations, either a struct with X (time series)
%                             or just a matrix containing the time series
%               Alternatively, it can be an (unflipped) array of
%                   autocorrelation matrices (ndim x ndim x no.lags x no. trials),
%                   as computed for example by getCovMats()
% T             length of series. If data is supplied as the
%               autocorrelation matrices, then this is not required and
%               can be specified as []
% options:
%  maxlag        max lag to consider
%  nbatch        no. of channels to evaluate at each iteration (0 for all)
%  noruns        how many random initialisations will be carried out
%  standardise   if 1, standardise the data
%  partial       if 1, base on partial correlation instead of correlation
%  maxcyc        for each initialization, maximum number of cycles of the greedy algorithm
%  verbose       do we get loud?
%
% data_ref and T_ref refer to having a previous data set to
% with which we want to be consistent (see above). These are assumed to be
% already sign-disambiguated

% OUTPUT
% flips         (length(T) X No. channels) binary matrix saying which channels must be
%               flipped for each time series
% scorepath     cell with the score of the winning solutions
% covmats_unflipped  the disambiguated covariance matrices
%
% Author: Diego Vidaurre, University of Oxford.

if nargin < 3, options = struct; end

options = checkoptions_flip(options);
covmats_unflipped = Get_Global_Variables_For_BitFlip_Evaluation(data,T,options);
N = size(covmats_unflipped,4);

ndim = size(covmats_unflipped,1);
score = -Inf;
scorepath = cell(options.noruns,1);

if isinf(options.maxcyc) && (options.nbatch > 0)
    error('If maxcyc is Inf, options.nbatch must be 0')
end

% repetitions of the search
for r = 1:options.noruns

    flipsr = Init_Solution(N,ndim,options,r);
    score_r = EvaluateFlips(flipsr,covmats_unflipped);
    scorepath{r} = score_r;
    if options.verbose
        fprintf('Run %d, Init, score %f \n',r,score_r)
    end

    for cyc = 1:options.maxcyc

        if options.nbatch > 0
            channels = randperm(ndim,options.nbatch);
        else
            channels = randperm(ndim);
        end

        ScoreMatrix = zeros(N,ndim);
        for d = channels
            for j = 1:N
                ScoreMatrix(j,d) = EvaluateFlips(flipsr,covmats_unflipped,j,d);
            end
        end

        [score_r,I] = max(ScoreMatrix(:));
        [j,d] = ind2sub([N ndim],I);

        ds = score_r - max(scorepath{r});
        if ds > options.threshold
            flipsr(j,d) = ~flipsr(j,d);
            if options.verbose
                fprintf('Run %d, Cycle %d, score +%f, flipped (%d,%d) \n',r,cyc,ds,j,d)
            end
            scorepath{r} = [scorepath{r} score_r];
        elseif options.nbatch == 0
            break
        else
            fprintf('Run %d, Cycle %d, score +0 \n',r,cyc)
        end

    end

    if options.verbose
        fprintf('Run %d, Finish, score %f \n',r,scorepath{r}(end))
    end

    if scorepath{r}(end) > score
        score = scorepath{r}(end);
        flips = flipsr;
    end

end

if options.verbose
    fprintf('Final Score=%f\n',score)
end

% Among the equivalent flippings, we keep the one w/ the lowest no. of flips
for j = 1:N
    if mean(flips(j,:))>0.5
        flips(j,:) = 1 - flips(j,:);
    end
end

end

function options = checkoptions_flip(options)
if ~isfield(options,'maxlag'), options.maxlag = 10; end
if ~isfield(options,'noruns'), options.noruns = 1; end
if ~isfield(options,'probinitflip'), options.probinitflip = 0.25; end
if ~isfield(options,'standardise'), options.standardise = 1; end
if ~isfield(options,'partial'), options.partial = 0; end
if ~isfield(options,'verbose'), options.verbose = 1; end
if ~isfield(options,'nbatch'), options.nbatch = 0; end
if ~isfield(options,'maxcyc'), options.maxcyc = 10000; end
if ~isfield(options,'threshold'), options.threshold = 0.00001; end
end

function flipsr = Init_Solution(N,ndim,options,r)
% Prepare the necessary data structures
if ~isfield(options,'Flips')
    if r==1 % first run starts at no flips
        flipsr = zeros(N,ndim);
    else
        flipsr = binornd(1,options.probinitflip,N,ndim); % random init
    end
else
    flipsr = options.Flips;
end
end

function covmats_unflipped = Get_Global_Variables_For_BitFlip_Evaluation(data,T,options)
% Prepare the necessary data structures
if length(size(data))==4 % it is an array of autocorrelation matrices already
    covmats_unflipped = data; clear data
else
    covmats_unflipped = getAllCovMats(data,T,options);
end
end

function s = EvaluateFlips(flipsr,covmats_unflipped,j,d)
% evaluate for a change in subject j and channel d
if nargin>3, flipsr(j,d) = ~flipsr(j,d);  end
signmats = getSignMat(flipsr);
covmats = applySign(covmats_unflipped,signmats);
s = getscore(covmats);
end

function score = getscore(M)
% get the score from the (flipped) matrices of autocorrelation contained in M
% M is a matrix (ndim x ndim x lags x subjects) with autocovariances matrix for all subjects
N = size(M,4); ndim = size(M,1); L = size(M,3);
M = reshape(M,[(ndim^2)*L N]);
C = corr(M);
score = mean(C(triu(true(N),1)));
end

function CovMats = applySign(CovMats,SignMats)
% apply matrices of sign flipping to the autocorrelation matrices
N = size(SignMats,3); nlags = size(CovMats,3);
for j = 1:N
    CovMats(:,:,:,j) = CovMats(:,:,:,j) .* repmat(SignMats(:,:,j),[1 1 nlags]);
end
end

function SignMats = getSignMat(Flips)
% Construct matrices of sign flipping for the autocorrelation matrices
[N,ndim] = size(Flips);
SignMats = zeros(ndim,ndim,N);
for j = 1:N
    flips = ones(1,ndim);
    flips(Flips(j,:)==1) = -1;
    SignMats(:,:,j) = flips' * flips;
end
end


function f = not(f)
%~   CLASSICFUN logical NOT.
%   NOT(F) returns a CLASSICFUN which evaluates to one at all points where F is
%   zero and one otherwise.  F cannot have any roots in its domain.  If F
%   does have roots, then LOGICAL(F) will return garbage with no warning. F may
%   be complex.

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

f.onefun = not(f.onefun);

end


function varargout=phaseplot(f,varargin)
%PHASEPLOT  Phase plot
%   Usage: phaseplot(f,op1,op2, ... );
%          phaseplot(f,fs,op1,op2, ... );
%
%   `phaseplot(f)` plots the phase of f using a |dgt|.
%
%   `phaseplot(f,fs)` does the same for a signal with sampling rate *fs* Hz.
%
%   `phaseplot` should only be used for short signals (shorter than the
%   resolution of the screen), as there will otherwise be some visual
%   aliasing, such that very fast changing areas will look very smooth.
%   `phaseplot` always calculates the phase of the full time/frequency plane
%   (as opposed to |sgram|), and you therefore risk running out of memory
%   for long signals.
%
%   `phaseplot` takes the following flags at the end of the line of input
%   arguments:
%
%     'tfr',v     Set the ratio of frequency resolution to time resolution.
%                 A value $v=1$ is the default. Setting $v>1$ will give better
%                 frequency resolution at the expense of a worse time
%                 resolution. A value of $0<v<1$ will do the opposite.
%
%     'wlen',s    Window length. Specifies the length of the window
%                 measured in samples. See help of |pgauss| on the exact
%                 details of the window length.
%
%     'nf'        Display negative frequencies, with the zero-frequency
%                 centered in the middle. For real signals, this will just
%                 mirror the upper half plane. This is standard for complex
%                 signals.
%
%     'tc'        Time centering. Move the beginning of the signal to the
%                 middle of the plot. This is usefull for visualizing the
%                 window functions of the toolbox.
%
%     'thr',r     Keep the coefficients with a magnitude larger than r times the
%                 largest magnitude. Set the phase of the rest of the
%                 coefficients to zero. This is useful, because for small
%                 amplitude the phase values can be meaningless.
%
%     'timeinv'   Display the phase as computed by a time-invariant
%                 |dgt|. This is the default.
%
%     'freqinv'   Display the phase as computed by a frequency-invariant
%                 |dgt|.
%
%     'fmax',y    Display y as the highest frequency.
%
%     'colorbar'   Display the colorbar. This is the default.
%
%     'nocolorbar'  Do not display the colorbar.
%
%   For the best result when using `phaseplot`, use a circulant color
%   map, for instance `hsv`.
%
%   Examples:
%   ---------
%
%   The following code shows the phaseplot of a
%   periodic, hyperbolic secant visualized using the `hsv` colormap:::
%
%     phaseplot(psech(200),'tc','nf');
%     colormap(hsv);
%
%   The following phaseplot shows the phase of white, Gaussian noise:::
%
%     phaseplot(randn(200,1));
%     colormap(hsv);
%
%   See also: phaselock
%
%   Demos: demo_phaseplot
%
%   References: Carmona98practical

%   AUTHOR: Peter L. Søndergaard
%   TESTING: NA

if nargin<1
  error('Too few input arguments.');
end;

if sum(size(f)>1)>1
  error('Input must be a vector.');
end;

definput.import={'ltfattranslate','setnorm','tfplot'};
% Override the setting from tfplot, because phaseplot only uses the 'lin'
% plotting.
definput.flags.log={'lin'};
definput.importdefaults={'lin'};

% Define initial value for flags and key/value pairs.
definput.flags.wlen={'nowlen','wlen'};
definput.flags.tc={'notc','tc'};

definput.flags.fmax={'nofmax','fmax'};
definput.flags.phase={'timeinv','freqinv'};

if isreal(f)
  definput.flags.posfreq={'posfreq','nf'};
else
  definput.flags.posfreq={'nf','posfreq'};
end;

definput.keyvals.tfr=1;
definput.keyvals.wlen=0;
definput.keyvals.fmax=[];
definput.keyvals.thr=[];

[flags,kv,fs]=ltfatarghelper({'fs'},definput,varargin);

% Downsample
if ~isempty(kv.fmax)
  if ~isempty(fs)
    resamp=kv.fmax*2/fs;
  else
    resamp=kv.fmax*2/length(f);
  end;

  f=fftresample(f,round(length(f)*resamp));
  kv.fs=2*kv.fmax;
end;

% Always do the full STFT
L=length(f);
a=1;
b=1;
M=L;
N=L;

% Set an explicit window length, if this was specified.
if flags.do_wlen
  kv.tfr=kv.wlen^2/L;
end;

g={'gauss',kv.tfr,flags.norm};

if flags.do_nf
  coef=dgt(f,g,a,M,flags.phase);
else
  coef=dgtreal(f,g,a,M,flags.phase);
end;

if ~isempty(kv.thr)
  % keep only the largest coefficients.
  maxc=max(abs(coef(:)));
  mask=abs(coef)<maxc*kv.thr;
  coef(mask)=0;
end

coef = angle(coef);

if flags.do_nf
  plotdgt(coef,a,'argimport',flags,kv);
else
  plotdgtreal(coef,a,M,'argimport',flags,kv);
end;

if nargout>0
  varargout={coef};
end;


function [amu] = mg2amu(mg)
% Convert mass from milligrams to atomic mass units.
% Chad Greene 2012
amu = mg*602213665167500000000;


function [data,net] = prepare_data(P,Y,net,P0,Y0)
% 	Prepare Input Data for the use for NN Training and check for errors
%
% 	Args:
% 		P:		neural network Inputs
% 		Y: 		neural network Targets
% 		P0:		previous input data
% 		Y0:		previous output data
% 		net: 	neural network
% 	Returns:
% 		data:	struct containing data for training or calculating putput

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

%  Ceck if input and output data match structure of NN
if size(P,1) ~= net.nn(1)
    error('Dimension of Input Data P does not match number of inputs of the NN')
end
if size(Y,1) ~= net.nn(end)
    error('Dimension of Output Data Y does not match number of outputs of the NN')
end
if size(Y,2) ~= size(P,2)
    error('Input P and output Y must have same number of datapoints Q')
end

%check if previous data is given
if (~isempty(P0)) && (~isempty(Y0))
    %  Ceck if input and output of prevoius data match structure of NN
    if size(P0,1) ~= net.nn(1)
        error('Dimension of previous Input Data P0 does not match number of inputs of the NN')
    end
    if size(Y0,1) ~= net.nn(end)
        error('Dimension of previous Output Data Y0 does not match number of outputs of the NN')
    end
    if size(Y,2) ~= size(P,2)
    error('Previous Input and output data P0 and Y0 must have same number of datapoints Q0')
    end

    q0 = size(P0,2); %number of prevoius Datapoints given
    a=cell(q0,net.M);  %initialise layer outputs
    for i=1:q0
        for j=1:net.M-1
            a{i,j}=zeros(net.nn(j+1),1); %layer ouputs of hidden layers are unknown -> set to zero
        end
        a{i,net.M}=Y0(:,i)./net.normY; %set layer ouputs of output layer
    end
    %add previous inputs and outputs to inpu/output matrices
    P_ = [P0,P];
    Y_ = [Y0,Y];
else
	%add previous inputs and outputs to inpu/output matrices
    P_ = P;
    Y_ = Y;
    q0=0;
    a={};
end

% normalize
P_norm = P_;
Y_norm = Y_;
if isfield(net, 'normP')==0
    normP = ones(size(P_,1),1);
    for p = 1:size(P_,1)
        normP(p) = max(max(abs(P_(p,:))),1.0);
        P_norm(p,:) = P_(p,:)./normP(p);
    end
    normY = ones(size(Y_,1),1);
    for y = 1:size(Y_,1)
        normY(y) = max(max(abs(Y_(y,:))),1.0);
        Y_norm(y,:) = Y_(y,:)./normY(y);
    end
    net.normP = normP;
    net.normY = normY;
else
    for p = 1:size(P_,1)
        P_norm(p,:) = P_(p,:)./net.normP(p);
    end
    for y = 1:size(Y_,1)
        Y_norm(y,:) = Y_(y,:)./net.normY(y);
    end
end

%Create data dict
data = {};
data.P = P_norm;
data.Y = Y_norm;
data.a = a;
data.q0 = q0;

end


classdef BundleAdjust < matlab.mixin.Copyable

    properties
        % Cameras and landmarks are represented by graph nodes
        %   coord(camera) -> camera pose, 6 vector
        %   vdata(camera) -> index into state vector or [] if a fixed camera
        %   coord(landmark) -> landmark position, first 3 elements of 6 vector
        %   vdata(landmark) -> index into state vector
        %
        % An edge from camera to a landmark holds the observed projection as a
        % property
        %   edata(edge) -> uv*

        g         % PGraph object describing visibility

        camera    % camera projection model

        cameras   % list of graph nodes corresponding to cameras
        points    % list of graph nodes corresponding to landmarks

        fixedcam     % logical array of cameras that are fixed
        fixedpoint   % logical array of landmarks that are fixed
    end

    properties (Dependent=true)
        ncams       % number of cameras
        nlandmarks  % number of landmark points
        ndim        % size of the linear system
    end

    methods

        function ba = BundleAdjust(camera)
            %BundleAdjust.BundleAdjust Bundle adjustment problem constructor
            %
            % Notes::
            % - A cameraModel file must exist on the path to evaluate the point projection
            %   and Jacobians.

            ba.camera = camera;  % stash the camera model
            ba.g = PGraph(6);    % initialize the graph, nodes have 6D coordinates
        end

        function n = get.ncams(ba)
            n = length(ba.cameras);
        end

        function n = get.nlandmarks(ba)
            n = length(ba.points);
        end

        function n = get.ndim(ba)
            n = 6*(ba.ncams - sum(ba.fixedcam)) + 3*(ba.nlandmarks - sum(ba.fixedpoint));
        end

        function n = cameraIdx(ba, i)
            n = 6*i - 5;
        end

        function n = landmarkIdx(ba, j)
            n = ba.ncams*6 + 3*j-2;
        end

        function n = cameraIdx2(ba, i)
            if ba.fixedcam(i)
                n = 0;
            else
                n = 6*(i - sum(ba.fixedcam(1:i))) - 5;
            end
        end

        function n = landmarkIdx2(ba, j)
            if ba.fixedpoint(j)
                n = 0;
            else
                n = 6*(ba.ncams - sum(ba.fixedcam)) + 3 * (j - sum(ba.fixedpoint(1:j))) - 2;
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % METHODS TO SUPPORT BUILDING PROBLEMS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function vc = add_camera(ba, T, varargin)
            %BundleAdjust.add_camera Add camera to bundle adjustment problem
            %
            % VC = BA.add_camera(T, options) is the vertex id of a camera node added to
            % a bundle adjustment problem.  The camera has pose T which can be SE3 or a
            % vector (1x7) comprising translation and quaternion.
            %
            % VC = BA.add_camera() as above but the camera pose is the null pose in SE3.
            %
            % Options::
            % 'fixed'    This camera is fixed (anchored) and will not be adjusted in
            %            the optimization process.
            %
            % See also BundleAdjust.add_landmark, BundleAdjust.add_projection, PGraph.

            if nargin == 1
                T = SE3();
            end

            opt.fixed = false;
            opt = tb_optparse(opt, varargin);

            if isvec(T, 7)
                t = T(1:3); q = T(4:7);
            else
                [R,t] = tr2rt(T);
                q = UnitQuaternion(R).double;
                t = t';
            end

            if q(1) < 0
                q = -q;
            end
            x = [t q(2:4)];
            vc = ba.g.add_node(x);
            ba.cameras = [ba.cameras vc];
            ba.g.setvdata(vc, length(ba.cameras));

            ba.fixedcam = [ba.fixedcam opt.fixed];
        end

        function vp = add_landmark(ba, P, varargin)
            %BundleAdjust.add_landmark Add landmark to bundle adjustment problem
            %
            % VL = BA.add_landmark(P, options) is the vertex id of a landmark node added to
            % a bundle adjustment problem.  The landmark has position P (3x1).
            %
            % Options::
            % 'fixed'    This landmark is fixed (anchored) and will not be adjusted in
            %            the optimization process.
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_projection, PGraph.

            assert(isvec(P), 'P must be a 3-vector');

            opt.fixed = false;
            opt = tb_optparse(opt, varargin);

            x = [P(:); 0; 0; 0];
            vp = ba.g.add_node(x);
            ba.points = [ba.points vp];
            ba.g.setvdata(vp, length(ba.points));

            ba.fixedpoint = [ba.fixedpoint opt.fixed];

        end

        function e = add_projection(ba, c, p, uv)
            %BundleAdjust.add_projection Add camera to bundle adjustment problem
            %
            % EP = BA.add_projection(VC, VL, UV) is the edge id of an edge added to
            % a bundle adjustment problem.  it represents the observed projection UV (2x1) of the landmark node VL as
            % seen by the camera node VC.
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_landmark, PGraph.

            assert(isvec(uv, 2), 'uv must be a 2-vector');

            e = ba.g.add_edge(c, p);
            ba.g.setedata(e, uv(:));
        end

        function load_sba(ba, cameraFile, pointFile, calibFile)
            %BundleAdjust.load_sba Load bundle adjustment problem from files
            %
            % BA.load_sba(camfile, pointfile, calibfile) loads a bundle adjustment
            % problem from data files as distributed with the SBA package
            %
            % Example::
            %
            % To solve the 7-point bundle adjustment problem distributed with SBA:
            %
            %           ba = BundleAdjust();
            %           pth = 'sba-1.6/demo/'
            %           ba.load_sba([pth '7cams.txt'], [pth '7pts.txt'], [pth 'calib.txt']);
            %
            % Reference::
            % - Sparse Bundle Adjustment package by Manolis Lourakis
            %   http://users.ics.forth.gr/~lourakis/sba
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_landmark, BundleAdjust.add_projection, PGraph.

            % adopted from sba-1.6/matlab/eucsbademo.m

            % read in camera parameters
            [q1, q2, q3, q4, tx, ty, tz] = textread(cameraFile, '%f%f%f%f%f%f%f', 'commentstyle', 'shell');

            for i=1:length(q1)
                ba.add_camera( [tx(i) ty(i) tz(i) q1(i) q2(i) q3(i) q4(i)] );
            end

            % read points file line by line
            %
            % each line is a world point
            fid = fopen(pointFile);
            npts = 0;
            while ~feof(fid)
                line = fgets(fid);
                [A, count, errmsg, nextindex] = sscanf(line, '%f%f%f%f', [1, 4]); % read X, Y, Z, nframes
                if(size(A, 2)>0) % did we read anything?
                    npts=npts+1;

                    % create a node for this point
                    lm = ba.add_landmark(A(1:3));

                    % now find which cameras it was seen by
                    nframes = A(4);
                    for i=1:nframes % read "nframes" id, x, y triplets
                        [A, count, errmsg, j] = sscanf(line(nextindex:length(line)), '%f%f%f', [1, 3]); % read id, x, y
                        nextindex=nextindex+j; % skip the already read line prefix

                        % add a landmark projection
                        ba.add_projection( ba.cameras(A(1)+1), lm, A(2:3) );
                    end
                end
            end
            fclose(fid);

            % read in calibration parameters
            [a1, a2, a3] = textread(calibFile, '%f%f%f', 'commentstyle', 'shell');
            % a1(1)    a2(1)  a3(1)  a2(2)    a3(2)];
            % f/rho_u  skew   u0     f/rho_v  v0

            ba.camera.f = a1(1);
            ba.camera.rho = [1 a1(1)/a2(2)];
            ba.camera.pp = [a3(1) a3(2)];
        end

        function T = getcamera(ba, ii)
            assert(all(ii > 0), 'camera indices must be > 0');
            assert(all(ii <= ba.ncams), 'camera index out of range');
            if nargin == 1
                ii = [1:ba.ncams];
            end
            for k=1:length(ii)
                i = ii(k);
                node = ba.cameras(i);
                qv = ba.g.coord(node);
                T(k) = SE3(qv(1:3)') * UnitQuaternion.vec(qv(4:6)).SE3;
            end

        end

        function P = getlandmark(ba, jj)
            assert(all(jj > 0), 'landmark indices must be > 0');
            assert(all(jj <= ba.nlandmarks), 'landmark index out of range');
            if nargin == 1
                jj = [1:ba.nlandmarks];
            end
            for k=1:length(jj)
                j = jj(k);
                node = ba.points(j);
                qv = ba.g.coord(node);
                P(:,k) = qv(1:3);
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % METHODS TO SOLVE PROBLEMS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function [ba2,e] = optimize(ba, varargin)
            %BundleAdjust.optimize  Optimize the solution
            %
            % BA2 = BA.optimize(

            opt.iterations = 1000;
            opt.animate = false;
            opt.retain = false;
            opt.lambda = 0.1;
            opt.lambdamin = 1e-8;
            opt.dxmin = 1e-8;
            opt.tol = 0.05;

            [opt,args] = tb_optparse(opt, varargin);

            %g2 = PGraph(pg.graph);  % deep copy

            if length(args) > 0 && isvec(args{1}, ba.ndim)
                X = args{1};
            else
                X = ba.getstate();
            end

            lambda = opt.lambda;

                        X0 = X;

            t0 = cputime();

            fprintf('Initial cost %g\n', ba.errors(X));

            for i=1:opt.iterations
                if opt.animate
                    if ~opt.retain
                        clf
                    end
                    g2.plot();
                    pause(0.5)
                end

                tic

                % solve for the step
                [dX,energy] = ba.solve(X, lambda);
                % update the state
                Xnew = ba.updatestate(X, dX);
                % compute new value of cost
                enew = ba.errors(Xnew);

                dt = toc;
                fprintf('  total cost %g (solved in %.2g sec)', enew, dt);

                % are we there yet?
                if enew < opt.tol
                    break
                end

                % have we stopped moving
                if  norm(dX) < opt.dxmin
                    break
                end

                % do the Levenberg-Marquadt thing, was it a good update?
                if enew < energy
                    % step is accepted
                    X = Xnew;
                    if lambda > opt.lambdamin
                        lambda = lambda/sqrt(2);
                    end
                    if opt.verbose
                        fprintf(' -- step accepted: lambda = %g', lambda);
                    end
                else
                    % step is rejected
                    lambda = lambda*4;
                    if opt.verbose
                        fprintf(' -- step rejected: lambda = %g', lambda);
                    end
                end

                fprintf('\n');
            end

            tf = cputime();
            fprintf('\n * %d iterations in %.1f seconds\n', i, tf-t0);
            fprintf(' * %.2f pixels RMS error\n', sqrt(enew/ba.g.ne));

            % copy the object and update it
            ba2 = copy(ba);
            ba2.g = copy(ba.g);
            ba2.setstate(X);

            if nargout > 1
                e = enew;
            end
        end

        function [deltax,err] = solve(ba, X, lambda)

            if nargin < 3
                lambda =0;
            end

            %ba.init_tables();

            % create the Hessian and error vector
            [H,b,e] = ba.build_linear_system(X);

            % add damping term to the diagonal
            for i=1:ba.ndim
                H(i,i) = H(i,i) + lambda;
            end

            % solve for the state update
            % - could replace this with the Schur complement trick
            deltax = H \ b;

            if nargout > 1
                err = e;
            end
        end

        % build the Hessian and measurement vector
        function [H,b,etotal,J] = build_linear_system(ba, X)

            % allocate storage
            H = sparse(ba.ndim, ba.ndim);
            b = zeros(ba.ndim,1);

            etotal = 0;

            % loop over cameras
            for i=1:ba.ncams

                k = ba.cameraIdx(i);
                x = X(k:k+5);

                t = x(1:3); qv = x(4:6);

                % loop over all points viewed from this camera
                for p=ba.g.edges( ba.cameras(i) )
                    v = ba.g.vertices(p);
                    j = ba.g.vdata(v(2));

                    k = ba.landmarkIdx(j);
                    x = X(k:k+2);

                    P = x';

                    uv = ba.g.edata(p);

                    % compute Jacobians and projection

                    [uvhat,JA,JB] = ba.camera.derivs(t, qv, P);

                    % compute reprojection error
                    e = uvhat - uv;
                    etotal = etotal + e'*e;

                    ii = ba.cameraIdx2(i); jj = ba.landmarkIdx2(j);

                    % compute the block components of H and b for this edge

                    if ~ba.fixedcam(i) & ~ba.fixedpoint(j)
                        % adjustable point and camera
                        H_ii= JA'*JA;
                        H_ij= JA'*JB;
                        H_jj= JB'*JB;

                        H(ii:ii+5,ii:ii+5) = H(ii:ii+5,ii:ii+5) + H_ii;
                        H(ii:ii+5,jj:jj+2) = H(ii:ii+5,jj:jj+2) + H_ij;
                        H(jj:jj+2,ii:ii+5) = H(jj:jj+2,ii:ii+5) + H_ij';
                        H(jj:jj+2,jj:jj+2) = H(jj:jj+2,jj:jj+2) + H_jj;

                        b_i = -JA'*e;
                        b_j = -JB'*e;

                        b(ii:ii+5) = b(ii:ii+5) + b_i;
                        b(jj:jj+2) = b(jj:jj+2) + b_j;

                    elseif ba.fixedcam(i) & ~ba.fixedpoint(j)
                        % fixed camera and adjustable point

                        H_jj= JB'*JB;

                        H(jj:jj+2,jj:jj+2) = H(jj:jj+2,jj:jj+2) + H_jj;

                        b_j = -JB'*e;

                        b(jj:jj+2) = b(jj:jj+2) + b_j;

                    elseif ~ba.fixedcam(i) & ba.fixedpoint(j)
                        % adjustable camera and fixed point

                        H_ii= JA'*JA;

                        H(ii:ii+5,ii:ii+5) = H(ii:ii+5,ii:ii+5) + H_ii;

                        b_i = -JA'*e;

                        b(ii:ii+5) = b(ii:ii+5) + b_i;
                    end

                end
            end

        end

        function spyH(ba, X)
            H = build_linear_system(ba, X);
            spy(H);
        end

        function X = getstate(ba)
            %BundleAdjust.getstate Get the state vector
            %
            % X = BA.getstate() is a row vector containing the state vector of the
            % problem: all camera poses followed by all landmark coordinates.
            %
            % Notes::
            % - The length of the vector is given by BA.ndim.

            X = [];

            i = 0;
            for c=ba.cameras  % step through camera nodes
                X = [X  ba.g.coord(c)'];
            end

            for p=ba.points  % step through landmark nodes
                P = ba.g.coord( p );
                X = [X  P(1:3)'];
            end
        end

        function setstate(ba, X)
            %BundleAdjust.getstate Get the state vector
            %
            % X = BA.getstate() is a row vector containing the state vector of the
            % problem: all camera poses followed by all landmark coordinates.
            %
            % Notes::
            % - The length of the vector is given by BA.ndim.

            for i=1:ba.ncams  % step through camera nodes
                if ~ba.fixedcam(i)
                    k = ba.cameraIdx(i);
                    c = ba.cameras(i);
                    ba.g.setcoord(c, X(k:k+5));
                end
            end

            for j=1:ba.nlandmarks
                if ~ba.fixedpoint(j)
                    k = ba.landmarkIdx(j);
                    lm = ba.points(j);
                    ba.g.setcoord(lm, [X(k:k+2) 0 0 0]);
                end
            end
        end

        function Xnew = updatestate(ba, X, dX)
            dX = dX(:)';

            % for each camera we need to compound the camera pose with the
            % incremental relative pose
            for i=1:ba.ncams
                k = ba.cameraIdx(i);
                if ba.fixedcam(i)
                    Xnew(k:k+5) = X(k:k+5);
                else
                    x = X(k:k+5);
                    t = x(1:3); qv = x(4:6);  % get current pose

                    k2 = ba.cameraIdx2(i);
                    dx = dX(k2:k2+5);
                    dt = dx(1:3); dqv = dx(4:6); % get incremental pose
                    tnew = t+dt;  % assume translation in old frame
                    % compound the quaternion vector rotations
                    % - function qvmul is symbolically generated by ba
                    %qvnew = qvmul(qv(1), qv(2), qv(3), dqv(1), dqv(2), dqv(3));
                    qvnew = UnitQuaternion.qvmul(qv, dqv);

                    Xnew(k:k+5) = [tnew qvnew];
                end
            end

            % for each landmark we add the increment to its position
            for j=1:ba.nlandmarks
                k = ba.landmarkIdx(j);
                x = X(k:k+2);
                if ba.fixedpoint(j)
                    Xnew(k:k+2) = x;
                else
                    k2 = ba.landmarkIdx2(j);
                    dx = dX(k2:k2+2);
                    Xnew(k:k+2) = x + dx;
                end
            end
        end

        % Compute total squared reprojection error
        function etotal = errors(ba, X)

            if nargin < 2
                X = ba.getstate();
            end
            r = ba.getresidual(X);

            etotal = sum(r(:));
        end

        function r = getresidual(ba, X)
            % this is the squared reprojection errors

            if nargin == 1
                X = ba.getstate();
            end

            % loop over cameras
            for i=1:ba.ncams

                k = ba.cameraIdx(i);
                x = X(k:k+5);

                t = x(1:3); qv = x(4:6);

                % loop over all points viewed from this camera
                for p=ba.g.edges( ba.cameras(i) )
                    v = ba.g.vertices(p);
                    j = ba.g.vdata(v(2));

                    k = ba.landmarkIdx(j);
                    x = X(k:k+2);

                    P = x';

                    uv = ba.g.edata(p);

                    uvhat = ba.camera.derivs(t, qv, P);

                    % compute reprojection error
                    e = uvhat - uv;
                    r(i,j) =  e'*e;
                end
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % SUPPORT METHODS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function plot(ba, varargin)
            clf
            ba.g.plot('dims', 3, 'only', ba.points, 'EdgeColor', 0.8*[1 1 1], varargin{:});
            axis equal
            hold on

            colorOrder = get(gca, 'ColorOrder');
            for i=1:ba.ncams
                T = ba.getcamera(i);
                cam(i) = ba.camera.move(T);
                cidx = mod(i-1, numrows(colorOrder))+1;
                color = colorOrder(cidx,:);
                cam(i).plot_camera('scale', 0.2, 'color', color, 'persist')
            end
            hold off
            axis equal
            xlabel('X (m)')
            ylabel('Y (m)')
            zlabel('Z (m)')
            grid on
        end

        function display(ba)
            %BundleAdjust.display BundleAdjust parameters
            %
            % BA.display() displays the bundle adjustment parameters in compact format.
            %
            % Notes::
            % - This method is invoked implicitly at the command line when the result
            %   of an expression is a BundleAdjust object and the command has no trailing
            %   semicolon.
            %
            % See also BundleAdjust.char.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(ba) );
        end % display()

        function s = char(ba)
            %BundleAdjust.char Convert to string
            %
            % BA.char() is a string showing bundle adjustment parameters in compact format.
            %
            %
            % See also BundleAdjust.display.
            s = 'Bundle adjustment problem:';
            s = strvcat(s, sprintf('  %d cameras', ba.ncams));
            if any(ba.fixedcam)
                s = strvcat(s, sprintf('    locked cameras: %s', num2str(find(ba.fixedcam))));
            end

            s = strvcat(s, sprintf('  %d landmarks', ba.nlandmarks));

            s = strvcat(s, sprintf('  %d projections', ba.g.ne));

            s = strvcat(s, sprintf('  %d dimension linear problem', ba.ndim));
            c = ba.g.connectivity(ba.cameras);
            s = strvcat(s, sprintf('  landmarks per camera: min=%.1f, max=%.1f, avg=%.1f', min(c), max(c), mean(c)));
            c = ba.g.connectivity(ba.points);
            s = strvcat(s, sprintf('  cameras per landmark: min=%.1f, max=%.1f, avg=%.1f', min(c), max(c), mean(c)));
        end

    end % methods

end % classdef


% demo code for 3D human pose estimation from a monocular RGB image
% this demo code uses CPM (Convolutional Pose Machines) for 2D pose
% estimation, however, any 2D pose estimator from image can be applied.
% prediction: The 2D pose, a 14X2 matrix, 14 corresponding position as
% listed in the following order, 3 columns with [x y] coordinates.
% (head)
% (neck)
% (right shoulder)
% (right elbow)
% (right wrist)
% (left shoulder)
% (left elbow)
% (left wrist)
% (right hip)
% (right knee)
% (right ankle)
% (left hip)
% (left knee)
% (left ankle)

%% setup
% param = model_config();
load('3D_library.mat');

%% input image path
test_image = ['imgs/im0371.jpg'];
im = imread(test_image);

% load the precomputed 2D pose derived by Convolutional Pose Machines,
% other 2D pose estimation methods is applicable, format the 2D pose in the
% definition described above
load('0371.mat');

%{
% get image size
[H,W,~] = size(im);

%% detect human using CPM
rectangle(1) = 1;
rectangle(2) = 1;
rectangle(3) = W-1;
rectangle(4) = H-1;
[heatMaps, human_c] = applyModel_human3d(test_image, param, rectangle, 2);

%% 2D pose estimation using CPM with the bounding box specified by the
% detected human postition
rectangle(1) = human_c(1) - 0.5*W;
rectangle(2) = human_c(2) - 0.5*H;
rectangle(3) = 1*W;
rectangle(4) = 1*H;
[heatMaps, prediction] = applyModel_human3d(test_image, param, rectangle, 1);
%}
Prediction{1}=prediction;

% manually adjust two hips' position due to the variation across MPII and
% H36M
prediction(9,2) = prediction(9,2) - 20;
prediction(12,2) = prediction(12,2) - 20;

%% extract the nearest neighbor
[j_p] = NN_pose(s1_s9_2d_n,s1_s9_3d,prediction);
% [j_p] = kNN_pose_procrus(s1_s9_2d_n,s1_s9_3d,prediction, 10);

% compute the scale between pixel and real world to recover real size of
% prediction
scale = (max(j_p(:,2))-min(j_p(:,2)))/(max(prediction(:,2))-min(prediction(:,2)));

% predict the depth of each joint by the exemplar
prediction(:,3) = j_p(:,3)/scale;

%% visualization
H=figure();
subplot(1,3,[1 2]);imshow(im);
% self-defined ground plane
prediction(:,3) = prediction(:,3) - min(prediction(:,3)) + 1300;
an_x_m = min(prediction(:,1));
an_x_M = max(prediction(:,1));
an_y_M = max(prediction(11,2),prediction(14,2));
x = [an_x_m-100 an_x_M+100 an_x_m-100 an_x_M+100];
y = [an_y_M-10 an_y_M-10 an_y_M+40 an_y_M+40];
z = [min(prediction(:,3)-120) min(prediction(:,3)-120) max(prediction(:,3)+120) max(prediction(:,3)+120)];
A = [x(:) y(:) z(:)];
% solve for the ground plane
[n,v,m,aved]=plane_fit(A);

% draw the prediction on the input image
vis_2d(prediction);

% draw the 3D pose
subplot(1,3,3);
vis_3d(prediction);

% draw the ground plane
planeplot(A,n,m)

% draw camera
scale = 50;
P = scale*[0 0 0;0.5 0.5 0.8; 0.5 -0.5 0.8; -0.5 0.5 0.8;-0.5 -0.5 0.8];
cen = mean(prediction);P1=(P+repmat([cen(1:2), 800],[5,1]));
line([P1(1,1) P1(2,1)],[P1(1,2) P1(2,2)],[P1(1,3) P1(2,3)],'color','k')
line([P1(1,1) P1(3,1)],[P1(1,2) P1(3,2)],[P1(1,3) P1(3,3)],'color','k')
line([P1(1,1) P1(4,1)],[P1(1,2) P1(4,2)],[P1(1,3) P1(4,3)],'color','k')
line([P1(1,1) P1(5,1)],[P1(1,2) P1(5,2)],[P1(1,3) P1(5,3)],'color','k')

line([P1(2,1) P1(3,1)],[P1(2,2) P1(3,2)],[P1(2,3) P1(3,3)],'color','k')
line([P1(3,1) P1(5,1)],[P1(3,2) P1(5,2)],[P1(3,3) P1(5,3)],'color','k')
line([P1(5,1) P1(4,1)],[P1(5,2) P1(4,2)],[P1(5,3) P1(4,3)],'color','k')
line([P1(4,1) P1(2,1)],[P1(4,2) P1(2,2)],[P1(4,3) P1(2,3)],'color','k')

% adjust the viewing angle
view(26,-56);
axis equal
axis off


function  kern = sdrbfKernParamInit(kern)

% SDRBFKERNPARAMINIT SDRBF kernel initialization
% FORMAT
% DESC
% Initializes the switching dynamical latent force model structure with
% some initial parameters for the RBF underlying kernels. The initial
% parameters are passed through an option in kern.
% RETURN kern : the kernel structure with the default parameters placed in.
% ARG kern : the kernel structure which requires initialisation.
%
% SEEALSO : kernCreate, kernParamInit, rbfKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% Create the basic structure based on the lfm kernel.
kern = rbfKernParamInit(kern);

if isfield(kern, 'options') && isfield(kern.options, 'isNormalised') && ...
        kern.options.isNormalised,
    kern.isNormalised = 1;
end

if isfield(kern, 'options') && isfield(kern.options, 'nIntervals')
    kern.nIntervals = kern.options.nIntervals;
    if isfield(kern.options, 'nlfPerInt')
        kern.nlfPerInt = kern.options.nlfPerInt;
    else
        kern.nlfPerInt = 1;
    end
    kern.inverseWidth = ones(kern.nlfPerInt, kern.nIntervals); % Total number of inverse widths.
    % An option for the initialization of the switching times
    if isfield(kern.options, 'switchingTimes')
        if kern.nIntervals == length(kern.options.switchingTimes)
            kern.switchingTimes = kern.options.switchingTimes;
        else
            error('The number of intervals does not match the information of the swicthing time points')
        end
    else
        partition = linspace(-0.1,1, kern.nIntervals + 1);
        kern.switchingTimes = partition(1:end-1);
    end
else
    kern.nIntervals = 1;
    kern.switchingTimes = -0.1;
    warning('LFM:Instead:SDLFM', 'Use the LFM kernel instead.')
end

% Number of parameters computed as: inverseWidth (= nlf*nIntervals) and the
% switching points (= nIntervals). The variance, which is a parameter in
% the original rbf kernel, is not counted as a parameter. It's left here,
% so that it can be used when making sdrbf kernel computations and gradients.

kern.nParams = kern.nlfPerInt*kern.nIntervals + kern.nIntervals;
dimParam = [kern.nlfPerInt*kern.nIntervals kern.nIntervals];
kern.inverseWidthIndx = 1:dimParam(1);
kern.switchingTimesIndx = dimParam(1)+1:sum(dimParam(1:2));
%kern.transforms.index = 1:(kern.nlfPerInt*kern.nIntervals);
kern.transforms.index = [1:(kern.nlfPerInt*kern.nIntervals) (kern.nlfPerInt*kern.nIntervals)+2:kern.nParams];
kern.transforms.type = optimiDefaultConstraint('positive');