text:
%QUATERNION Quaternion class
%
% A quaternion is a compact method of representing a 3D rotation that has
% computational advantages including speed and numerical robustness.
% A quaternion has 2 parts, a scalar s, and a vector v and is typically
% written: q = s <vx, vy, vz>.
%
% A unit-quaternion is one for which s^2+vx^2+vy^2+vz^2 = 1.  It can be
% considered as a rotation by an angle theta about a unit-vector V in space where
%
%         q = cos (theta/2) < v sin(theta/2)>
%
% Q = Quaternion(X) is a unit-quaternion equivalent to X which can be any
% of:
%   - orthonormal rotation matrix.
%   - homogeneous transformation matrix (rotation part only).
%   - rotation angle and vector
%
% Methods::
%  inv       inverse of quaterion
%  norm      norm of quaternion
%  unit      unitized quaternion
%  plot      same options as trplot()
%  interp    interpolation (slerp) between q and q2, 0<=s<=1
%  scale     interpolation (slerp) between identity and q, 0<=s<=1
%  dot       derivative of quaternion with angular velocity w
%  R         equivalent 3x3 rotation matrix
%  T         equivalent 4x4 homogeneous transform matrix
%  double    quaternion elements as 4-vector
%  inner     inner product of two quaternions
%
% Overloaded operators::
%  q1==q2    test for quaternion equality
%  q1~=q2    test for quaternion inequality
%  q+q2      elementwise sum of quaternions
%  q-q2      elementwise difference of quaternions
%  q*q2      quaternion product
%  q*v       rotate vector by quaternion, v is 3x1
%  s*q       elementwise multiplication of quaternion by scalar
%  q/q2      q*q2.inv
%  q^n       q to power n (integer only)
%
% Properties (read only)::
%  s         real part
%  v         vector part
%
% Notes::
% - Quaternion objects can be used in vectors and arrays.
%
% References::
% - Animating rotation with quaternion curves,
%   K. Shoemake,
%   in Proceedings of ACM SIGGRAPH, (San Fran cisco), pp. 245-254, 1985.
% - On homogeneous transforms, quaternions, and computational efficiency,
%   J. Funda, R. Taylor, and R. Paul,
%   IEEE Transactions on Robotics and Automation, vol. 6, pp. 382-388, June 1990.
% - Robotics, Vision & Control,
%   P. Corke, Springer 2011.
%
% See also trinterp, trplot.

% TODO
% properties s, v for the vector case

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO
%  constructor handles R, T trajectory and returns vector
%  .r, .t on a quaternion vector??

classdef Quaternion

    properties (SetAccess = private)
        s       % scalar part
        v       % vector part
    end

    methods

        function q = Quaternion(a1, a2)
        %Quaternion.Quaternion Constructor for quaternion objects
        %
        % Construct a quaternion from various other orientation representations.
        %
        % Q = Quaternion() is the identitity unit-quaternion 1<0,0,0> representing a null rotation.
        %
        % Q = Quaternion(Q1) is a copy of the quaternion Q1
        %
        % Q = Quaternion([S V1 V2 V3]) is a quaternion formed by specifying directly its 4 elements
        %
        % Q = Quaternion(S) is a quaternion formed from the scalar S and zero vector part: S<0,0,0>
        %
        % Q = Quaternion(V) is a pure quaternion with the specified vector part: 0<V>
        %
        % Q = Quaternion(TH, V) is a unit-quaternion corresponding to rotation of TH about the
        % vector V.
        %
        % Q = Quaternion(R) is a unit-quaternion corresponding to the SO(3)
        % orthonormal rotation matrix R (3x3).  If R (3x3xN) is a sequence then Q
        % (Nx1) is a vector of Quaternions corresponding to the elements of R.
        %
        % Q = Quaternion(T) is a unit-quaternion equivalent to the rotational part
        % of the SE(3) homogeneous transform T (4x4). If T (4x4xN) is a sequence
        % then Q (Nx1) is a vector of Quaternions corresponding to the elements of
        % T.

            if nargin == 0
                q.v = [0,0,0];
                q.s = 1;
            elseif isa(a1, 'Quaternion')
            %   Q = Quaternion(q)       from another quaternion
                q = a1;
            elseif nargin == 1
                if isvec(a1, 4)
            %   Q = Quaternion([s v1 v2 v3])    from 4 elements
                    a1 = a1(:);
                    q.s = a1(1);
                    q.v = a1(2:4)';
                elseif isrot(a1) || ishomog(a1)
            %   Q = Quaternion(R)       from a 3x3 or 4x4 matrix
                    for i=1:size(a1,3)
                        q(i) = Quaternion( tr2q(a1(:,:,i)) );
                    end

                elseif length(a1) == 3
            %   Q = Quaternion(v)       from a vector

                    q.s = 0;
                    q.v = a1(:)';
                elseif length(a1) == 1
            %   Q = Quaternion(s)       from a scalar
                    q.s = a1(1);
                    q.v = [0 0 0];
                else
                    error('RTB:Quaternion:badarg', 'unknown dimension of input');
                end
            elseif nargin == 2
                if isscalar(a1) && isvector(a2)
                %   Q = Quaternion(theta, v)    from vector plus angle
                    q.s = cos(a1/2);
                    q.v = sin(a1/2)*unit(a2(:)');
                else
                    error ('RTB:Quaternion:badarg', 'bad argument to quaternion constructor');
                end

            end
        end

        function s = char(q)
        %Quaternion.char Convert to string
        %
        % S = Q.char() is a compact string representation of the quaternion's value
        % as a 4-tuple.  If Q is a vector then S has one line per element.

            if length(q) > 1
                s = '';
                for qq = q;
                    s = char(s, char(qq));
                end
                return
            end
            s = [num2str(q.s), ' < ' ...
                num2str(q.v(1)) ', ' num2str(q.v(2)) ', '   num2str(q.v(3)) ' >'];
        end

        function display(q)
        %Quaternion.display Display quaternion
        %
        % Q.display() displays a compact string representation of the quaternion's value
        % as a 4-tuple.  If Q is a vector then S has one line per element.
        %
        % Notes::
        % - This method is invoked implicitly at the command line when the result
        %   of an expression is a Quaternion object and the command has no trailing
        %   semicolon.
        %
        % See also Quaternion.char.

            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            if loose
                disp(' ');
            end
            disp(char(q))
            if loose
                disp(' ');
            end
        end

        function v = double(q)
        %Quaternion.double Convert a quaternion to a 4-element vector
        %
        % V = Q.double() is a 4-vector comprising the quaternion
        % elements [s vx vy vz].

            v = [q.s q.v];
        end

        function qi = inv(q)
        %Quaternion.inv Invert a unit-quaternion
        %
        % QI = Q.inv() is a quaternion object representing the inverse of Q.

            qi = Quaternion([q.s -q.v]);
        end

        function qu = unit(q)
        %Quaternion.unit Unitize a quaternion
        %
        % QU = Q.unit() is a unit-quaternion representing the same orientation as Q.
        %
        % See also Quaternion.norm.

            qu = q / norm(q);
        end

        function n = norm(q)
        %Quaternion.norm Quaternion magnitude
        %
        % QN = Q.norm(Q) is the scalar norm or magnitude of the quaternion Q.
        %
        % Notes::
        % - This is the Euclidean norm of the quaternion written as a 4-vector.
        % - A unit-quaternion has a norm of one.
        %
        % See also Quaternion.inner, Quaternion.unit.

            n = norm(double(q));
        end

        function n = inner(q1, q2)
        %Quaternion.inner Quaternion inner product
        %
        % V = Q1.inner(Q2) is the inner (dot) product of two vectors (1x4),
        % comprising the elements of Q1 and Q2 respectively.
        %
        % Notes::
        % - Q1.inner(Q1) is the same as Q1.norm().
        %
        % See also Quaternion.norm.

            n = double(q1)*double(q2)';
        end

        function q = interp(Q1, Q2, r)
        %Quaternion.interp Interpolate quaternions
        %
        % QI = Q1.interp(Q2, S) is a unit-quaternion that interpolates a rotation
        % between Q1 for S=0 and Q2 for S=1.
        %
        % If S is a vector QI is a vector of quaternions, each element
        % corresponding to sequential elements of S.
        %
        % Notes::
        % - This is a spherical linear interpolation (slerp) that can be interpretted
        %   as interpolation along a great circle arc on a sphere.
        % - The value of S is clipped to the interval 0 to 1.
        %
        % References::
        % - Animating rotation with quaternion curves,
        %   K. Shoemake,
        %   in Proceedings of ACM SIGGRAPH, (San Fran cisco), pp. 245-254, 1985.
        %
        % See also Quaternion.scale, ctraj.

            q1 = double(Q1);
            q2 = double(Q2);

            cosTheta = q1*q2';
            % take shortest path along the great circle, patch by Gauthier Gras
            if cosTheta < 0
                q1 = - q1;
                cosTheta = - cosTheta;
            end;

            theta = acos(cosTheta);
            count = 1;

            % clip values of r
            r(r<0) = 0;
            r(r>1) = 1;

            q(length(r)) = Quaternion();  % preallocate space for Quaternion vector

            for i=1:length(r)
                if theta == 0
                    q(i) = Q1;
                else
                    q(i) = Quaternion( (sin((1-r(i))*theta) * q1 + sin(r(i)*theta) * q2) / sin(theta) );
                end
            end
        end

        function q = scale(Q, r)
        %Quaternion.scale Interpolate rotations expressed by quaternion objects
        %
        % QI = Q.scale(S) is a unit-quaternion that interpolates between a null
        % rotation (identity quaternion) for S=0 to Q for S=1.  This is a spherical
        % linear interpolation (slerp) that can be interpretted as interpolation
        % along a great circle arc on a sphere.
        %
        % If S is a vector QI is a vector of quaternions, each element
        % corresponding to sequential elements of S.
        %
        % Notes::
        % - This is a spherical linear interpolation (slerp) that can be interpretted
        %   as interpolation along a great circle arc on a sphere.
        %
        % See also Quaternion.interp, ctraj.

            q2 = double(Q);

            if any(r<0) || (r>1)
                error('r out of range');
            end
            q1 = [1 0 0 0];         % identity quaternion
            theta = acos(q1*q2');

            if length(r) == 1
                if theta == 0
                    q = Q;
                else
                    q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit;
                end
            else
                count = 1;
                for R=r(:)'
                    if theta == 0
                        qq = Q;
                    else
                        qq = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit;
                    end
                    q(count) = qq;
                    count = count + 1;
                end
            end
        end

        function e = eq(q1, q2)
        %EQ Test quaternion equality
        %
        % Q1==Q2 is true if the quaternions Q1 and Q2 are equal.
        %
        % Notes::
        % - Overloaded operator '=='.
        % - Note that for unit Quaternions Q and -Q are the equivalent
        %   rotation, so non-equality does not mean rotations are not
        %   equivalent.
        % - If Q1 is a vector of quaternions, each element is compared to
        %   Q2 and the result is a logical array of the same length as Q1.
        % - If Q2 is a vector of quaternions, each element is compared to
        %   Q1 and the result is a logical array of the same length as Q2.
        % - If Q1 and Q2 are vectors of the same length, then the result
        %   is a logical array of the same length.
        %
        % See also Quaternion.ne.
            if (numel(q1) == 1) && (numel(q2) == 1)
                e = all( eq(q1.double, q2.double) );
            elseif (numel(q1) >  1) && (numel(q2) == 1)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) == q2;
                end
            elseif (numel(q1) == 1) && (numel(q2) > 1)
                e = zeros(1, numel(q2));
                for i=1:numel(q2)
                    e(i) = q2(i) == q1;
                end
            elseif numel(q1) == numel(q2)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) == q2(i);
                end
            else
                error('RTB:quaternion:badargs');
            end
        end

        function e = ne(q1, q2)
        %NE Test quaternion inequality
        %
        % Q1~=Q2 is true if the quaternions Q1 and Q2 are not equal.
        %
        % Notes::
        % - Overloaded operator '~='
        % - Note that for unit Quaternions Q and -Q are the equivalent
        %   rotation, so non-equality does not mean rotations are not
        %   equivalent.
        % - If Q1 is a vector of quaternions, each element is compared to
        %   Q2 and the result is a logical array of the same length as Q1.
        % - If Q2 is a vector of quaternions, each element is compared to
        %   Q1 and the result is a logical array of the same length as Q2.
        % - If Q1 and Q2 are vectors of the same length, then the result
        %   is a logical array of the same length.
        %
        % See also Quaternion.eq.
            if (numel(q1) == 1) && (numel(q2) == 1)
                e = all( ne(q1.double, q2.double) );
            elseif (numel(q1) >  1) && (numel(q2) == 1)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) ~= q2;
                end
            elseif (numel(q1) == 1) && (numel(q2) > 1)
                e = zeros(1, numel(q2));
                for i=1:numel(q2)
                    e(i) = q2(i) ~= q1;
                end
            elseif numel(q1) == numel(q2)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) ~= q2(i);
                end
            else
                error('RTB:quaternion:badargs');
            end
        end

        function qp = plus(q1, q2)
        %PLUS Add quaternions
        %
        % Q1+Q2 is the element-wise sum of quaternion elements.
        %
        % Notes::
        % - Overloaded operator '+'
        % - The result is not guaranteed to be a unit-quaternion.
        %
        % See also Quaternion.minus, Quaternion.mtimes.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')
                qp = Quaternion(double(q1) + double(q2));
            end
        end

        function qp = minus(q1, q2)
        %Quaternion.minus Subtract quaternions
        %
        % Q1-Q2 is the element-wise difference of quaternion elements.
        %
        % Notes::
        % - Overloaded operator '-'
        % - The result is not guaranteed to be a unit-quaternion.
        %
        % See also Quaternion.plus, Quaternion.mtimes.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')

                qp = Quaternion(double(q1) - double(q2));
            end
        end

        function qp = mtimes(q1, q2)
        %Quaternion.mtimes Multiply a quaternion object
        %
        % Q1*Q2   is a quaternion formed by the Hamilton product of two quaternions.
        % Q*V     is a vector formed by rotating the vector V by the quaternion Q.
        % Q*S     is the element-wise multiplication of quaternion elements by the scalar S.
        %
        % Notes::
        % - Overloaded operator '*'
        % - If the two multiplicands are unit-quaternions, the product will be a
        %   unit quaternion.
        %
        % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')
            %QQMUL  Multiply unit-quaternion by unit-quaternion
            %
            %   QQ = qqmul(Q1, Q2)
            %
            %   Return a product of unit-quaternions.
            %
            %   See also: TR2Q

                % decompose into scalar and vector components
                s1 = q1.s;  v1 = q1.v;
                s2 = q2.s;  v2 = q2.v;

                % form the product
                qp = Quaternion([s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)]);

            elseif isa(q1, 'Quaternion') && isa(q2, 'double')

            %QVMUL  Multiply vector by unit-quaternion
            %
            %   VT = qvmul(Q, V)
            %
            %   Rotate the vector V by the unit-quaternion Q.
            %
            %   See also: QQMUL, QINV

                if length(q2) == 3
                    qp = q1 * Quaternion([0 q2(:)']) * inv(q1);
                    qp = qp.v(:);
                elseif length(q2) == 1
                    qp = Quaternion( double(q1)*q2);
                else
                    error('quaternion-vector product: must be a 3-vector or scalar');
                end

            elseif isa(q2, 'Quaternion') && isa(q1, 'double')
                if length(q1) == 3
                    qp = q2 * Quaternion([0 q1(:)']) * inv(q2);
                    qp = qp.v;
                elseif length(q1) == 1
                    qp = Quaternion( double(q2)*q1);
                else
                    error('quaternion-vector product: must be a 3-vector or scalar');
                end
            end
        end

        function qp = mpower(q, p)
        %Quaternion.mpower Raise quaternion to integer power
        %
        % Q^N is the quaternion Q raised to the integer power N.
        %
        % Notes::
        % - Overloaded operator '^'
        % - Computed by repeated multiplication.
        % - If the argument is a unit-quaternion, the result will be a
        %   unit quaternion.
        %
        % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            % check that exponent is an integer
            if (p - floor(p)) ~= 0
                error('quaternion exponent must be integer');
            end

            qp = q;

            % multiply by itself so many times
            for i = 2:abs(p)
                qp = qp * q;
            end

            % if exponent was negative, invert it
            if p<0
                qp = inv(qp);
            end
        end

        function qq = mrdivide(q1, q2)
        %Quaternion.mrdivide Quaternion quotient.
        %
        % Q1/Q2   is a quaternion formed by Hamilton product of Q1 and inv(Q2).
        % Q/S     is the element-wise division of quaternion elements by the scalar S.
        %
        % Notes::
        % - Overloaded operator '/'
        % - If the dividend and divisor are unit-quaternions, the quotient will be a
        %   unit quaternion.
        %
        % See also Quaternion.mtimes, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            if isa(q2, 'Quaternion')
                % qq = q1 / q2
                %    = q1 * qinv(q2)

                qq = q1 * inv(q2);
            elseif isa(q2, 'double')
                qq = Quaternion( double(q1) / q2 );
            end
        end

        function plot(Q, varargin)
        %Quaternion.plot Plot a quaternion object
        %
        % Q.plot(options) plots the quaternion as an oriented coordinate frame.
        %
        % Options::
        % Options are passed to trplot and include:
        %
        % 'color',C          The color to draw the axes, MATLAB colorspec C
        % 'frame',F          The frame is named {F} and the subscript on the axis labels is F.
        % 'view',V           Set plot view parameters V=[az el] angles, or 'auto'
        %                    for view toward origin of coordinate frame
        %
        % See also trplot.

            %axis([-1 1 -1 1 -1 1])

            trplot( Q.R, varargin{:});
            drawnow
        end

        function r = R(q)
        %Quaternion.R Convert to orthonormal rotation matrix
        %
        % R = Q.R() is the equivalent SO(3) orthonormal rotation matrix (3x3).  If
        % Q represents a sequence (Nx1) then R is 3x3xN.
        %

            r = zeros(3,3,numel(q));
            for i=1:numel(q)
                r(:,:,i) = t2r( q2tr(q(i)) );
            end
        end

        function t = T(q)
        %Quaternion.T Convert to homogeneous transformation matrix
        %
        % T = Q.T() is the equivalent SE(3) homogeneous transformation matrix
        % (4x4).    If Q represents a sequence (Nx1) then T is 4x4xN.
        %
        % Notes:
        % - Has a zero translational component.
            t = zeros(4,4,numel(q));
            for i=1:numel(q)
                t(:,:,i) = q2tr(q(i));
            end
        end

        function qd = dot(q, omega)
        %Quaternion.dot Quaternion derivative
        %
        % QD = Q.dot(omega) is the rate of change of a frame with attitude Q and
        % angular velocity OMEGA (1x3) expressed as a quaternion.
            E = q.s*eye(3,3) - skew(q.v);
            omega = omega(:);
            qd = Quaternion([-0.5*q.v*omega; 0.5*E*omega]);
        end
    end % methods
end % classdef

%TR2Q   Convert homogeneous transform to a unit-quaternion
%
%   Q = tr2q(T)
%
%   Return a unit-quaternion corresponding to the rotational part of the
%   homogeneous transform T.
%
%   See also: Q2TR

function q = tr2q(t)

    if ishomog(t)
        t = t2r(t);
    end
    qs = sqrt(trace(t)+1)/2.0;
    kx = t(3,2) - t(2,3);   % Oz - Ay
    ky = t(1,3) - t(3,1);   % Ax - Nz
    kz = t(2,1) - t(1,2);   % Ny - Ox

    if (t(1,1) >= t(2,2)) && (t(1,1) >= t(3,3))
        kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1
        ky1 = t(2,1) + t(1,2);          % Ny + Ox
        kz1 = t(3,1) + t(1,3);          % Nz + Ax
        add = (kx >= 0);
    elseif (t(2,2) >= t(3,3))
        kx1 = t(2,1) + t(1,2);          % Ny + Ox
        ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1
        kz1 = t(3,2) + t(2,3);          % Oz + Ay
        add = (ky >= 0);
    else
        kx1 = t(3,1) + t(1,3);          % Nz + Ax
        ky1 = t(3,2) + t(2,3);          % Oz + Ay
        kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1
        add = (kz >= 0);
    end

    if add
        kx = kx + kx1;
        ky = ky + ky1;
        kz = kz + kz1;
    else
        kx = kx - kx1;
        ky = ky - ky1;
        kz = kz - kz1;
    end
    nm = norm([kx ky kz]);
    if nm == 0
        q = Quaternion([1 0 0 0]);
    else
        s = sqrt(1 - qs^2) / nm;
        qv = s*[kx ky kz];

        q = Quaternion([qs qv]);

    end
end

%Q2TR   Convert unit-quaternion to homogeneous transform
%
%   T = q2tr(Q)
%
%   Return the rotational homogeneous transform corresponding to the unit
%   quaternion Q.
%
%   See also: TR2Q

function t = q2tr(q)

    q = double(q);
    s = q(1);
    x = q(2);
    y = q(3);
    z = q(4);

    r = [   1-2*(y^2+z^2)   2*(x*y-s*z) 2*(x*z+s*y)
        2*(x*y+s*z) 1-2*(x^2+z^2)   2*(y*z-s*x)
        2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2)   ];
    t = eye(4,4);
    t(1:3,1:3) = r;
    t(4,4) = 1;
end

author: Allopart

repo: rbpf-gmapping


text:
function A01 = synA01min(A11, A00, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 01 this function assigns the minimum value at the
% neighbouring gridpoints of colours 11 and 00.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 7, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n00, m00]=size(A00);
[n11, m11]=size(A11);
%[n01, m01]=size(A01);
n01=n00;
m01=m11;
if     m01 == m00
  S=min(stripL(extR(A00,cmax)), A00);
elseif m01 == m00-1
  S=min(stripL(A00), stripR(A00));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA01min - A11 and A00 do not match ');
end
if     n01 == n11
  T=min(A11, stripD(extU(A11, cmax)));
elseif n01 == n11+1
  T=min(extD(A11, cmax), extU(A11, cmax));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA01min - A11 and A00 do not match ');
end
%Note: all(size(S) == size(T)) & all(size(S) == [n01 m01]) always holds.
A01=min(S, T);
%-----------------------------------------------------------------------------

author: Sable

repo: mcbench-benchmarks


text:
function a = propa_yes_random ( prob, n, key )

%*****************************************************************************80
%
%% PROPA_YES_RANDOM returns a random matrix with property A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PROB, the probability that a link between
%    two eligible nodes will be made.
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );
%
%  Assign each index randomly to one of two sets.
%  SET(I) is 0 if I is in set 0, and 1 if it is in set 1.
%
  seed = key;
  [ set, seed ] = sub_random ( n, seed );

  for i = 1 : n
    for j = 1 : n
      if ( set(i) ~= set(j) )
        [ chance, seed ] = r8_uniform_01 ( seed );
        if ( chance <= prob )
          a(i,j) = 1.0;
        end
      end
    end
  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
%rf_dualBand.m
%Jamie Near, McGill University 2014.
%
% USAGE:
% [rf,AMPINT]=rf_dualBand(tp,df,n,bw,ph,shft)
%
% DESCRIPTION:
% creates an n-point dual banded gaussian inversion RF pulse with duration
% tp(ms).  The first band will be at f=0Hz and the second band will be at
% df Hz.  Bw is the bandwidth of the two selection bands in Hz.
%
% INPUTS:
% tp         = pulse duration in ms.
% df         = frequency of 2nd gaussian band [Hz].
% n          = number of points in rf waveform.
% bw         = bandwidth of both selection bands [Hz].
% ph         = phase of the second gaussian.
% shft       = frequency shift applied to both bands.
%
% OUTPUTS:
% rf         = Output rf waveform for a dual banded rf pulse, in FID-A rf
%              pulse structure format.
% AMPINT     = Calculated amplitude integral (for use in Siemens .pta files).

function [rf,AMPINT]=rf_dualBand(tp,df,n,bw,ph,shft)

%convert the pulse duration into seconds;
tps=tp/1000;

%Make a time-scale for the rf pulse.
t=[-tps/2+(tps/(2*n)):tps/n:tps/2-(tps/(2*n))]';

%Calculate the fwhm and c parameter for the first band
fwhmf1=bw;
fwhmt1=1/fwhmf1;
c1=fwhmt1/(2*sqrt(2*log(2)));

%calculate the fwhm and c parameter for the second band
fwhmf2=bw;
fwhmt2=1/fwhmf2;
c2=fwhmt2/(2*sqrt(2*log(2)));

%calculate the time-domain gaussian waveforms for the 1st and 2nd bands.
gauss1=exp((-t.^2)./(2*c1^2));
gauss2=  exp((-t.^2)./(2*c2^2)).*exp(-i*df*2*pi*t);  %shift second gauss by +df

%add phase to the second band
gauss2=addphase(gauss2,ph);

%Here we compute the Amplitude integral(AMPINT), which is used by magnetom
%to calculate the transmitter power that is required in order to achieve
%the desired flip angle.

gscaled=gauss1/max(gauss1);
AI=sum(gscaled);

dgauss=(gauss1+gauss2).*exp(-i*shft*2*pi*t);

dgauss_scaled=dgauss/max(abs(dgauss));
AMPINT=AI/max(abs(dgauss));

rfwaveform=zeros(n,3);
rfwaveform(:,1)=phase(dgauss_scaled)*180/pi;
rfwaveform(:,2)=abs(dgauss_scaled);
rfwaveform(:,3)=ones(n,1);

%Now find out the w1-max of the pulse:
[mv,sc]=bes(rfwaveform,tp,'b',0,0,5,40000);
plot(sc,mv(3,:));
xlabel('w1 (kHz)');
ylabel('mz');
w1max=input('Input desired w1max in kHz:  ');
w1max=w1max*1000; %convert w1max to [Hz]
tw1=tps*w1max;

%for the time bandwidth product, we can simply calculate:
tbw=2*tps*bw;

rf.waveform=rfwaveform;
rf.type='inv';
rf.tw1=tw1;
rf.tbw=tbw;

author: CIC-methods

repo: FID-A

save_path: github-repos/MATLAB/CIC-methods-FID-A


text:
function colors = getColorChoices
%getColorChoices Returns a structure containing several high-contrast colors.
%   COLORS = getColorChoices returns a structure containing several colors
%   defined as RGB triplets.

%   Copyright 2005 The MathWorks, Inc.
%   $Revision $  $Date: 2005/05/27 14:07:15 $

% First color in the list will be the default.
temp = {'Medium Blue',   [ 72  72 248]/255
        'Light Blue',    [ 72 136 248]/255
        'Light Red',     [248  79  79]/255
        'Green',         [ 72 248  72]/255
        'Yellow',        [248 246  74]/255
        'Magenta',       [248  72 248]/255
        'Cyan',          [ 72 248 248]/255
        'Light Gray',    [232 232 232]/255
        'Black',         [  0   0   0]/255};

colors = struct('Label', temp(:,1), 'Color', temp(:,2));


text:
function s = ellipsoidSurfaceArea(elli)
%ELLIPSOIDSURFACEAREA  Approximated surface area of an ellipsoid.
%
%   S = ellipsoidSurfaceArea(ELLI)
%   Computes an approximation of the surface area of an ellipsoid.
%   ELLI is a 1-by-9 row vector given by [XC YC ZC A B C THETA PHI PSI],
%   where (XC YC ZC) is the center, (A B C) is the length of each semi axis
%   and (THETA PHI PSI) representes the orientation.
%   If ELLI is a 1-by-3 row vector, it is assumed to contain only the
%   lengths of semi-axes.
%
%   This functions computes an approximation of the surface area, given by:
%   S = 4 * pi * ( (a^p * b^p + a^p * c^p + b^p * c^p) / 3) ^ (1/p)
%   with p = 1.6075. The resulting error should be less than 1.061%.
%
%   Example
%   ellipsoidSurfaceArea
%
%   See also
%   geom3d, ellipsePerimeter, oblateSurfaceArea, prolateSurfaceArea
%
%   References
%   * http://en.wikipedia.org/wiki/Ellipsoid
%   * http://mathworld.wolfram.com/Ellipsoid.html
%

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

%% Parse input argument

if size(elli, 2) == 9
    a = elli(:, 4);
    b = elli(:, 5);
    c = elli(:, 6);

elseif size(elli, 2) == 3
    a = elli(:, 1);
    b = elli(:, 2);
    c = elli(:, 3);
end

p = 1.6075;
s = 4 * pi * ( (a.^p .* b.^p + a.^p .* c.^p + b.^p .* c.^p) / 3) .^ (1 / p);

author: mattools

repo: matGeom

save_path: github-repos/MATLAB/mattools-matGeom


text:
x = linspace(-20,2,100);
w = exp(-0.5*x.^2 -0.5*log(2*pi));
f = exp(-normcdfln(x));
g = 1./normcdf(x);
%g = exp((log(1+exp(0.88+x))./1.5).^2);
%g = exp(0.5*log(2*pi) +0.5*x.^2 + log(x));
plot(x, f.*w, x, g.*w)
legend('exp(-normcdfln)','1/normcdf');

if 0
  true = -27.3843074988;
  [abs(log(normcdf(-7))-true) abs(normcdfln(-7)-true)]
end

if 0
  %matnet
  %imports('c:/Documents and Settings/minka/Depots/Infer/Core/bin/Debug/Core.dll')
  h = g;
  for i = 1:length(x)
    h(i) = 1./cl.MMath.NormalCdf(x(i));
  end
  %h = 0.5*(-x + sqrt(x.*x + 8/pi));
  plot(x, f.*w, x, g.*w, x, h.*w)
  legend('exp(-normcdfln)','1/normcdf', '1/normcdf2');

  plot(x, f.*w - h.*w)
end

% evalf(subs(t=1e-4,subs(x=2*(1-t)/t,erfc(x)*exp(x*x))),100);

if 0
% test approximations
a = exp(sqrt(2/pi));
b = 1/log(2/pi*a);
g = log(a-1 + exp(x.*exp(1./(x.^2 + b))));
g = log(a-1 + exp(x));
plot(x, f, x, g)
plot(x, log(exp(f)+1-a)./x)
plot(x, 1./log(log(exp(f)+1-a)./x))
end

% read `/u/tpminka/src/maple/gauss_cdf`;
% f := 1/sqrt(2*Pi)*exp(-1/2*x^2)/gauss_cdf(-x);
% g := exp(sqrt(2/Pi))-1+exp(x);
% plot(f, x=0..50);
% h := log(exp(f*sqrt(Pi/2)-1)+1);
% h := log(exp(f - sqrt(2/Pi))+1);
% h := 1/log(log(exp(f) +1-exp(sqrt(2/Pi)))/x);
% asympt(h,x);

author: andrewssobral

repo: lrslibrary


text:
   eval(['Ip = Ip_' num2str(kk) ';']);
   eval(['In = In_' num2str(kk) ';']);

   xr = xr_list(kk);
   yr = yr_list(kk);

   fprintf(1,'Processing image %d...',kk);

   if ~recompute_corner,
		[x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1);
   	xproj = x;
   else
      eval(['xproj = xproj_' num2str(kk) ';']);
      x = cornerfinder(xproj+1,Ip,winty,wintx);
      xproj = x - 1;
   end;

   Np_proj = size(x,2);

	figure(2);
	image(Ip);
	hold on;
	plot(xproj(1,:)+1,xproj(2,:)+1,'r+');
	%title('Click on your reference point');
	xlabel('Xc (in camera frame)');
	ylabel('Yc (in camera frame)');
	hold off;

	%disp('Click on your reference point...');

   %[xr,yr] = ginput2(1);

   xr = xr_list(kk);
   yr = yr_list(kk);

	err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2));
	ind_ref = find(err == min(err));

	ref_pt = xproj(:,ind_ref);

	figure(2);
	hold on;
   plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off;
   title(['Image ' num2str(kk)]);
   drawnow;

   if ~recompute_corner,

   	off_x = mod(ind_ref-1,n_sq_x+1);
   	off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1));

   	x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj));

   	eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image

	end;

   eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image

repo: ekfmonoslam


text:
function cs = loadPHL(fname)
%

[pathstr, name, ext] = fileparts(char(fname));

if isempty(ext), ext = '.phl';end
if isempty(pathstr) && ~exist([name,ext],'file')
  pathstr = mtexCifPath;
end

% load file
if ~exist(fullfile(pathstr,[name ext]),'file')
  try
    fname = copyonline(fname);
  catch %#ok<CTCH>
    dir(fullfile(mtexCifPath,'*.phl'))
    error(['I could not find the corresponding phl file.' ...
      'Above you see the list of localy avaible phl files.'])
  end
else
  fname = fullfile(pathstr,[name ext]);
end

cs = {};
doc = xmlread(fname);
root = doc.getDocumentElement();
phaseList = root.getElementsByTagName('ClassInstance');

for i = 1:phaseList.getLength

  thisPhase = phaseList.item(i-1);
  if ~strcmp(thisPhase.getAttribute('Type'),'TEBSDExtPhaseEntry'), continue; end

  mineral = char(thisPhase.getAttribute('Name'));
  phaseEntry = thisPhase.getElementsByTagName('TEBSDPhaseEntry').item(0);

  %SG = phaseEntry.getElementsByTagName('SG').item(0).getFirstChild.getNodeValue;
  IT = str2num(phaseEntry.getElementsByTagName('IT').item(0).getFirstChild.getNodeValue);
  density = str2num(phaseEntry.getElementsByTagName('DENSITY').item(0).getFirstChild.getNodeValue);
  abc = str2num(phaseEntry.getElementsByTagName('Dim').item(0).getFirstChild.getNodeValue);
  angles = str2num(phaseEntry.getElementsByTagName('Angles').item(0).getFirstChild.getNodeValue);

  cs{end+1} = crystalSymmetry('spaceId',IT,abc,angles,'density',density,'mineral',mineral);

end

% fname = '/home/hielscher/mtex/master/data/cif/crystal.phl';
%

author: mtex-toolbox

repo: mtex

save_path: github-repos/MATLAB/mtex-toolbox-mtex


text:
function [ n_data, x, fx ] = logarithmic_integral_values ( n_data )

%*****************************************************************************80
%
%% LOGARITHMIC_INTEGRAL_VALUES returns values of the logarithmic integral LI(X).
%
%  Discussion:
%
%    The logarithmic integral is defined as:
%
%      LI(X) = integral ( 0 <= T <= Z ) dT / log ( T )
%
%    The principal value of the integral is taken.  There is a
%    branch cut discontinuity in the complex plane from -infinity
%    to +1.
%
%    Abramowitz and Stegun assume 1 < X.
%
%    In Mathematica, the function can be evaluated by:
%
%      LogIntegral[x]
%
%    There is a simple relationship with the exponential integral EI:
%
%      LI(X) = EI(LN(X))
%
%    The function LI(X) provides a good approximation to PI(X),
%    the number of primes less than or equal to X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
      0.0000000000000000E+00, ...
     -0.3238978959329102E-01, ...
     -0.8512648672879405E-01, ...
     -0.1574149028946895E+00, ...
     -0.2529494192126213E+00, ...
     -0.3786710430610880E+00, ...
     -0.5468514142104170E+00, ...
     -0.7809468775455607E+00, ...
     -0.1134011957382327E+01, ...
     -0.1775800683423525E+01, ...
     -0.2443622553873225E+01, ...
     -0.3124190050507211E+01, ...
     -0.2872935510329120E+01, ...
     -0.2164282524138207E+01, ...
     -0.1440351296279408E+01, ...
     -0.6864884538258716E+00, ...
      0.1250649863152964E+00, ...
      0.1045163780117493E+01, ...
      0.2967585095039051E+01, ...
      0.5253718299558931E+01, ...
      0.8519716463711059E+01, ...
      0.1360509217709172E+02, ...
      0.2193466832805100E+02, ...
      0.3604254831722944E+02, ...
      0.6051306533791733E+02, ...
      0.1037211171690373E+03, ...
      0.1810780396816945E+03, ...
      0.3211144156746837E+03 ];

  x_vec = [ ...
     0.000000E+00, ...
     0.100000E+00, ...
     0.200000E+00, ...
     0.300000E+00, ...
     0.400000E+00, ...
     0.500000E+00, ...
     0.600000E+00, ...
     0.700000E+00, ...
     0.800000E+00, ...
     0.900000E+00, ...
     0.950000E+00, ...
     0.975000E+00, ...
     0.103125E+01, ...
     0.106250E+01, ...
     0.112500E+01, ...
     0.125000E+01, ...
     0.150000E+01, ...
     0.200000E+01, ...
     0.400000E+01, ...
     0.800000E+01, ...
     0.160000E+02, ...
     0.320000E+02, ...
     0.640000E+02, ...
     0.128000E+03, ...
     0.256000E+03, ...
     0.512000E+03, ...
     0.102400E+04, ...
     0.204800E+04 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


text:
function test_suite = test_trimeshSurfaceArea
%TEST_TRIMESHSURFACEAREA  Test case for the file trimeshSurfaceArea
%
%   Test case for the file trimeshSurfaceArea

%   Example
%   test_trimeshSurfaceArea
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-10-17,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function test_Octahedron(testCase) %#ok<*DEFNU>

[v, e, f] = createOctahedron(); %#ok<ASGLU>
area = trimeshSurfaceArea(v, f);

a = sqrt(2);
exp = 2 * sqrt(3) * a * a;

testCase.assertEqual(exp, area, 'AbsTol', .1);


text:
function voi = bf_sources_voi(BF, S)
% Generate a set of VOIs specified in MNI coordinates
% Copyright (C) 2013 Wellcome Trust Centre for Neuroimaging

% $Id: bf_sources_voi.m 7703 2019-11-22 12:06:29Z guillaume $

%--------------------------------------------------------------------------
if nargin == 0
    label = cfg_entry;
    label.tag = 'label';
    label.name = 'Label';
    label.strtype = 's';
    label.help = {'Label for the VOI'};

    pos = cfg_entry;
    pos.tag = 'pos';
    pos.name = 'MNI coordinates';
    pos.strtype = 'r';
    pos.num = [1 3];
    pos.help = {'Locations for the VOI in MNI coordinates'};
    pos.val = {};

    ori = cfg_entry;
    ori.tag = 'ori';
    ori.name = 'Orientation';
    ori.strtype = 'r';
    ori.num = [1 3];
    ori.help = {'Source orientatons (only for single points, leave zeros for unoriented)'};
    ori.val = {[0 0 0]};

    voidef = cfg_branch;
    voidef.tag = 'voidef';
    voidef.name = 'VOI';
    voidef.val = {label, pos, ori};

    mask = cfg_files;
    mask.tag = 'mask';
    mask.name = 'MNI mask';
    mask.filter = 'image';
    mask.ufilter = '.*';
    mask.num     = [1 1];
    mask.help = {'Select a mask image'};

    maskdef = cfg_branch;
    maskdef.tag = 'maskdef';
    maskdef.name = 'Mask VOI';
    maskdef.val  = {label, mask};

    vois = cfg_repeat;
    vois.tag = 'vois';
    vois.name = 'VOIs';
    vois.num  = [1 Inf];
    vois.values = {voidef, maskdef};
    vois.val = {voidef};

    radius = cfg_entry;
    radius.tag = 'radius';
    radius.name = 'Radius';
    radius.strtype = 'r';
    radius.num = [1 1];
    radius.val = {0};
    radius.help = {'Radius (in mm) for the VOIs (leave 0 for single point)'};

    resolution = cfg_entry;
    resolution.tag = 'resolution';
    resolution.name = 'Resolution';
    resolution.strtype = 'r';
    resolution.num = [1 1];
    resolution.val = {5};
    resolution.help = {'Resolution for placing grid points in each VOI (in mm)'};

    voi = cfg_branch;
    voi.tag = 'voi';
    voi.name = 'VOIs in MNI space';
    voi.val = {vois, radius, resolution};

    return
elseif nargin < 2
    error('Two input arguments are required');
end

iskull = export(gifti(BF.data.mesh.tess_iskull), 'ft');

M1 = BF.data.transforms.toNative;
M1 = BF.data.transforms.toMNI/M1;

iskull = ft_convert_units(ft_transform_geometry(M1, iskull));

% transform MNI coords in MNI space into space where we are doing the
% beamforming
M = inv(BF.data.transforms.toMNI);

if S.radius > 0
    vec = -S.radius:S.resolution:S.radius;
    [X, Y, Z]  = ndgrid(vec, vec, vec);
    sphere   = [X(:) Y(:) Z(:)];
    sphere(sqrt(X(:).^2 + Y(:).^2 + Z(:).^2) > S.radius, :) = [];
    npnt = size(sphere, 1);
else
    sphere = 0;
    npnt = 1;
end

grid = bf_sources_grid(BF, struct('resolution', S.resolution, 'space', 'MNI template'));
mnigrid = ft_transform_geometry(BF.data.transforms.toMNI, grid);

nvoi = numel(S.vois);
voi = [];
voi.label = {};
voi.pos = [];
ori = [];
voi.pos2voi = [];

for i = 1:nvoi
    switch char(fieldnames(S.vois{i}))
        case 'voidef'
            voi.label{i} = S.vois{i}.voidef.label;
            voi.pos = [voi.pos; sphere+repmat(S.vois{i}.voidef.pos, npnt, 1)];
            ori     = [ori; S.vois{i}.voidef.ori];
            voi.pos2voi  = [voi.pos2voi i*ones(1, npnt)];
        case 'maskdef'
            voi.label{i} = S.vois{i}.maskdef.label;
            V   = spm_vol(char(S.vois{i}.maskdef.mask));

            vox = spm_eeg_inv_transform_points(inv(V.mat), mnigrid.pos);
            Y   = spm_sample_vol(V, vox(:, 1),  vox(:, 2), vox(:, 3), 0);
            ind = find(~isnan(Y) & abs(Y)>0);
            voi.pos = [voi.pos; mnigrid.pos(ind, :)];
            ori = [ori;zeros(length(ind), 3)];
            voi.pos2voi  = [voi.pos2voi i*ones(1, length(ind))];
    end
end

voi.label = voi.label(:);

% Remove points outside the brain
inside = ft_inside_headmodel(voi.pos, struct('bnd', iskull));

voi.pos(~inside, :)  = [];
voi.pos2voi(~inside) = [];

if any(any(ori))
    voi.ori = ori(inside, :);
end

voi = ft_transform_geometry(M, voi);

author: spm


text:
function [go,s1]=fmri_loc1_checkDataTypesForGLM(fid,dataTYPES)
% Usage: checkDataTypesForGLM(fid)
%
% Fid = file id for error log text file
%
% Function for use in GLM batch script context.
% 1. Find 'MotionComp_RefScan1' dataTYPE #
% 2. Find 'loloc_run1' scan # for MC dataTYPE
%
% If conditions not met (e.g., EXACT STRING MATCHES for 1,2,3), will report
% error and return, skipping that subject, and set GO flag to 0.
%
% DY 06/08/2008
% AL 11/08 Modified for loc1 GLM

if notDefined('fid')    fid=1;   end
go=1; % flag that is set to 0 if any errors in datatype fields
s1=struct;

% Names for particular dataTYPES / scans
mcName = 'MotionComp_RefScan1';
lo1Name = 'loloc_run1';

% Only set the mc_dt variable if there is one MC dataTYPE, else log the
% person and find some way to deal with it (for now, we RETURN)
% NOTE: Golijeh usually runs the LO localizer scan first, so these
% scans should always use the MotionComp_RefScan1 dataTYPE.
mcs = zeros(1,length(dataTYPES));
mcs=strcmp({dataTYPES.name},mcName);
if sum(mcs)==1
    mc_dt=find(mcs==1);
    fprintf(fid,'Found %s for dataTYPES %d\n',mcName, mc_dt);
    fprintf('Found %s for dataTYPES %d\n',mcName, mc_dt);
else
    fprintf(fid,'Error: Found %d %s dataTYPES, skipping... \n\n',sum(mcs),mcName);
    fprintf('Error: Found %d %s dataTYPES, skipping... \n\n',sum(mcs),mcName);
    go=0;
    return
end

% Find scans with "loloc_run#" in the annotation field
lo1scans=zeros(1,length(dataTYPES(mc_dt).scanParams));
lo1scans=strcmp({dataTYPES(mc_dt).scanParams.annotation},lo1Name);
if sum(lo1scans)==1
    lo_scan1=find(lo1scans==1);
    fprintf(fid,'Found %s for dataTYPES %d scan %d \n',lo1Name, mc_dt, lo_scan1);
    fprintf('Found %s for dataTYPES %d scan %d\n',lo1Name, mc_dt, lo_scan1);
else
    fprintf(fid,'Error: Found %d %s scans, skipping... \n\n',sum(lo1scans),lo1Name);
    fprintf('Error: Found %d %s scans, skipping... \n\n',sum(lo1scans),lo1Name);
    go=0;
    return
end

% Find parfile and scangroup assignments for those scans
% First check if these fields exist. If they do not, print the relevant
% error to file and exit, setting go=0.

if (~isfield(dataTYPES(mc_dt).scanParams(lo_scan1),'parfile')) | ...
        (~isfield(dataTYPES(mc_dt).scanParams(lo_scan1),'scanGroup'))
    fprintf(fid,'Error: Parfile and/or scanGroup fields do not exist for dataTYPES %d... \n\n',mc_dt);
    fprintf('Error: Parfile and/or scanGroup fields do not exist for dataTYPES %d... \n\n',mc_dt);
    go=0;
    return
end

parfile1=dataTYPES(mc_dt).scanParams(lo_scan1).parfile;
scangroup1=dataTYPES(mc_dt).scanParams(lo_scan1).scanGroup;
fprintf(fid,'Parfile for %s: %s \n',lo1Name, parfile1);
fprintf('Parfile for %s: %s \n',lo1Name, parfile1);
fprintf(fid,'ScanGroup for %s: %s \n',lo1Name, scangroup1);
fprintf('ScanGroup for %s: %s \n',lo1Name, scangroup1);

% Set s1 struct fields
s1.mc=mc_dt;
s1.scan=lo_scan1;
s1.parfile=parfile1;
s1.scangroup=scangroup1;

return

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
% msTrainClassifiersCv

LOAD = 1;

nsegments = [5 7 10 15 20 25 35 50 60 70 80 90 100 Inf]; % number of segments per segmentation
labeltol = 0.9; % required percentage of single-label pixels for segment to be good
nclasses = 23;
ncv = 5;

datadir = '~/data/eccv08/msrc21/';
imdir = '~/data/msrc/MSRC_ObjCategImageDatabase_v2/Images/';
outdir = '~/data/eccv08/msrc21/';

if LOAD
    load(fullfile(datadir, 'msrc_imsegs.mat'));
    load(fullfile(datadir, 'trainLabels.mat'));
    load(fullfile(datadir, 'trainTestFn_tu.mat'));
    load(fullfile(datadir, 'msSegmentFeatures_tu.mat'));
end

disp('Getting cross-validation subsets');
if ~exist('testcv', 'var')
    testcv = cell(ncv, 1);
    traincv = cell(ncv, 1);
    ind = train(randperm(numel(train)));
    fi = floor(numel(train)*(0:ncv-1)/ncv+1);
    li = floor(numel(train)*(1:ncv)/ncv);
    for k = 1:ncv
        testcv{k} = ind(fi(k):li(k));
        traincv{k} = setdiff(train, testcv{k});
    end
    save(fullfile(outdir, 'msCvind_tu.mat'), 'traincv', 'testcv');
end

disp('Training segmentation classifier')
if ~exist('segclassifier', 'var')
    for k = 1:ncv
        disp(num2str(k))
        trainind = traincv{k};
        traindata = segfeatures(trainind, :);
        trainlabels = seggood(trainind, :);
        trainweights = trainw(trainind, :);
        segclassifier(k) = mcmcTrainSegmentationClassifier2(traindata, trainlabels, trainweights);
    end
    save(fullfile(outdir, 'msSegmentationClassifier_cv_tu.mat'), 'segclassifier');
end

disp('Training label classifier')
if ~exist('labelclassifier', 'var')
    for k = 1:ncv
        disp(num2str(k))
        trainind = traincv{k};
        traindata = segfeatures(trainind, :);
        trainlabels = seglabel(trainind, :);
        trainweights = trainw(trainind, :);
        labelclassifier(k) = msTrainLabelClassifier(traindata, trainlabels, trainweights, ...
		classnames, Inf);
        %labelclassifier = mcmcTrainSegmentClassifier2(traindata, trainlabels, trainweights);
    end
    save(fullfile(outdir, 'msLabelClassifier_cv_tu.mat'), 'labelclassifier');
end

author: zouchuhang

repo: LayoutNet


text:
switch method
    case{1} % TVD 0(h^2)
        % Using discrete ordinate method (discrete and constant velocity
        % values in phase-space domain)
        a = v(:,1);
        % Load initial condition
        f = f0;
%            for tsteps = time
               f_eq = f_equilibrium_1d(r,ux,v,t,theta);
                % initialize variables
                 u_next = zeros(1,nx);
                 u_eq = zeros(1,nx);
                 u = zeros(1,nx);
                 for i = 1:nv
                      % load subcase
                      u_eq(:) = f_eq(i,:);
                      u(:) = f(i,:);
                      % Compute the smoothness factors, r(j), from data, u(j).
                       [r] = theta1d(u,a(i));
                        % Compute the Flux Limiter
                       [phi] = fluxlimiter1d(r,1); % using limiter = 1
                       % Compute TVD Fluxes
                       [F_left,F_right] = TVDflux1d(u,a(i),dtdx,phi);
                       % Compute next time step
                        u_next = u - dtdx*(F_right - F_left) ...
                        + (dt/r_time)*(u_eq-u);
                        % BC
                        u_next(1) = u_next(2);
                        u_next(nx) = u_next(nx-1);
                        % UPDATE info
                         u = u_next;
                        % Going back to f
                         f(i,:) = u(:);
                 end
                   % Compute macroscopic moments
                   [n,j_x,E] = macromoments1d(k,w,f,v);

                   % UPDATE macroscopic properties
                    % (here lies a paralellizing computing chalenge)
                   [r,ux,t,p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%                         [p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%            end
           case{2} % WENO k = 3 i.e. O(h^5)
    otherwise
          error('Order must be between 1 and 2');
end

repo: Aero-matlab


text:
function [Y, R, E] = IsomapII(D, n_fcn, n_size, options);

% ISOMAPII   Computes Isomap embedding using an advanced version of
%             the algorithm in Tenenbaum, de Silva, and Langford (2000),
%             which can take advantage of sparsity in the graph and
%             redundancy in the distances.
%
% [Y, R, E] = isomapII(D, n_fcn, n_size, options);
%
% Input:
%    D = input-space distances between pairs of N points, which can
%     take 1 of 3 forms:
%      (1) a full N x N matrix (as in isomap.m)
%      (2) a sparse N x N matrix (missing entries are treated as INF)
%      (3) the name of a function (e.g. 'd_fun') that takes
%            one argument, i, and returns a row vector containng the
%            distances from all N points to point i.
%
%    n_fcn = neighborhood function ('epsilon' or 'k')
%    n_size = neighborhood size (value for epsilon or k)
%
%    options = optional structure of options:
%      options.dims = (row) vector of embedding dimensionalities to use
%                        (1:10 = default)
%      options.comp = which connected component to embed, if more than one.
%                        (1 = largest (default), 2 = second largest, ...)
%      options.display = plot residual variance and 2-D embedding?
%                        (1 = yes (default), 0 = no)
%      options.overlay = overlay graph on 2-D embedding?
%                        (1 = yes (default), 0 = no)
%      options.verbose = display progress reports?
%                        (1 = yes (default), 0 = no)
%      options.dijkstra = use dijkstra's algorithm for shortest paths with
%                         full N x N distance matrix.
%                         (1 = yes (default), 0 = use Floyd; Floyd should
%                          be used only if you are unable to MEX dijkstra.cpp)
%      options.Kmax = maximum number of neighbors (used for sparse versions
%                        of epsilon; by default, estimated by random sample)
%      options.landmarks = (row) vector of landmark points to use in MDS.
%                 (MDS finds the configuration that best approximates
%                  the distances from all points to the landmark points.
%                  The default landmark points are 1:N (i.e. all the points),
%                  which is equivalent to classical MDS.  Good
%                  results may often be obtained using a number of
%                  landmarks that is much smaller than N, but much
%                  larger than the data's intrinsic dimensionality.
%                  Note that this extension is experimental!  For
%                  discussion, see Steyvers, de Silva, and Tenenbaum
%                  (in preparation).)
%
% Output:
%    Y = Y.coords is a cell array, with coordinates for d-dimensional embeddings
%         in Y.coords{d}.  Y.index contains the indices of the points embedded.
%    R = residual variances for embeddings in Y
%    E = edge matrix for neighborhood graph
%

%    BEGIN COPYRIGHT NOTICE
%
%    Isomap II code -- (c) 1998-2000 Josh Tenenbaum
%
%    This code is provided as is, with no guarantees except that
%    bugs are almost surely present.  Published reports of research
%    using this code (or a modified version) should cite the
%    article that describes the algorithm:
%
%      J. B. Tenenbaum, V. de Silva, J. C. Langford (2000).  A global
%      geometric framework for nonlinear dimensionality reduction.
%      Science 290 (5500): 2319-2323, 22 December 2000.
%
%    Comments and bug reports are welcome.  Email to jbt@psych.stanford.edu.
%    I would also appreciate hearing about how you used this code,
%    improvements that you have made to it, or translations into other
%    languages.
%
%    You are free to modify, extend or distribute this code, as long
%    as this copyright notice is included whole and unchanged.
%
%    END COPYRIGHT NOTICE

%%%%% Step 0: Initialization and Parameters %%%%%

if nargin < 3
     error('Too few input arguments');
elseif nargin < 4
     options = struct('dims',1:10,'overlay',1,'comp',1,'display',1,'dijkstra',1,'verbose',1);
end

if ischar(D)
     mode = 3;
     d_func = D;
     N = length(feval(d_func,1));
elseif issparse(D)
     mode = 2;
     N = size(D,1);
     if ~(N==size(D,2))
          error('D must be a square matrix');
     end;
else
     mode = 1;
     N = size(D,1);
     if ~(N==size(D,2))
         error('D must be a square matrix');
     end;
end

if n_fcn=='k'
     K = n_size;
     if ~(K==round(K))
         error('Number of neighbors for k method must be an integer');
     end
     if ((mode==2) & ~(min(sum(D'>0))>=K))
         error('Sparse D matrix must contain at least K nonzero entries in each row');
     end
elseif n_fcn=='epsilon'
     epsilon = n_size;
     if isfield(options,'Kmax')
         K = options.Kmax;
     elseif (mode==3)    %% estimate maximum equivalent K %%
         tmp = zeros(10,N);
         for i=1:10
             tmp(i,:) = feval(d_func,ceil(N*rand));
         end
         K = 2*max(sum(tmp'<epsilon));    % just to be safe
     end
else
     error('Neighborhood function must be either epsilon or k');
end

if (mode == 3)
     INF = inf;
else
     INF =  1000*max(max(D))*N;  %% effectively infinite distance
end

if ~isfield(options,'dims')
     options.dims = 1:10;
end
if ~isfield(options,'overlay')
     options.overlay = 1;
end
if ~isfield(options,'comp')
     options.comp = 1;
end
if ~isfield(options,'display')
     options.display = 1;
end
if ~isfield(options,'verbose')
     options.verbose = 1;
end
if ~isfield(options,'landmarks')
     options.landmarks = 1:N;
end
if ~isfield(options,'dijkstra')
     options.dijkstra = 1;
end
dims = options.dims;
comp = options.comp;
overlay = options.overlay;
displ = options.display;
verbose = options.verbose;
landmarks = options.landmarks;
use_dijk = options.dijkstra;

Y.coords = cell(length(dims),1);
R = zeros(1,length(dims));

%%%%% Step 1: Construct neighborhood graph %%%%%
disp('Constructing neighborhood graph...');

if ((mode == 1) & (use_dijk == 0))
     if n_fcn == 'k'
         [tmp, ind] = sort(D);
         tic;
         for i=1:N
             D(i,ind((2+K):end,i)) = INF;
             if ((verbose == 1) & (rem(i,50) == 0))
                 disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
     elseif n_fcn == 'epsilon'
         warning off    %% Next line causes an unnecessary warning, so turn it off
         D =  D./(D<=epsilon);
         D = min(D,INF);
         warning on
     end
     D = min(D,D');    %% Make sure distance matrix is symmetric
elseif ((mode == 1) & (use_dijk == 1))
     if n_fcn == 'k'
         [tmp, ind] = sort(D);
         tic;
         for i=1:N
             D(i,ind((2+K):end,i)) = 0;
             if ((verbose == 1) & (rem(i,50) == 0))
                 disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
     elseif n_fcn == 'epsilon'
         D =  D.*(D<=epsilon);
     end
     D = sparse(D);
     D = max(D,D');    %% Make sure distance matrix is symmetric
elseif (mode == 2)
     if n_fcn == 'k'
         Di = zeros(N*K,1);      Dj = zeros(N*K,1);       Ds = zeros(N*K,1);
         counter = 0;
         [a,b,c] = find(D);
         tic;
         for i=1:N
             l = find(a==i);
             [g,f] = sort(c(l));
             Di(counter+(1:K)) = i;
             Dj(counter+(1:K)) = b(l(f(1:K)));
             Ds(counter+(1:K)) = g(1:K);
             counter = counter+K;
             if ((verbose == 1) & (rem(i,50) == 0))
                  disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
         D = sparse(Di(1:counter), Dj(1:counter), Ds(1:counter));
         clear Di Dj Ds counter;
     elseif n_fcn == 'epsilon'
         D =  D.*(D<=epsilon);
     end
     D = max(D,D');    %% Make sure distance matrix is symmetric
elseif (mode == 3)
     Di = zeros(N*(K+1),1);      Dj = zeros(N*(K+1),1);       Ds = zeros(N*(K+1),1);
     counter = 0;
     tic;
     for i=1:N
         d = feval(d_func,i);
         if n_fcn == 'k'
             [c,b] = sort(d);
             Di(counter+(1:(K+1))) = i;
             Dj(counter+(1:(K+1))) = b(1:(K+1));
             Ds(counter+(1:(K+1))) = c(1:(K+1));
             counter = counter+(K+1);
         elseif n_fcn == 'epsilon'
             [a,b,c] = find(d.*(d<=epsilon));
             l = length(a);
             Di(counter+(1:l)) = i;
             Dj(counter+(1:l)) = b;
             Ds(counter+(1:l)) = c;
             counter = counter+l;
         end
         if ((verbose == 1) & (rem(i,50) == 0))
              disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
         end
     end
     D = sparse(Di(1:counter), Dj(1:counter), Ds(1:counter));
     clear Di Dj Ds counter;
     D = max(D,D');    %% Make sure distance matrix is symmetric
end

if (overlay == 1)
     if ((mode == 1) & (use_dijk == 0))
         E = int8(1-(D==INF));  %%  Edge information for subsequent graph overlay
     else
         [a,b,c] = find(D);
         E = sparse(a,b,ones(size(a)));
     end
end

%%%%% Step 2: Compute shortest paths %%%%%
disp('Computing shortest paths...');

if ((mode==1) & (use_dijk == 0))
     tic;
     for k=1:N
         D = min(D,repmat(D(:,k),[1 N])+repmat(D(k,:),[N 1]));
         if ((verbose == 1) & (rem(k,20) == 0))
              disp([' Iteration: ' num2str(k) '     Estimated time to completion: 'num2str((N-i)*toc/i/60) ' minutes']);
         end
     end
else
     D = dijkstra(D, landmarks);
end

%%%%% Step 3: Construct low-dimensional embeddings (Classical MDS) %%%%%
disp('Constructing low-dimensional embeddings (Classical MDS)...');

%%%%% Remove outliers from graph %%%%%
disp('  Checking for outliers...');

if ((mode == 1) & (use_dijk == 0))
     [tmp, firsts] = min(D==INF);     %% first point each point connects to
else
     [tmp, firsts] = min(D==inf);     %% first point each point connects to
end
[comps, I, J] = unique(firsts);    %% first point in each connected component
n_comps = length(comps);           %% number of connected components
size_comps = sum((repmat(firsts,n_comps,1)==((1:n_comps)'*ones(1,N)))');
                                   %% size of each connected component
[tmp, comp_order] = sort(size_comps);  %% sort connected components by size
comps = comps(comp_order(end:-1:1));
size_comps = size_comps(comp_order(end:-1:1));
if (comp>n_comps)
     comp=1;                              %% default: use largest component
end
Y.index = find(firsts==comps(comp)); %% list of points in relevant component
Y.index = setdiff(Y.index,find(isinf(min(D)))); %% prune points that don't connect
                                                %% to any landmarks
N = length(Y.index);
[tmp, landmarks, land_ind] = intersect(landmarks,Y.index);
                                       %% list of landmarks in component
nl = length(landmarks);
D = full(D(landmarks,Y.index))';
disp(['    Number of connected components in graph: ' num2str(n_comps)]);
disp(['    Embedding component ' num2str(comp) ' with ' num2str(length(Y.index)) ' points.']);

dims = unique(min(dims,nl-1));    %% don't embed in more dimensions than landmarks-1
if (nl==N)
     opt.disp = 0;
     [vec, val] = eigs(-.5*(D.^2 - sum(D.^2)'*ones(1,N)/N - ones(N,1)*sum(D.^2)/N + sum(sum(D.^2))/(N^2)), max(dims), 'LR', opt);
else
     subB = -.5*(D.^2 - sum(D'.^2)'*ones(1,nl)/nl - ones(N,1)*sum(D.^2)/N+sum(sum(D.^2))/(N*nl));
     opt.disp = 0;
     [alpha,beta] = eigs(subB'*subB, max(dims), 'LR', opt);
     val = beta.^(1/2);
     vec = subB*alpha*inv(val);
     clear subB alpha beta;
end
h = real(diag(val));
[foo,sorth] = sort(h);  sorth = sorth(end:-1:1);
val = real(diag(val(sorth,sorth)));
vec = vec(:,sorth);

D = reshape(D,N*nl,1);
for di = 1:length(dims)
     Y.coords{di} = real(vec(:,1:dims(di)).*(ones(N,1)*sqrt(val(1:dims(di)))'))';
     r2 = 1-corrcoef(reshape(real(L2_distance(Y.coords{di}, Y.coords{di}(:,land_ind))),N*nl,1),D).^2;
     R(di) = r2(2,1);
     if (verbose == 1)
         disp(['  Isomap on ' num2str(N) ' points with dimensionality ' num2str(dims(di)) '  --> residual variance = ' num2str(R(di))]);
     end
end

clear D;

%%%%%%%%%%%%%%%%%% Graphics %%%%%%%%%%%%%%%%%%

if (displ==1)
     %%%%% Plot fall-off of residual variance with dimensionality %%%%%
     figure;
     hold on
     plot(dims, R, 'bo');
     plot(dims, R, 'b-');
     hold off
     ylabel('Residual variance');
     xlabel('Isomap dimensionality');

     %%%%% Plot two-dimensional configuration %%%%%
     twod = find(dims==2);
     if ~isempty(twod)
         figure;
         hold on;
         plot(Y.coords{twod}(1,:), Y.coords{twod}(2,:), 'ro');
         if (overlay == 1)
             gplot(E(Y.index, Y.index), [Y.coords{twod}(1,:); Y.coords{twod}(2,:)]');
             title('Two-dimensional Isomap embedding (with neighborhood graph).');
         else
             title('Two-dimensional Isomap.');
         end
         hold off;
     end
end

return;


text:
function [ output_args ] = gmm_hu(I,M,options)
%GMM_HU Summary of this function goes here
%   Detailed explanation goes here
beta1 = 1e4;
beta2 = 0; % 2e4
eta = 0.05;

rho1 = 1e4;

if isstruct(options)
    arg_set = fieldnames(options);
    for i = 1:length(arg_set)
        eval([arg_set{i},'=options.',arg_set{i},';']);
    end
end

names = cell(1,M);
for i = 1:M
    names{i} = ['endmember ',num2str(i)];
end

%% initialize parameters
[Y,~,rows,cols] = reshape_hsi(I,[]);
[N,B] = size(Y);

beta1 = beta1*B/M;
beta2 = beta2*B/M;
rho1 = rho1*N/M^2;

if beta1 < 1e-9
    beta1 = 1e-9;
end

[W,Neighbors] = image2graph(I,eta,1e-9);

D = diag(sum(W,2));
L = D - W;
L = sparse(L);
KL = L - beta2/beta1*speye(N);

W = ones(M,M);
D = diag(sum(W,2));
H = D - W;

[mu_jk,sigma_jk,w_jk,K,A] = gmm_init(I,M);

%% Create all the k indices
% K = [1 2 3 1];
K_inds  = cell(1,M);
for j = 1:M
    K_inds{j} = (1:K(j));
end
K_all = cartprod(K_inds{:});
K1 = size(K_all,1);

%% iterate by MM (EM)
w_k = w_jk2w_k(w_jk,K_all);

sigma = Y_noise;

I_B = eye(B);

delta_t0 = 1e-12;

delta_t_mu = delta_t0;
delta_t_sigma = delta_t0;
delta_t_A = delta_t0;

der_mu0 = mu_jk;
der_sigma0 = sigma_jk;
der_A0 = zeros(N,M);
for j = 1:length(der_mu0)
    der_mu0{j}(:,:) = 0;
    der_sigma0{j}(:,:,:) = 0;
end

s = [];
s.N = N;
s.B = B;
s.K1 = K1;
s.sigma = sigma;
s.K_all = K_all;
s.Y = Y;
s.beta1 = beta1;
s.KL = KL;

s.A = A;
s.mu_jk = mu_jk;
s.sigma_jk = sigma_jk;
s.w_k = w_k;

Is = (1:N*B);
Is = repmat(reshape(Is, [B,1,N]), 1, B);
Is = Is(:);

Js = (1:N*B);
Js = repmat(reshape(Js, [1,B,N]), B, 1);
Js = Js(:);

s.Is = Is;
s.Js = Js;

max_iter = 200;
eval_totals = zeros(max_iter, 1);
eval_Ns = zeros(max_iter, 1);
eval_As = zeros(max_iter, 1);
eval_Rs = zeros(max_iter, 1);

for iter = 1:200
    %% E step
    % update gamma_nk

    N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, s);

    gamma_nk = (ones(N,1)*w_k) .* N_nk;
    gamma_nk = gamma_nk ./ repmat(sum(gamma_nk,2), 1, K1);
    s.gamma_nk = gamma_nk;

    %% M step
    % update w_k
    w_k = sum(gamma_nk, 1) / N;
    s.w_k = w_k;

    delta_t = delta_t0;
    disp('Process M step optimization.');

    % update mu
    der_mu = calc_der_mu(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y);
    s.der_mu = der_mu;
    delta_t_mu = calc_time_step_adaptive(@eval_obj_fun_mu, @update_mu, ...
        mu_jk, s, delta_t_mu, delta_t0);
    mu_jk = update_mu(mu_jk, s, delta_t_mu);
    s.mu_jk = mu_jk;

    % update sigma
    der_sigma = calc_der_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y);
    s.der_sigma = der_sigma;
    delta_t_sigma = calc_time_step_adaptive(@eval_obj_fun_sigma, @update_sigma, ...
        sigma_jk, s, delta_t_sigma, delta_t0);
    sigma_jk = update_sigma(sigma_jk, s, delta_t_sigma);
    s.sigma_jk = sigma_jk;

    % update A
    der_A = calc_der_A(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, KL, beta1);
    s.der_A = der_A;
    delta_t_A = calc_time_step_adaptive(@eval_obj_fun_A, @update_A, ...
        A, s, delta_t_A, delta_t0);
    A = update_A(A, s, delta_t_A);
    s.A = A;

    if mod(iter,40) == 0
        endmember_scatter_plot_end_var(Y,w_jk,mu_jk,sigma_jk,names);
        set(gcf,'name',['Scatter plot of iteration ',num2str(iter)]);
        show_abundances(A,size(I,1),size(I,2));
        set(gcf,'name',['Abundances of iteration ',num2str(iter)]);
        pause(0.01);
    end

    [eval_total, eval_N, eval_A, eval_R] = calc_obj_fun(A, ...
        sigma, mu_jk, sigma_jk, K_all, Y, KL, beta1, w_k, s);
    eval_totals(iter) = eval_total;
    eval_Ns(iter) = eval_N;
    eval_As(iter) = eval_A;
    eval_Rs(iter) = eval_R;

    disp(['EM iteration ', num2str(iter)]);

end

w_jk = w_k2w_jk(w_k, K_all);

show_abundances(A,size(I,1),size(I,2));
figure('name', 'Total objective function value vs iteration number');
plot(eval_totals);

figure('name', 'Data fidelity term value vs iteration number');
plot(eval_Ns);

figure('name', 'Abundance smoothness term value vs iteration number');
plot(eval_As);

figure('name', 'Endmembers clossness term value vs iteration number');
plot(eval_Rs);

function [eval_total, eval_N, eval_A, eval_R] = calc_obj_fun(A, ...
    sigma, mu_jk, sigma_jk, K_all, Y, KL, beta1, w_k, options)
N = size(A,1);

N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, options);
eval_N = -sum(log(sum((ones(N,1)*w_k) .* N_nk, 2)));
eval_A = (beta1/2)*trace(A'*KL*A);
eval_R = 0;
eval_total = eval_N + eval_A + eval_R;

function N_nk = calc_gaussians(A, sigma, mu_jk, sigma_jk, K_all, Y, options)
[N,B] = size(Y);
K1 = size(K_all,1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% old implementation

% N_nk1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)'-mu_nk(:,:,n,k);
%         N_nk1(n,k) = det(sigma_nk(:,:,n,k))^(-1/2) * ...
%             exp(-0.5 * y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk));
%     end
% end
% N_nk1 = N_nk1 / (2*pi)^(B/2);

% new implementation

N_nk = zeros(N,K1);
for k = 1:K1
    y = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), options);
    N_nk(:,k) = exp(y);
end

% norm(N_nk - N_nk1) / norm(N_nk)

function val = eval_obj_fun_mu(params, options)
mu_jk = params;

A = options.A;
sigma = options.sigma;
% mu_jk = options.mu_jk;
sigma_jk = options.sigma_jk;
K_all = options.K_all;
gamma_nk = options.gamma_nk;
Y = options.Y;
rho1 = options.rho1;
w_k = options.w_k;
% beta1 = options.beta1;
% KL = options.KL;
H = options.H;

[N,B] = size(Y);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);

val2 = zeros(1,K1);
for k = 1:K1
    Y1 = (Y - mu_nk(:,:,k))';
    Y2 = Y1 .* repmat(gamma_nk(:,k)', B, 1);
    sigma_k = block_diag(sigma_nk(:,:,:,k), options);
    val2(k) = Y1(:)' * (sigma_k \ Y2(:));
end
val = sum(sum(0.5 * val2));

% tic
% N_nk = zeros(N,K1);
% for k = 1:K1
%     N_nk(:,k) = logmvn(Y, reshape(mu_nk(:,:,:,k),B,N)', sigma_nk(:,:,:,k), options);
% end
%
% val3 = -sum(sum(0.5 * gamma_nk .* N_nk));
% toc

val_mu = 0;
[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);
for k = 1:K1
    val_mu = val_mu + w_k(k) * trace(mu_all(:,:,k) * H * mu_all(:,:,k)');
end
val_mu = rho1 / 2 * val_mu;

val = val + val_mu;

function val = eval_obj_fun_sigma(params, s)
sigma_jk = params;

A = s.A;
sigma = s.sigma;
mu_jk = s.mu_jk;
% sigma_jk = options.sigma_jk;
K_all = s.K_all;
gamma_nk = s.gamma_nk;
Y = s.Y;
% beta1 = options.beta1;
% KL = options.KL;

N = size(A,1);
K1 = size(K_all,1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% val1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
%         val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
%     end
% end
%
% val = sum(sum(0.5 * gamma_nk .* val1));

N_nk = zeros(N,K1);
for k = 1:K1
    N_nk(:,k) = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), s);
end

val = -sum(sum(0.5 * gamma_nk .* N_nk));

function val = eval_obj_fun_A(params, options)
A = params;

% A = options.A;
sigma = options.sigma;
mu_jk = options.mu_jk;
sigma_jk = options.sigma_jk;
K_all = options.K_all;
gamma_nk = options.gamma_nk;
Y = options.Y;
beta1 = options.beta1;
KL = options.KL;

N = size(A,1);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% val1 = zeros(N,K1);
% for n = 1:N
%     for k = 1:K1
%         y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
%         val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
%     end
% end

% val = sum(sum(0.5 * gamma_nk .* val1)) + beta1/2 * trace(A'*KL*A);

N_nk = zeros(N,K1);
for k = 1:K1
    N_nk(:,k) = logmvn(Y, mu_nk(:,:,k), sigma_nk(:,:,:,k), options);
end

val = -sum(sum(0.5 * gamma_nk .* N_nk)) + beta1/2 * trace(A'*KL*A);

function der_mu = calc_der_mu(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
lambda_nk = calc_lambda_nk(gamma_nk,mu_nk,sigma_nk,Y);

% calculate der_mu
% tic
% der_mu = mu_jk;
% for j = 1:M
%     for k = 1:K(j)
%         lambda_sum = sum(lambda_nk(:,:,K_all(:,j)==k), 3);
%         lambda_alpha = repmat(A(:,j), [1,B]);
%         der_mu{j}(k,:) = -sum(lambda_sum .* lambda_alpha, 1);
%     end
% end
% toc

% tic
der_mu = mu_jk;
temp = zeros(M,B,K1);
for k = 1:K1
    temp(:,:,k) = A' * lambda_nk(:,:,k);
end

for j = 1:M
    for k = 1:K(j)
        der_mu{j}(k,:) = -sum(temp(j,:,K_all(:,j)==k), 3)';
    end
end
% toc

% mdif(der_mu{1},der_mu1{1});

function der_sigma = calc_der_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
[~,B] = size(Y);
K = max(K_all,[],1);
K1 = size(K_all,1);

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);
[lambda_nk,psi_nk] = calc_lambda_psi_nk(gamma_nk,mu_nk,sigma_nk,prec,Y);

% calculate der_mu, der_sigma, der_A
% tic
% der_sigma1 = sigma_jk;
% for j = 1:M
%     for k = 1:K(j)
%         psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
%         psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
%         der_sigma1{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);
%     end
% end
% toc

% tic
psi_k = reshape(psi_nk, [B*B,N,K1]);
der_sigma = sigma_jk;
temp = zeros(M,B*B,K1);
for k = 1:K1
    temp(:,:,k) = (A.^2)' * psi_k(:,:,k)';
end

for j = 1:M
    for k = 1:K(j)
        der_sigma{j}(:,:,k) = reshape(-sum(temp(j,:,K_all(:,j)==k), 3)', B, B);
    end
end
% toc

% disp('');

function lambda_nk = calc_lambda_nk(gamma_nk,mu_nk,sigma_nk,Y)
[N,B,K1] = size(mu_nk);

% tic
% lambda_nk1 = zeros(N,B,K1);
% for n = 1:N
%     for k = 1:K1
%         lambda_nk1(n,:,k) = gamma_nk(n,k) * ...
%             (sigma_nk(:,:,n,k) \ (Y(n,:)' - mu_nk(n,:,k)'));
%     end
% end
% toc

% tic
lambda_nk = zeros(N,B,K1);
for k = 1:K1
    Y1 = Y - mu_nk(:,:,k);
    Y1 = Y1';
    Y2 = Y1 .* repmat(gamma_nk(:,k)', B, 1);
    sigma_k = block_diag(sigma_nk(:,:,:,k));
    lambda_nk(:,:,k) = reshape(sigma_k \ Y2(:), [B,N])';
end
% toc

function [lambda_nk,psi_nk] = calc_lambda_psi_nk(A, sigma, mu_jk, ...
    sigma_jk, K_all, gamma_nk, Y)
[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

[N,B,K1] = size(mu_nk);

% tic
% lambda_nk1 = zeros(N,B,K1);
% psi_nk1 = zeros(B,B,N,K1);
%
% for k = 1:K1
%     Y1 = Y - mu_nk(:,:,k);
%     for n = 1:N
%         sigma_y_n_mu_nk = prec(:,:,n,k) * Y1(n,:)';
%         lambda_nk1(n,:,k) = gamma_nk(n,k) * sigma_y_n_mu_nk;
%         psi_nk1(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
%             sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
%     end
% end
% toc

% tic
lambda_nk = zeros(N,B,K1);
psi_nk = zeros(B,B,N,K1);

for k = 1:K1
    Y1 = Y - mu_nk(:,:,k);
    sigma_y_n_mu_nk = multiprod(prec(:,:,:,k), Y1', [1 2], [1]);
    lambda_nk(:,:,k) = (repmat(gamma_nk(:,k)', B, 1) .* sigma_y_n_mu_nk)';

    tmp1 = reshape(sigma_y_n_mu_nk, [B 1 N]);
    tmp2 = reshape(sigma_y_n_mu_nk, [1 B N]);
    tmp3 = multiprod(tmp1, tmp2, [1 2], [1 2]) - prec(:,:,:,k);
    psi_nk(:,:,:,k) = 0.5 * multiprod(reshape(gamma_nk(:,k), [1 1 N]), tmp3);
end
% toc
% mdiff(lambda_nk,lambda_nk1);
% mdiff(psi_nk,psi_nk1);

function der_A = calc_der_A(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, ...
    Y, KL, beta1)
[N,K1] = size(gamma_nk);
M = length(mu_jk);
B = size(mu_jk{1},2);

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);
[lambda_nk,psi_nk] = calc_lambda_psi_nk(gamma_nk,mu_nk,sigma_nk,prec,Y);

% calculate der_mu, der_sigma, der_A
% tic
%
% der_A1 = zeros(N,M);
% for n = 1:N
%     for j = 1:M
%         for k = 1:K1
%             der_A1(n,j) = der_A1(n,j) - sum(lambda_nk(n,:,k) .* mu_jk{j}(K_all(k,j),:)) ...
%                 - 2 * A(n,j) * sum(sum(psi_nk(:,:,n,k) .* sigma_jk{j}(:,:,K_all(k,j))));
%         end
%     end
% end
% toc

% tic
[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);
psi_k = reshape(psi_nk, [B*B,N,K1]);

der_A = zeros(N,M);
temp1 = zeros(N,M);
for k = 1:K1
    der_A = der_A - lambda_nk(:,:,k) * mu_all(:,:,k);
    temp1 = temp1 - psi_k(:,:,k)' * sigma_all(:,:,k);
end
der_A = der_A + 2 * A .* temp1;
% toc

der_A = der_A + beta1 * KL * A;

function mu_jk_new = update_mu(mu_jk, options, delta_t)
der_mu = options.der_mu;

mu_jk_new = mu_jk;

for j = 1:length(mu_jk)
    mu_jk_new{j} = mu_jk{j} - delta_t * der_mu{j};
end

function sigma_jk_new = update_sigma(sigma_jk, options, delta_t)
der_sigma = options.der_sigma;

sigma_jk_new = sigma_jk;

for j = 1:length(sigma_jk)
    sigma_jk_new{j} = sigma_jk{j} - delta_t * der_sigma{j};
%     sigma_jk_new{j} = sigma_jk{j};
    for k = 1:size(sigma_jk{j},3)
        cov_mat = sigma_jk_new{j}(:,:,k);
        [V,D] = eig((cov_mat + cov_mat')/2);
        d = diag(D);
        if (min(d) < 0)
            disp('There is a negative eigenvalue in the updated sigma_jk');
            d(d<0) = 1e-6 * max(abs(d));
            D = diag(d);
        end
        sigma_jk_new{j}(:,:,k) = V*D*V';
    end
end

function A_new = update_A(A, options, delta_t)
der_A = options.der_A;

A_new = A - delta_t * der_A;
A_new = project_to_simplex(A_new);

function [mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all)
[N,M] = size(A);
K1 = size(K_all,1);
B = size(mu_jk{1},2);

[mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all);

sigma2_I = sigma^2 * eye(B);

% old implementation
% mu_nk1 = zeros(B,1,N,K1);
% sigma_nk1 = zeros(B,B,N,K1);
% for n = 1:N
%     for k = 1:K1
%         mu_nk1(:,:,n,k) = mu_all(:,:,k) * A(n,:)';
%         sigma_nk1(:,:,n,k) = reshape(sigma_all(:,:,k) * (A(n,:).^2)', B, B) + sigma2_I;
%     end
% end

% new implementation is 10 times faster
mu_nk = zeros(N,B,K1);
sigma_nk = zeros(B*B,N,K1);
for k = 1:K1
    mu_nk(:,:,k) = A * mu_all(:,:,k)';
    sigma_nk(:,:,k) = sigma_all(:,:,k) * (A.^2)' + repmat(sigma2_I(:),1,N);
end
% mu_nk = reshape(mu_nk,[B,1,N,K1]);
sigma_nk = reshape(sigma_nk,[B,B,N,K1]);

function [mu_all,sigma_all] = calc_mu_sigma_all(mu_jk, sigma_jk, K_all)
[~,B] = size(mu_jk{1});
[K1,M] = size(K_all);

mu_all = zeros(B,M,K1);
sigma_all = zeros(B*B,M,K1);
for i = 1:K1
    for j = 1:M
        mu_all(:,j,i) = mu_jk{j}(K_all(i,j),:)';
        sigma_all(:,j,i) = reshape(sigma_jk{j}(:,:,K_all(i,j)), B*B, 1);
    end
end

function [mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all)
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
% [N,B,K1] = size(mu_nk);
% tic
% prec1 = zeros(size(sigma_nk));
% for n = 1:N
%     for k = 1:K1
%         prec1(:,:,n,k) = inv(sigma_nk(:,:,n,k));
%     end
% end
% toc
%
% tic
prec = multinv(sigma_nk);
% toc
%
% mdiff(prec,prec1);

% tic
% prec1 = cell(1,K1);
% for k = 1:K1
%     sigma_k = block_diag(sigma_nk(:,:,:,k), options);
%     R = chol(sigma_k);
%     S = inv(R);
%     prec1{k} = S*S';
% end
%
% prec2 = zeros(B,B,N,K1);
% for k = 1:K1
%     [~,~,S] = find(prec1{k});
%     prec2(:,:,:,k) = reshape(S, [B,B,N]);
% end
%
% toc
%
% mdif(prec,prec2)

function w_k = w_jk2w_k(w_jk, K_all)
w_k = ones(1, size(K_all,1));
for i = 1:size(K_all,1)
    k = K_all(i,:);
    for j = 1:length(w_jk)
        w_k(i) = w_k(i)*w_jk{j}(k(j));
    end
end

function w_jk = w_k2w_jk(w_k, K_all)
M = size(K_all,2);
w_jk = cell(1,M);
K = zeros(1,M);
for j = 1:M
    K(j) = max(K_all(:,j));
    w_jk{j} = zeros(1,K(j));
end

for j = 1:M
    for l = 1:K(j)
        w = w_k(K_all(:,j)==l);
        w_jk{j}(l) = sum(w);
    end
end

%% obsolete
function delta_t = calc_time_step(A, sigma, mu_jk, sigma_jk, K_all, ...
    gamma_nk, Y, delta_t, delta_t0, der_mu, der_sigma, der_A, beta1, KL)
val_ori = eval_obj_fun_M(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, beta1, KL);
[mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, der_mu, der_sigma, der_A, delta_t);
val_new = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);

if val_new < val_ori
    val_old = val_new;
    while 1
        delta_t = delta_t * 10;
        [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
            der_mu, der_sigma, der_A, delta_t);
        val = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);
        if val < val_old
            val_old = val;
        else
            delta_t = delta_t / 10;
            break;
        end
    end
else
    while delta_t > delta_t0
        delta_t = delta_t / 10;
        [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
            der_mu, der_sigma, der_A, delta_t);
        val = eval_obj_fun_M(A_new, sigma, mu_jk_new, sigma_jk_new, K_all, gamma_nk, Y, beta1, KL);
        if val < val_ori
            break;
        end
    end
end

function [mu_jk_new, sigma_jk_new, A_new] = update_params(mu_jk, sigma_jk, A, ...
    der_mu, der_sigma, der_A, delta_t)
mu_jk_new = mu_jk;
sigma_jk_new = sigma_jk;

for j = 1:length(mu_jk)
    mu_jk_new{j} = mu_jk{j} - delta_t * der_mu{j};
    sigma_jk_new{j} = sigma_jk{j} - delta_t * der_sigma{j};
%     sigma_jk_new{j} = sigma_jk{j};
    for k = 1:size(sigma_jk{j},3)
        cov_mat = sigma_jk_new{j}(:,:,k);
        [V,D] = eig((cov_mat + cov_mat')/2);
        d = diag(D);
        if (min(d) < 0)
            disp('There is a negative eigenvalue in the updated sigma_jk');
            d(d<0) = 1e-6 * max(abs(d));
            D = diag(d);
        end
        sigma_jk_new{j}(:,:,k) = V*D*V';
    end
end

A_new = A - delta_t * der_A;
A_new = project_to_simplex(A_new);

function val = eval_obj_fun_M(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y, beta1, KL)
N = size(A,1);
K1 = size(K_all,1);
[mu_nk,sigma_nk] = calc_mu_sigma_nk(A, sigma, mu_jk, sigma_jk, K_all);
val1 = zeros(N,K1);
for n = 1:N
    for k = 1:K1
        y_n_mu_nk = Y(n,:)' - mu_nk(:,:,n,k);
        val1(n,k) = log(det(sigma_nk(:,:,n,k))) + y_n_mu_nk' * (sigma_nk(:,:,n,k) \ y_n_mu_nk);
    end
end

val = sum(sum(0.5 * gamma_nk .* val1)) + beta1/2 * trace(A'*KL*A);

function [der_mu, der_sigma, der_A] = calc_derivatives(A, sigma, mu_jk, ...
    sigma_jk, K_all, gamma_nk, Y, beta1, KL)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

der_mu = mu_jk;
der_sigma = sigma_jk;

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

lambda_nk = zeros(B,1,N,K1);
psi_nk = zeros(B,B,N,K1);
der_A = zeros(N,M);
for n = 1:N
    for k = 1:K1
        sigma_y_n_mu_nk = prec(:,:,n,k) * (Y(n,:)' - mu_nk(:,:,n,k));
        lambda_nk(:,:,n,k) = gamma_nk(n,k) * sigma_y_n_mu_nk;
        psi_nk(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
            sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
    end
end
% calculate der_mu, der_sigma, der_A
for j = 1:M
    for k = 1:K(j)
        lambda_sum = sum(lambda_nk(:,:,:,K_all(:,j)==k), 4);
        lambda_alpha = reshape(repmat(A(:,j)', [B,1]), [B,1,N]);
        der_mu{j}(k,:) = -sum(lambda_sum .* lambda_alpha, 3);

        psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
        psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
        der_sigma{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);
    end
end

for n = 1:N
    for j = 1:M
        for k = 1:K1
            der_A(n,j) = der_A(n,j) - sum(lambda_nk(:,:,n,k) .* mu_jk{j}(K_all(k,j),:)') ...
                - 2 * A(n,j) * sum(sum(psi_nk(:,:,n,k) .* sigma_jk{j}(:,:,K_all(k,j))));
        end
    end
end
der_A = der_A + beta1 * KL * A;

function [der_mu, der_sigma] = calc_derivative_sigma(A, sigma, mu_jk, sigma_jk, K_all, gamma_nk, Y)
[N,M] = size(A);
B = size(mu_jk{1},2);
K1 = size(K_all,1);
K = max(K_all,[],1);

der_mu = mu_jk;
der_sigma = sigma_jk;

for j = 1:M
    der_mu{j} = zeros(size(der_mu{j}));
end

[mu_nk,sigma_nk,prec] = calc_mu_sigma_prec(A, sigma, mu_jk, sigma_jk, K_all);

psi_nk = zeros(B,B,N,K1);
for n = 1:N
    for k = 1:K1
        sigma_y_n_mu_nk = prec(:,:,n,k) * (Y(n,:)' - mu_nk(:,:,n,k));
        psi_nk(:,:,n,k) = 0.5 * gamma_nk(n,k) * ( ...
            sigma_y_n_mu_nk * sigma_y_n_mu_nk' - prec(:,:,n,k));
    end
end
% calculate der_sigma
for j = 1:M
    for k = 1:K(j)
        psi_sum = sum(psi_nk(:,:,:,K_all(:,j)==k), 4);
        psi_alpha = reshape(repmat(A(:,j)'.^2, [B*B,1]), [B,B,N]);
        der_sigma{j}(:,:,k) = -sum(psi_sum .* psi_alpha, 3);

%         temp = zeros(B,B);
%         for n = 1:N
%             for k1 = 1:K1
%                 if k == K_all(k1,j)
%                     temp = temp - A(n,j)^2 * psi_nk(:,:,n,k1);
%                 end
%             end
%         end
    end
end

author: zhouyuanzxcv

repo: Hyperspectral


text:
%% Options
%
%%
%
% Many functions provided by MTEX can be customized by options. An option
% is passed to a method as a string parameter followed by a value. For
% example, almost all plotting methods support the option *resolution*
% followed by a double value specifying the resolution in radians.  The code below
% demonstrates the effect of changing this parameter.

odf = SantaFe
plotPDF(odf,Miller(1,0,0,odf.CS),'resolution',10*degree,'contour','linewidth',2);

%%

plotPDF(odf,Miller(1,0,0,odf.CS),'resolution',2.5*degree,'contour','linewidth',2);

%%
% Options that are not followed by a value are called flags. In the above
% example, *contour* is a flag that tells the plotting routine to plot
% contour lines. Options and flags to a function are always optional and
% can be passed in any order. If conflicting options or flags are passed,
% i.e., the resolution is specified twice, the later option in the list is
% considered to be the right one.
%


function EXP_A = get_SV_matrix_exponential( Q, dt, xi, v1, v2, fv, psi_J, m_0, N )

Qt = dt*Q';
EXP_A = ones(m_0,m_0,N);
for j = 1:N
    EXP_A(:,:,j) = Qt + diag(v1*xi(j) - v2*xi(j)^2 + dt*psi_J(xi(j)));  %This incorporates the dt
end

%%% Define Toeplitz matrix (note the special structure:
%%% .. Before exponentiation it is: Toeplitz + skew-symmetric + zero diagonal
Lambda_dxi = zeros(m_0,m_0);

for k=1:m_0
    for j = k+1:m_0
        Lambda_dxi(k,j) = fv(k) - fv(j);
        Lambda_dxi(j,k) = -Lambda_dxi(k,j);
    end
end

Lambda_dxi = exp(Lambda_dxi); %note: not matrix exponential

EXP_A(:,:,1) = expm(EXP_A(:,:,1));  %matrix exponential
%%% (note: for n=1, Lambda is matrix of ones, so we start next loop at n=2)

Lambda = Lambda_dxi; %initialize Lambda for n = 2

for j = 2:N
    EXP_A(:,:,j) = expm(EXP_A(:,:,j)).*Lambda;  %matrix exponential
    Lambda = Lambda.*Lambda_dxi; %update Lambda
end

end


text:
%Figure 1.1       Feedback Control of Dynamic Systems, 5e
%                      Franklin, Powell, Emami

% script to generate figure 1.1
clf
sim('fig1_01bsim',16)
plot(roomtemp(:,1),roomtemp(:,2))
grid on;
axis([0 16 0 70]);
title('Figure 1.1(b) Plot of room temperature and thermostat setting')
hold on
plot(heatin(:,1),10*heatin(:,2))
x=[0 16];
y=[50 50];
plot(x,y)
xlabel('Time (hours)');
ylabel('Temperature (degrees F)');
gtext('Room Temperature')
gtext ('Outside Temperature')
gtext(' Furnace on')
gtext(' Furnace off')


text:
function this = alt_ba_optical_flow(varargin)
%
%ALT_BA_OPTICAL_FLOW
%
%   ALT_BA_OPTICAL_FLOW([IMGS]) constructs a ALT_BA optical flow object
%   with the optional image sequence IMGS ([n x m x 2] array).
%   ALT_BA_OPTICAL_FLOW(O) constructs BA optical flow object by copying O.
%
%   This is a member function of the class 'alt_ba_optical_flow'.
%
% Authors: Deqing Sun, Department of Computer Science, Brown University
% Contact: dqsun@cs.brown.edu
% $Date: 2009 $
% $Revision: $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

error(nargchk(0, 1, length(varargin)));

  switch (length(varargin))
    case 0

      this.images          = [];
      this.lambda          = 5;
      this.lambda_q        = 5;    % Quadratic formulation of the objective function

      this.sor_max_iters   = 1e4;       % 100 seems sufficient

      this.limit_update    = true;      % limit the flow incrment to be less than 1 per linearization step
      this.display         = false;

      this.solver          = 'backslash';   % 'sor' 'pcg' for machines with limited moemory
      this.warping_mode    = 'backward';

      this.texture              = false;     % use texture component as input
      this.deriv_filter         = [1 -8 0 8 -1]/12; % 5-point 7 point [-1 9 -45 0 45 -9 1]/60;
      this.median_filter_size   = []; %[5 5];
      this.interpolation_method = 'cubic';  % 'bi-cubic', 'cubic', 'bi-linear'

      % For Graduated Non-Convexity (GNC) optimization
      this.gnc_iters       = 3;
      this.alpha           = 1;             % change linearly from 1 to 0 through the GNC stages

      this.max_iters       = 10;            % number of warping per pyramid level
      this.max_linear      = 1;             % maximum number of linearization performed per warping, 1 OK for HS

      % For GNC stage 1
      this.pyramid_levels  = 4;
      this.pyramid_spacing = 2;

      % For GNC stage 2 to end
      this.gnc_pyramid_levels     = 2;
      this.gnc_pyramid_spacing    = 1.25;

      method = 'lorentzian'; %'geman_mcclure'; %
      this.spatial_filters = {[1 -1], [1; -1]};
      for i = 1:length(this.spatial_filters);
          this.rho_spatial_u{i}   = robust_function(method, 0.03); % 0.1
          this.rho_spatial_v{i}   = robust_function(method, 0.03);
      end;
      this.rho_data        = robust_function(method, 1.5); % 6.3

      this.seg             = [];    % sore segementatio result
      this.mfT             = 15;    % threshold for intensity-median-filter
      this.imfsz           = [7 7]; % for intensity-median-filter
      this.qterm            = true;   % true: use the qterm
      this.lambda2         = 1e-1;      % weight for coupling term
      this.lambda3         = 1;         % weight for non local term term
      this.weightRatio     = 1;     % lambdaA/weight on the forth term
      this.itersLO         = 1;     % # Li & Osher iterations
      this.color_images     = [];
      this.replacment       = true;

%       this.rho_couple       = robust_function('quadratic', 1);      % penalty function for coupling term
      this.rho_couple       = robust_function('charbonnier', 1e-3);      % penalty function for coupling term

      this = class(this, 'alt_ba_optical_flow');

    case 1
      if isa(varargin{1}, 'alt_ba_optical_flow')
        this = varargin{1};
      else
          this = alt_ba_optical_flow;
          this.images = varargin{1};
      end

    otherwise
      error('Incompatible arguments!');

  end

author: kristinbranson

repo: JAABA


text:
function t = uplus(t)
%UPLUS Unary plus for a ktensor.
%
%   See also KTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

% This function does nothing!

repo: mtt

save_path: github-repos/MATLAB/andrewssobral-mtt


text:
function v = tfocs_normsq( x, scaling )

% TFOCS_NORMSQ    Squared norm.
%    By default, TFOCS_NORMSQ(X) = TFOCS_DOT(X,X). However, certain
%    objects may have more efficient ways of computing this value.
%    If so, TFOCS_NORMSQ should be overloaded to take advantage of
%    this. However, the numerical equivalence to TFOCS_DOT(X,X) must
%    be preserved.
%
%   TFOCS_NORMSQ( X, D ) = TFOCS_DOT( X, D.*X ) is a scaled norm

if nargin < 2 || isempty(scaling)
    v = tfocs_dot( x, x );
elseif numel(scaling) == 1
    v = scaling*tfocs_dot( x, x );
else
    v = tfocs_dot( x, scaling.*x );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.

author: cvxr

repo: TFOCS

save_path: github-repos/MATLAB/cvxr-TFOCS


text:
% Figures 6.8-6.10: Quadratic smoothing
% Section 6.3.3
% Boyd & Vandenberghe "Convex Optimization"
% Original by Lieven Vandenberghe
% Adapted for CVX Argyris Zymnis - 10/2005
%
% Suppose we have a signal x, which does not vary too rapidly
% and that x is corrupted by some small, rapidly varying noise v,
% ie. x_cor = x + v. Then if we want to reconstruct x from x_cor
% we should solve (with x_hat as the parameter)
%        minimize ||x_hat - x_cor||_2 + lambda*phi_quad(x_hat)
%
% where phi_quad(x) = sum(x_(i+1)-x_i)^2 , for i = 1 to n-1.
% The parameter lambda controls the ''smoothness'' of x_hat.
%
% The first figure which is generated shows the original and
% the corrupted signals. The second figure shows the tradeoff curve
% obtained when varying lambda and the third figure shows three
% reconstructed signals.
%
% NOTE: This is not a good problem to use CVX on. By exploiting
% the sparsity in this case, we can solve this problem much more
% efficiently using least squares.

randn('state',0);

n = 4000;  t = (0:n-1)';
exact = 0.5*sin((2*pi/n)*t).*sin(0.01*t);
corrupt = exact + 0.05*randn(size(exact));

figure(1)
subplot(211)
plot(t,exact,'-');
axis([0 n -0.6 0.6])
title('original signal');
ylabel('ya');

subplot(212)
plot(t,corrupt,'-');
axis([0 n -0.6 0.6])
xlabel('x');
ylabel('yb');
title('corrupted signal');
%print -deps smoothrec_signals.eps % figure 6.8, page 313

A = sparse(n-1,n);
A(:,1:n-1) = -speye(n-1,n-1);  A(:,2:n) = A(:,2:n)+speye(n-1,n-1);

% tradeoff curve, figure 6.9, page 313
nopts = 100;
lambdas = logspace(-10,10,nopts);

obj1 = [];  obj2 = [];

fprintf('computing 100 points on tradeoff curve ... \n');

for i=1:nopts

  lambda = lambdas(i);
  cvx_begin quiet
    variable x(n)
    minimize(norm(x-corrupt)+lambda*norm(x(2:n)-x(1:n-1)))
  cvx_end
  obj1 = [obj1, norm(full(A*x))];
  obj2 = [obj2, norm(full(x-corrupt))];

  fprintf('tradeoff point %d\n',i);
end;

figure(2)
plot(obj2,obj1,'-');  hold on;
plot(0,norm(A*corrupt),'o');
plot(norm(corrupt),0,'o');  hold off;
xlabel('x');
ylabel('y');
title('||xhat-xcorr||_2 vs. ||D xhat||_2');
%print -deps smoothrec_tradeoff.eps % figure 6.9, page 313

%three smooth signals, figure 6.10, page 314
nopts = 3;
alphas = [8 3 1];
xrecon = [];

for i=1:3
   fprintf(1,'Reconstructed Signals: %d of 3 \n',i)
   alpha = alphas(i);
   cvx_begin quiet
    variable x(n)
    minimize(norm(x(2:n)-x(1:n-1)))
    subject to
        norm(x-corrupt) <= alpha;
   cvx_end
   xrecon = [xrecon, x];

end

figure(3)
subplot(311), plot(xrecon(:,1));
axis([0 n -0.6 0.6])
ylabel('ya');
title('||xhat-xcorr||_2=8');
subplot(312), plot(xrecon(:,2));
axis([0 n -0.6 0.6])
ylabel('yb');
title('||xhat-xcorr||_2=3');
subplot(313), plot(xrecon(:,3));
axis([0 n -0.6 0.6])
xlabel('x');
ylabel('yc');
title('||xhat-xcorr||_2=1');
%print -deps smoothrec_results.eps % figure 6.10, page 314

repo: CVX

save_path: github-repos/MATLAB/cvxr-CVX