text:
function savedxf(node,face,elem,fname)
%
% savedxf(node,face,elem,fname)
%
% save a surface mesh to DXF format
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2010/04/25
%
% input:
%      node: input, surface node list, dimension (nn,3)
%      face: input, surface face element list, dimension (be,3)
%      elem: input, tetrahedral element list, dimension (ne,4)
%      fname: output file name
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin==2)
   fname=face;
   face=[];
   elem=[];
end

if(nargin==3)
   fname=elem;
   elem=[];
end

fid=fopen(fname,'wt');
if(fid==-1)
    error('You do not have permission to save mesh files.');
end

fprintf(fid,'0\nSECTION\n2\nHEADER\n0\nENDSEC\n0\nSECTION\n2\nENTITIES\n');

if(~isempty(face))
  fprintf(fid,'0\nPOLYLINE\n66\n1\n8\nI2M\n70\n64\n71\n%d\n72\n%d\n',size(node,1),size(face,1));
end

if(~isempty(node))
  node=node(:,1:3);
  fprintf(fid,'0\nVERTEX\n8\nI2M\n10\n%.16f\n20\n%.16f\n30\n%.16f\n70\n192\n',node');
end

if(~isempty(face))
  face=face(:,1:3);
  fprintf(fid,'0\nVERTEX\n8\nI2M\n62\n254\n10\n0.0\n20\n0.0\n30\n0.0\n70\n128\n71\n%d\n72\n%d\n73\n%d\n',face');
end

fprintf(fid,'0\nSEQEND\n0\nENDSEC\n');

if(~isempty(face))
  fprintf(fid,'0\nSECTION\n2\nENTITIES\n0\nINSERT\n8\n1\n2\nMesh\n10\n0.0\n20\n0.0\n30\n0.0\n41\n1.0\n42\n1.0\n43\n1.0\n50\n0.0\n0\nENDSEC\n');
end

fprintf(fid,'0\nEOF');

fclose(fid);


text:
%kvpml 'Estimate Fractal Dimension of Image Based on P(m,L) (K1)'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros vpml.pane file
%
% Parameters:
% InputFile: i 'Input Image ', required: 'input image'
% Integer: l 'Lower window size', default: 3: 'select the initial size of the sliding window'
% Integer: u 'Upper window size', default: 5: 'select the final size of the sliding window'
% Integer: s 'Window step size ', default: 2: 'select the window step size or interval'
% Integer: q 'Range of moments ', default: 3: 'select the range of moments (-r/2 <= 0 >= r/2)'
% OutputFile: o1 'Output Image ', required: 'resulting multiband fractal dimension output image'
% OutputFile: o2 'Output FD Image ', required: 'output image specifying fractal dimension of each class'
% OutputFile: f1 'Output ASCII File', optional: 'output file for P(m,L) statistics'
%
% Example: [o1, o2, f1] = kvpml(i, {'i','';'l',3;'u',5;'s',2;'q',3;'o1','';'o2','';'f1',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% vpml - Estimate Fractal Dimension of Image Based on P(m,L)  (K1)
%
%  DESCRIPTION
% .I vpml
% estimates the fractal dimension of an image based on the probability
% that there are m pixels within a window of size L centered on a pixel
% from a particular class.  For a selected range of window sizes (L),
% the window is centered on the first occurrence of the pixel belonging
% to a particular class.  The number of pixels within a window of size L,
% belonging to a specific class are counted (including the center pixel
% of the window), and a "histogram" is formed as the window is moved over
% the image.  This "histogram" represents the total number of occurrences,
% m, of a class of pixels in a window of size L.  From this, a normalized
% histogram is formed, which yields an estimate of the probability density
% function, P(m,L), for each window size L.
%
% All selected moments (q) of the P(m,L) distributions for each of the
% desired window sizes are determined, and a linear regression or "best fit
% line" is found for the moment generating function, log(M(L))^1/q, versus
% the log of the window size, log(L).  The slope of the "best fit line"
% provides an estimate for the fractal dimension, D.
%
% The center pixel of the largest window, Lmax, is replaced with the computed
% fractal dimension based on the probability density function, P(m,L).
% The largest window size, Lmax, determines the resulting size of the fractal
% dimension image.  This results in a border of size (Lmax / 2) around the
% image, which is set to zero.
%
% The input arguments are described as follows:
%
%  "-i" 15
% specifies the input image, which must be of data type BYTE or INTEGER.
% The input image must be a single band image.
%
%  "-o1" 15
% specifies the output image, which will be a multiband image representing
% the fractal dimension of the input image for each of the specified
% moments.  The resulting output image will be of data storage type
% FLOAT, and will have a border of pixels of value zero.  The size of
% the border will be determined by the size of the largest window
% specified.  This can be determined from the following formulation,
% (Lmax -1) / 2 = border size.  Where Lmax is the size of the largest
% window used.  For example, if the largest window size is 9, then the
% border size will be (9 - 1) / 2 = 4 pixels.
%
%  "-o2" 15
% specifies fractal dimension image, representing the fractal dimension
% vectors for each class and moment.  This image will always have a row
% size of 1 and a column size determined by the number of classes in the
% input image.  The number of data bands will be equal to the number of
% moments used in the estimation of the fractal dimension.
%
%  "-f1" 15
% uses an ASCII file as output for the image information specifying the
% size of the input image, number of classes in the input image, and the
% range of moments used in the estimation of the fractal dimension.  Also
% included is a listing of the fractal dimensions for each class and
% moment.
%
%  "-l" 15
% specifies the initial or lower size of the sliding window.  This MUST be
% an odd number resulting in a window with a center pixel.  This means that
% the minimum size of the sliding window is 3 x 3.  The default value is
% 3, resulting in an initial window size of 3 x 3.
%
%  "-u" 15
% specifies the final or upper size of the sliding window.  This MUST also
% be an odd number resulting in a window with a center pixel.  The size of
% the upper window MUST be at least one step greater than the lower window
% size.  That is, if the lower window is 3 x 3, then the minimum size of
% the upper window must be 5 x 5, or one step greater than the lower window.
% This is necessary, since there must be at least two points for a best
% fit line to be formed determining the slope and ultimately the fractal
% dimension.
%
%  "-s" 15
% specifies the step size or interval used when specifying a range of
% window sizes.  The default value is two, corresponding to the next odd
% window size with a center pixel.  This may be helpful when a wide range
% of window sizes is required, as it will reduce the number of points
% generated for the best fit line and hence the number of calculations.
%
%  "-q" 15
% specifies the range of moments to base the P(m,L) fractal dimension
% calculations on.  This must be an odd number, corresponding to the
% number of moments centered on zero.  For example, a value of 5 would
% result in the computation of the fractal dimension for the range of
% moments, -2, -1, 0, 1, 2.  The default value is 3, resulting in the
% range of -1, 0, 1.
%
% The input image MUST be of data storage type BYTE or INTEGER.
%
%
%
%  EXAMPLES
%
% vpml -i in_img -o1 out_img1 -o2 out_img2 -f1 file1 -u 11 -s 4 -q 5
% this will estimate the fractal dimension of the in_img using a
% range of sliding window sizes from 3 x 3 (default lower value of 3)
% to an upper size of 11 x 11, with a step size of 4.  This means that
% windows of size 3 x 3, 7 x 7, and 11 x 11 will be used in the fractal
% dimension calculation.  A value of 5 was selected for the number of
% moments to be calculated, meaning that the fractal dimension will be
% computed for the range of moments, -2, -1, 0, 1, 2.  The resulting
% output image, out_img1 will consist of a five-band FLOAT image with
% each band representing the fractal dimension for a particular moment, q.
% The out_img2 will consist of a single row, five-band image specifying
% the fractal dimension for each class of the input image and moment, q.
% The ASCII file1 will provide the user with information on the size of
% the input image, number of classes, and the resulting fractal dimensions
% for each class and moment.
%
%  "SEE ALSO"
% vfractal(1)
%
%  RESTRICTIONS
%
% The input image MUST be of data storage type BYTE or INTEGER.
% The output images are of data storage type FLOAT.
%
%  REFERENCES
% A reference for the pml algorithm is p. 67 of The Science of Fract.
% Images edited by Hienz-Otto Peitgen and Dietmar Saupe.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kvpml(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kvpml(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'l', 3;'u', 5;'s', 2;'q', 3;'o1', '__output';'o2', '__output';'f1', '__output'};
maxval={0,10000,10000,10000,1000,0,0,1};
minval={0,3,5,2,3,0,0,1};
istoggle=[0,1,1,1,1,0,0,1];
was_set=istoggle * 0;
paramtype={'InputFile','Integer','Integer','Integer','Integer','OutputFile','OutputFile','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=2; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'vpml"  '],Inputs,narglist);

author: aludnam

repo: MATLAB

save_path: github-repos/MATLAB/aludnam-MATLAB


text:
% elementwise production of all previous layers, which are required to have the
% same size.
%
classdef HadamardNode < GraphNode

    methods
        function obj = HadamardNode(dimOut)
            obj = obj@GraphNode('Hadamard',dimOut);
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);

            obj.a = prev_layers{1}.a;
            for i=2:length(prev_layers)
                obj.a = obj.a .* conj(prev_layers{i}.a);
            end

            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,prev_layers, future_layers)
            if obj.skipGrad || obj.skipBP
                return;
            end

            future_grad = obj.GetFutureGrad(future_layers);
            for i=1:length(prev_layers)
                [D(1) D(2) D(3) D(4)] = size(prev_layers{i}.a);
                if i==1
                    grad = future_grad .* prev_layers{2}.a;
                else
                    grad = future_grad .* prev_layers{1}.a;
                end
                for j = 1:4
                    if D(j) == 1
                        grad = sum(grad,j);
                    end
                end
                obj.grad{i} = conj(grad);
            end

            obj = backward@GraphNode(obj, prev_layers, future_layers);
        end

    end

end

author: singaxiong

repo: SignalGraph


text:
function domOrient3M = calcDominantOrientation(scanArray3M, mask3M, patchSizeV, dim2d3dFlag, hWait)
% function domOrient3M = calcDominantOrientation(scanArray3M, mask3M, patchSizeV, dim2d3dFlag, hWait)
%
% Dominant orientation calculation.
%
% APA, 10/14/2016

% Flag to draw waitbar
waitbarFlag = 0;
if exist('hWait','var') & ishandle(hWait)
    waitbarFlag = 1;
end

% Get indices of non-NaN voxels
calcIndM = ~isnan(scanArray3M) & mask3M;

% % Grid resolution
slcWindow = 2 * patchSizeV(3) + 1;
rowWindow = 2 * patchSizeV(1) + 1;
colWindow = 2 * patchSizeV(2) + 1;

% Build distance matrices
numColsPad = floor(colWindow/2);
numRowsPad = floor(rowWindow/2);
numSlcsPad = floor(slcWindow/2);

% Get number of voxels per slice
[numRows, numCols, numSlices] = size(scanArray3M);
numVoxels = numRows*numCols;

% Pad q, so that sliding window works also for the edge voxels
if exist('padarray.m','file')
    %scanArrayTmp3M = padarray(scanArray3M,[numRowsPad numColsPad
    %numSlcsPad],NaN,'both'); % aa commented
    q = padarray(scanArray3M,[numRowsPad numColsPad numSlcsPad],NaN,'both');
else
    q = padarray_oct(scanArray3M,[numRowsPad numColsPad numSlcsPad],NaN,'both');
end

% Create indices for 2D blocks
[m,n,~] = size(q);
m = uint32(m);
n = uint32(n);
colWindow = uint32(colWindow);
rowWindow = uint32(rowWindow);
slcWindow = uint32(slcWindow);

% Index calculation adapted from
% http://stackoverflow.com/questions/25449279/efficient-implementation-of-im2col-and-col2im

%// Start indices for each block
start_ind = reshape(bsxfun(@plus,[1:m-rowWindow+1]',[0:n-colWindow]*m),[],1); %//'

%// Row indices
lin_row = permute(bsxfun(@plus,start_ind,[0:rowWindow-1])',[1 3 2]);  %//'

%// Get linear indices based on row and col indices and get desired output
% imTmpM = A(reshape(bsxfun(@plus,lin_row,[0:ncols-1]*m),nrows*ncols,[]));
indM = reshape(bsxfun(@plus,lin_row,(0:colWindow-1)*m),rowWindow*colWindow,[]);

[Fx,Fy] = gradient(q);
%Fx = abs(Fx);
%Fy = abs(Fy);

domOrient3M = zeros(size(scanArray3M));
domOrient2M = zeros(size(scanArray3M(:,:,1)));

tic
% Iterate over slices. compute cooccurance for all patches per slice
for slcNum = 1:numSlices %(1+numSlcsPad):(numSlices+numSlcsPad)

    disp(['--- Orientation Calculation for Slice # ', num2str(slcNum), ' ----'])

    calcSlcIndV = calcIndM(:,:,slcNum);
    calcSlcIndV = calcSlcIndV(:);
    numCalcVoxs = sum(calcSlcIndV);

    indSlcM = indM(:,calcSlcIndV);

    FxSlc = Fx(:,:,slcNum+numSlcsPad);
    FySlc = Fy(:,:,slcNum+numSlcsPad);

    domOrientM = zeros(2,numCalcVoxs);
    for i = 1:numCalcVoxs % size(indSlcM,2)
        Y = [FxSlc(indSlcM(:,i)) FySlc(indSlcM(:,i))];
        Y(sum(isnan(Y),2)>0,:) = [];
        %pp = pca(Y);
        %pp1M(:,i) = pp(1,:)';
        if ~isempty(Y)
            [Usvd, S, Vsvd] = svd(Y);
            domOrientM(:,i) = Vsvd(:,1);
        end
    end
    domOrientSlc2M = domOrient2M;
    domOrientSlc2M(calcSlcIndV) = atan2(domOrientM(2,:),domOrientM(1,:));
    domOrient3M(:,:,slcNum) = domOrientSlc2M;

    if waitbarFlag
        set(hWait, 'Vertices', [[0 0 slcNum/numSlices slcNum/numSlices]' [0 1 1 0]']);
        drawnow;
    end

end
toc

author: cerr

repo: CERR

save_path: github-repos/MATLAB/cerr-CERR


text:
function [track_res, min_err, model] = L1APG_track_frame(img, model)

paraT = model.para;

% parameters
n_sample = paraT.n_sample;
sz_T = paraT.sz_T;
rel_std_afnv = paraT.rel_std_afnv;
nT = paraT.nT;

% L1 function settings
angle_threshold = paraT.angle_threshold;
para.Lambda = model.Lambda;
para.nT = paraT.nT;
para.Lip = paraT.Lip;
para.Maxit = paraT.Maxit;
alpha = 50; % this parameter is used in the calculation of the likelihood of particle filter

% initialization
T = model.T;
T_mean = model.T_mean;
norms = model.norms;
map_aff = model.map_aff;
A = model.A;
Temp = model.Temp;
Dict = model.Dict;
Temp1 = model.Temp1;
min_err = 0;

%-Draw transformation samples from a Gaussian distribution
aff_samples = model.aff_samples;
sc = sqrt(sum(map_aff(1:4).^2)/2);
std_aff = rel_std_afnv .* [1, sc, sc, 1, sc, sc];
aff_samples = draw_sample(aff_samples, std_aff); %draw transformation samples from a Gaussian distribution

%-Crop candidate targets "Y" according to the transformation samples
[Y, Y_inrange] = crop_candidates(double(img), aff_samples(:,1:6), sz_T);
if(sum(Y_inrange==0) == n_sample)
    sprintf('Target is out of the frame!\n');
end

[Y, Y_crop_mean, Y_crop_std] = whitening(Y); % zero-mean-unit-variance
[Y, Y_crop_norm] = normalizeTemplates(Y);    % norm one

%-L1-LS for each candidate target
eta_max	= -inf;
q = zeros(n_sample,1); % minimal error bound initialization

% first stage L2-norm bounding
for j = 1:n_sample
    if Y_inrange(j)==0 || sum(abs(Y(:,j)))==0
        continue;
    end

    % L2 norm bounding
    q(j) = norm(Y(:,j) - Temp1*Y(:,j));
    q(j) = exp(-alpha*q(j)^2);
end
%  sort samples according to descend order of q
[q, indq] = sort(q, 'descend');

% second stage
p = zeros(n_sample,1); % observation likelihood initialization
n = 1;
tau = 0;
while (n < n_sample) && (q(n) >= tau)
    c = APGLASSOup(Temp'*Y(:,indq(n)), Dict, para);
    D_s = (Y(:,indq(n)) - A(:,1:nT)*c(1:nT)).^2;  % reconstruction error
    p(indq(n)) = exp(-alpha*(sum(D_s)));          % probability w.r.t samples
    tau = tau + p(indq(n))/(2*n_sample-1);        % update the threshold

    if(sum(c(1:nT)) < 0) %remove the inverse intensity patterns
        continue;
    elseif(p(indq(n)) > eta_max)
        id_max	= indq(n);
        c_max	= c;
        eta_max = p(indq(n));
        min_err = sum(D_s);
    end
    n = n + 1;
end

% target transformation parameters with the maximum probability
map_aff = aff_samples(id_max,1:6);
a_max	= c_max(1:nT);
% resample the samples wrt. the probability
[aff_samples, ~] = resample(aff_samples, p, map_aff);
[~, indA] = max(a_max);
min_angle = images_angle(Y(:,id_max), A(:,indA));

%-Template update
model.occlusionNf = model.occlusionNf - 1;
level = 0.03;
if (min_angle > angle_threshold && model.occlusionNf < 0)
    trivial_coef = c_max(nT+1:end);
    trivial_coef = reshape(trivial_coef, [sz_T 3]);
    trivial_coef = rgb2gray(trivial_coef);
    trivial_coef = im2bw(trivial_coef, level);

    se = [0 0 0 0 0;
          0 0 1 0 0;
          0 1 1 1 0;
          0 0 1 0 0;
          0 0 0 0 0];
    trivial_coef = imclose(trivial_coef, se);

    cc = bwconncomp(trivial_coef);
    stats = regionprops(cc, 'Area');
    areas = [stats.Area];

    % occlusion detection
    if isempty(areas) == 1 || max(areas) < round(0.25*prod(sz_T))
        disp('Update!')
        % find the tempalte to be replaced
        [~,indW] = min(a_max(1:nT));

        % insert new template
        T(:,indW)	= Y(:,id_max);
        T_mean(indW)= Y_crop_mean(id_max);
        norms(indW) = Y_crop_std(id_max)*Y_crop_norm(id_max);

        [T, ~] = normalizeTemplates(T);
        A(:,1:nT)	= T;

        %Temaplate Matrix
        Temp = A;
        Dict = Temp'*Temp;
        Temp1 = T*pinv(T);

        model.T = T;
        model.T_mean = T_mean;
        model.norms = norms;
        model.A = A;
        model.Temp = Temp;
        model.Dict = Dict;
        model.Temp1 = Temp1;
    else
        model.occlusionNf = 5;
        % update L2 regularized term
        para.Lambda(3) = 0;
        fprintf('target %d occluded in online mode!\n', model.id);
    end
elseif model.occlusionNf < 0
    para.Lambda(3) = paraT.lambda(3);
end
model.Lambda = para.Lambda;
model.map_aff = map_aff;
model.aff_samples = aff_samples;

%-Store tracking result
track_res = map_aff';

%-Demostration and debugging
if paraT.bDebug
    % draw tracking results
    img	= showTemplates(img, T, T_mean, norms, sz_T, nT);
    imshow(uint8(img));
    color = [1 0 0];
    drawAffine(map_aff, sz_T, color, 2);
    drawnow;
end

author: yuxng


text:
%% quiverTriad
% Below is a demonstration of the features of the |quiverTriad| function

%%
clear; close all; clc;

%% Syntax
% |[varargout]=quiverTriad(varargin);|

%% Description
% UNDOCUMENTED
%% Examples
%
%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

author: gibbonCode

repo: GIBBON

save_path: github-repos/MATLAB/gibbonCode-GIBBON


text:
function [fibercurvatures, curves]=dtiComputeFiberGroupCurvatures(fg)

%For fibers within a fibergroup fg computes their curvature values, returning an array of curvatures. Useful if
%feeding this function resampled fg and then computing correlations between
%these curves.
%Works only  if all the fibers are the same length -- use
%dtiFiberCurvature for prosessing individual fibers otherwise

%ER 04/2008

nfibers=size(fg.fibers, 1);
numNodes=size(fg.fibers{1}, 2);
curves=zeros(3, numNodes, nfibers);

for i=1:nfibers
    %Compute curvature representations
    fibercurvatures(i, :)=dtiFiberCurvature(fg.fibers{i}); %fiber_curvature_vals=fiber_curvature(fiber);
    curves(:, :, i)=fg.fibers{i};
end

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function varargout = lzipdecode(varargin)
%
% output = lzipdecode(input)
%    or
% output = lzipdecode(input,info)
%
% Decompressing an Lzip-compressed byte-stream to recover the original data
% This function depends on the ZMat toolbox (http://github.com/fangq/zmat)
%
% authors:Qianqian Fang (q.fang <at> neu.edu)
%
% input:
%      input: a string, int8/uint8 vector or numerical array to store Lzip-compressed data
%      info (optional): a struct produced by the zmat/lzipencode function during
%            compression; if not given, the inputs/outputs will be treated as a
%            1-D vector
%
% output:
%      output: the decompressed byte stream stored in a uint8 vector; if info is
%            given, output will restore the original data's type and dimensions
%
% examples:
%      [bytes, info]=lzipencode(eye(10));
%      orig=lzipdecode(bytes,info);
%
% license:
%     BSD or GPL version 3, see LICENSE_{BSD,GPLv3}.txt files for details
%
% -- this function is part of JSONLab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab)
%

if(nargin==0)
    error('you must provide at least 1 input');
end
if(exist('zmat','file')==2 || exist('zmat','file')==3)
    if(nargin>1)
        [varargout{1:nargout}]=zmat(varargin{1},varargin{2:end});
    else
        [varargout{1:nargout}]=zmat(varargin{1},0,'lzip',varargin{2:end});
    end
else
    error('you must install ZMat toolbox to use this feature: http://github.com/fangq/zmat')
end

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
%% This is for testing the SerialLink functions in the robotics Toolbox
function tests = SerialLinkTest

    tests = functiontests(localfunctions);
    clc

end

function setupOnce(tc)

    mdl_puma560;
    tc.TestData.p560 = p560;
    mdl_planar2
    tc.TestData.p2 = p2;
    mdl_puma560akb
    tc.TestData.p560m = p560m;
    mdl_stanford
    tc.TestData.stanf = stanf;
end

function teardownOnce(tc)
    close all
end

function SerialLink_test(tc)
    % test making a robot from links
    L(1)=Link([1 1 1 1 1]);
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    % test makin a robot from DH matrix
    DH = [pi/2 0 0 1; 0 2 0 0];
    R2 = SerialLink(DH,'name','Robot2');

end

function SerialLinkMDH_test(tc)
    % minimalistic test of modified DH functionality
    %  most code is common, need to exercise MDH code in Link and also for
    %  RNE

    mdl_puma560akb

    % qr is not set by the script, it clashes with the builtin function QR(tc)
    qz = [0 0 0 0 0 0];

    T = p560m.fkine(qz);
    J = p560m.jacobe(qz);
    tau = p560m.rne(qz, qz, qz);
end

function dyn_test(tc)

    mdl_puma560
    p560.dyn;

    mdl_puma560akb
    p560m.dyn;
end

%    *                          - compound two robots
function compound_test(tc)
    L(1)=Link([1 1 1 1 1]);
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    DH = [pi/2 0 0 1; 0 2 0 0];
    R2 = SerialLink(DH,'name','Robot2');
    %Test two different compunding situations
    R3 = R1*R2;
    R4 = R1*R3;
end

%%     Kinematic methods
%        fkine                  - forward kinematics
function fkine_test(tc)

    % create simple RP robot we can reason about
    L(1)=Link([0 2 3 0 0]);
    L(2)=Link([0 0 0 0 1]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4) );

    tc.verifyTrue(isa( R1.fkine([0,0]), 'SE3') );

    tc.verifyEqual(R1.fkine([0,0]).T, transl(3,0,2), 'absTol',1e-6);
    tc.verifyEqual(R1.fkine([pi/2,0]).T, transl(0,3,2)*trotz(pi/2), 'absTol',1e-6);
    tc.verifyEqual(R1.fkine([0,1]).T, transl(3,0,3), 'absTol',1e-6);

    mdl_puma560

    T=p560.fkine( [0 0 0 0 0 0; 0 0.03 -0.0365 0 0 0] );
    tc.verifyEqual(length(T), 2, 'SE3' );;
end

function plot_test(tc)
    clf
    L(1)=Link([1 1 1 1 0]);
    L(1).qlim = [-5 5];
    L(2)=Link([0 1 0 1 0]);
    R1 = SerialLink(L,'name','robot1','comment', 'test robot','manufacturer', 'test',...
        'base', eye(4,4), 'tool', eye(4,4), 'offset', [1 1 0 0 0 0 ] );
    R1.plot([1 1]);
    close all
end

function plot_animate_test(tc)
    tc.assumeTrue(ispc || ismac);
    fname = fullfile(tempdir, 'puma.mp4');
    qt = jtraj(tc.TestData.p560.qz, tc.TestData.p560.qr, 50);
    tc.TestData.p560.plot(qt, 'movie', fname);
    tc.verifyTrue(exist(fname, 'file') == 2);
    delete(fname)
end

function plot3d_test(tc)
    clf
    tc.TestData.p560.plot3d( tc.TestData.p560.qn );
    close(gcf)
end

function teach_test(tc)
    tc.TestData.p560.teach();
    close all

    tc.TestData.p560.teach('eul');
    close all
    tc.TestData.p560.teach('eul', 'deg');
    close all
    tc.TestData.p560.teach('rpy');
    close all
    tc.TestData.p560.teach('rpy/xyz');
    close all
    tc.TestData.p560.teach('rpy/zyx');
    close all
    tc.TestData.p560.teach('approach');
    close all

    tc.TestData.p560.teach('callback', @(r, q) fprintf('hello') );
    close all

    tc.TestData.p2.teach();
    close all

    tc.TestData.p2.teach('2d');
    close all

    figure
    figure
    tc.TestData.p2.teach([0.2 0.3]);
    close all

    close all

end

%%--------------------------------------------------------------------------
%inverse kinematics for 6-axis arm with sph.wrist
function ikine6s_puma_test(tc)

    % puma case
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    T = p560.fkine(qn);
    qik = p560.ikine6s(T,'ru');
    tc.verifyEqual(p560.fkine(qik).T, T.T, 'absTol', 1e-6);
    tc.verifyTrue(qik(2) > 0);

    T = p560.fkine(qn);
    qik = p560.ikine6s(T,'ld');
    tc.verifyEqual(p560.fkine(qik).T, T.T, 'absTol', 1e-6);
    tc.verifyTrue(qik(2) < 0);

    % error handling
    mdl_puma560akb

    tc.verifyError( @() p560m.ikine6s(T), 'RTB:ikine:notsupported' );
    tc.verifyError( @() tc.TestData.p2.ikine6s(T), 'RTB:ikine:notsupported' );

end

function ikine6s_stanford_test(tc)

    % stanford arm case
    q = [pi/4 pi/4 0.3 pi/4 -pi/4 pi/4];
    T = tc.TestData.stanf.fkine(q);
    qik = tc.TestData.stanf.ikine6s(T);
    tc.verifyEqual(tc.TestData.stanf.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine6s_KR5_test(tc)

    % kr5 arm case
    mdl_KR5
    q = [0 pi/4 -pi 1 pi/4 0];
    T = KR5.fkine(q);
    qik = KR5.ikine6s(T);
    tc.verifyEqual(KR5.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine6s_IRB140_test(tc)

    tc.assumeTrue(false);  %HACK

    % nooffset type
    mdl_irb140
    q = [0 -3*pi/4 0 0 pi/4 0];
    T = irb140.fkine(q);
    qik = irb140.ikine6s(T);
    tc.verifyEqual(irb140.fkine(qik).T, T.T, 'absTol', 1e-6);

end

function ikine_test(tc)
    T = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    qik = tc.TestData.p560.ikine(T, [0 0 3 0 0 0]);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikunc_optim_test(tc)
    T = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    qik = tc.TestData.p560.ikunc(T);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikcon_optim_test(tc)

    tc.assumeTrue(false);  %HACK

    qn = [0 pi/4 -pi 0 pi/4 0];
    T = tc.TestData.p560.fkine(qn);
    qik = tc.TestData.p560.ikcon(T);

    T2 = tc.TestData.p560.fkine(qik);
    tc.verifyEqual(T.T, T2.T,'absTol',1e-6);
end

function ikine_sym_test(tc)
    % 2DOF test
    p2 = SerialLink(tc.TestData.p2); % clone it

    q = p2.ikine_sym(2);

    % is the solution sane
    tc.verifyLength(q, 2);
    tc.verifyTrue(iscell(q));
    tc.verifyTrue(isa(q{1}, 'sym'));
    tc.verifyLength(q{1}, 2);
    tc.verifyTrue(isa(q{2}, 'sym'));
    tc.verifyLength(q{1}, 2);

    % process the solutions
    q1 = subs(q{1}, {'tx', 'ty'}, {1,1});   % convert to numeric
    sol = eval(q1)';
    q2 = subs(q{2}, {'tx', 'ty', 'q1'}, {1,1, q1(1)});
    x = eval(q2);  % first solution
    sol(1,2) = x(1);
    q2 = subs(q{2}, {'tx', 'ty', 'q1'}, {1,1, q1(2)}); % second solution
    x = eval(q2);
    sol(2,2) = x(1);

    % check the FK is good
    tc.verifyEqual(transl(tc.TestData.p2.fkine(sol(1,:))), [1 1 0]);
    tc.verifyEqual(transl(tc.TestData.p2.fkine(sol(2,:))), [1 1 0]);

    tc.verifyError( @() tc.TestData.p2.ikine_sym(4), 'RTB:ikine_sym:badarg');
end

function ikine_sym2_test(tc)
    tc.assumeTrue(false);  %HACK
    % 3DOF test

    % create robot arm with no offset (IRB140 style)
    robot = SerialLink([0 0.5 0 pi/2; 0 0 0.5 0; 0 0 0.5 0])
    test_sym_ik(robot)

    % create robot arm with offset (Puma560 style)
    robot = SerialLink(tc.TestData.p560.links(1:3));
    robot = SerialLink(robot); % clone it
    test_sym_ik(robot)

    function test_sym_ik(robot)
        % test all 8 solutions are good

        q = robot.ikine_sym(3);

        % process the solutions
        T = robot.fkine([0.2, 0.3, 0.4]); % choose joint angles

        pe = num2cell(T.t');

        q1 = subs(q{1}, {'tx', 'ty', 'tz'}, pe);   % convert to numeric

        qik = [];
        ss = [];

        for s1=1:2
            try
                sol1 = eval(q1(s1));
            catch
                fprintf('** q1(%d) eval failure\n', s1);
            end

            q2 = subs(q{2}, {'tx', 'ty', 'tz', 'q1'}, [pe q1(s1)]);

            for s2=1:2
                try
                    sol2 = eval(q2(s2));
                catch
                    fprintf('** q2(%d) eval failure\n', s2);
                    continue;
                end
                q3 = subs(q{3}, {'tx', 'ty', 'tz', 'q1', 'q2'}, [pe q1(s1), q2(s2)]);

                for s3=1:2
                    try
                        sol3 = eval(q3(s3));
                        qik = [qik; sol1 sol2 sol3];
                        ss = [ss; s1 s2 s3];
                    catch
                        fprintf('** q3(%d) eval failure\n', s3);
                    end
                end
            end
        end

        % check the FK is good, zero residual
        nn = [];
        for qq=qik'
            nn = [nn; norm(transl(robot.fkine(qq')-T))];
        end
        tc.verifyEqual(sum(nn), 0, 'absTol', 1e-6);
    end
end

%%--------------------------------------------------------------------------

function jacob0_test(tc)

    function J = jacob0_approx(robot, q, d)
        e = eye(robot.n);
        T0 = robot.fkine(q);
        R0 = t2r(T0);
        J = [];
        for i=1:robot.n
            Ji = (robot.fkine(q + e(i,:)*d) - T0) / d;
            J = [J [Ji(1:3,4); vex(Ji(1:3,1:3)*R0')]];
        end
    end
    % implictly tests jacobe
    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = tc.TestData.p560.jacob0(qz);
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    tc.verifyEqual(tc.TestData.p560.jacob0(qz), jacob0_approx(tc.TestData.p560, qz, 1e-8), 'absTol', 1e-4);
    tc.verifyEqual(tc.TestData.p560.jacob0(qn), jacob0_approx(tc.TestData.p560, qn, 1e-8), 'absTol', 1e-4);

    expected_out = jacob0_approx(tc.TestData.p560, qn, 1e-8)
    out = tc.TestData.p560.jacob0(qn*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacob0(qn, 'rpy');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'eul');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'exp');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'trans');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);
    out = tc.TestData.p560.jacob0(qn, 'rot');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);

    tc.verifyEqual(tc.TestData.p560.jacob0(qn), jacob0_approx(tc.TestData.p560, qn, 1e-8), 'absTol', 1e-4);

    out = tc.TestData.stanf.jacob0([0 0 0 0 0 0]);
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);
    tc.verifyEqual(tc.TestData.stanf.jacob0(qn), jacob0_approx(tc.TestData.stanf, qn, 1e-8), 'absTol', 1e-4);

end

function jacobe_test(tc)

    function J = jacobe_approx(robot, q, d)
        e = eye(robot.n);
        T0 = robot.fkine(q);
        R0 = t2r(T0);
        J = [];
        for i=1:robot.n
            Ji = (robot.fkine(q + e(i,:)*d) - T0) / d;
            J = [J [R0'*Ji(1:3,4); vex(R0'*Ji(1:3,1:3))]];
        end
    end

    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];

    out = tc.TestData.p560.jacobe(qz)
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);
    expected_out = jacobe_approx(tc.TestData.p560, qz, 1e-8)
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacobe(qz*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacobe(qn);
    expected_out = jacobe_approx(tc.TestData.p560, qn, 1e-8)

    tc.verifyEqual(out,expected_out,'absTol',1e-4);
    out = tc.TestData.p560.jacobe(qn*180/pi, 'deg');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    out = tc.TestData.p560.jacob0(qn, 'rpy');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'eul');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'exp');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [6 6]);

    out = tc.TestData.p560.jacob0(qn, 'trans');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);
    out = tc.TestData.p560.jacob0(qn, 'rot');
    tc.verifyClass(out, 'double');
    tc.verifySize(out, [3 6]);

    tc.verifyWarning( @() tc.TestData.p560.jacobn(qn), 'RTB:SerialLink:deprecated');
end

function maniplty_test(tc)
    mdl_puma560;
    q = [0 pi/4 -pi 1 pi/4 0];
    tc.verifyEqual(p560.maniplty(q), 0.0786, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'trans'), 0.1112, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'rot'), 2.5936, 'absTol',1e-4);
    tc.verifyEqual(p560.maniplty(q, 'asada', 'trans'), 0.2733, 'absTol',1e-4);
end

function jacob_dot_test(tc)

    tc.assumeTrue(false);  %HACK

    function Jdqd = jacob_dot_approx(robot, q, qd, d)
        e = eye(robot.n);
        J0 = robot.jacob0(q);
        Jdqd = zeros(6,1);
        for i=1:robot.n
            Ji = (robot.jacob0(q + e(i,:)*d) - J0) / d;
            Jdqd = Jdqd + Ji * qd(i);
        end
    end

    q = rand(1,6);
    qd = rand(1,6);

    jacob_dot_approx(tc.TestData.p560, q, qd, 1e-8)*qd'
    tc.TestData.p560.jacob_dot(q, qd)
end

%%     Dynamics methods

function rne_test(tc)

    % use something simple we can reason about

    mdl_twolink
    g = twolink.gravity(3);

    qz = [0 0];  % need this to get around a MATLAB bug passing values from a script

    % different poses
    Q = twolink.rne([0 0], qz, qz, 'slow');
    tc.verifyEqual(Q, g*[2 0.5], 'absTol', 1e-6);

    Q = twolink.rne([0 pi/2], qz, qz);
    tc.verifyEqual(Q, g*[1.5 0], 'absTol', 1e-6);

    Q = twolink.rne([pi/2 0], qz, qz);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);

    % change gravity
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]');
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0]);
    tc.verifyEqual(Q, g*[0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 -g]);
    tc.verifyEqual(Q, g*[-2 -0.5], 'absTol', 1e-6);

    % add an external force in end-effector frame
    %  y-axis is vertically upward

    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [1 0 0 0 0 0]);
    tc.verifyEqual(Q, [0 0], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [0 1 0 0 0 0]);
    tc.verifyEqual(Q, [2 1], 'absTol', 1e-6);
    Q = twolink.rne([0 0], qz, qz, 'gravity', [0 0 0], 'fext', [0 0 1 0 0 0]);
    tc.verifyEqual(Q, [0 0], 'absTol', 1e-6);

    % test the [q qd qdd] case
    Q1 = twolink.rne([1 2], [3 4], [5 6]);
    Q2 = twolink.rne([1 2 3 4 5 6]);
    tc.verifyEqual(Q1, Q2, 'absTol', 1e-6);

    % test the matrix input case
    Q1 = twolink.rne([1 2], [3 4], [5 6]);
    Q2 = twolink.rne([5 6], [1 2], [3 4]);
    Q3 = twolink.rne([3 4], [5 6], [1 2]);

    Q4 = twolink.rne([1 2; 5 6; 3 4], [3 4; 1 2; 5 6], [5 6; 3 4; 1 2]);
    tc.verifyEqual([Q1; Q2; Q3], Q4, 'absTol', 1e-6);

    % probably should do a robot with a prismatic axis (or two)

end

function rne_mdh_test(tc)
    q = rand(1,6);
    tc.TestData.p560m.rne(q, q, q);
end

%        accel                  - forward dynamics
function accel_test(tc)
    qd = 0.5 * [1 1 1 1 1 1];
    qz = [0 1 0 0 2 0];
    Q = tc.TestData.p560.rne(tc.TestData.p560.qn, qz, qz);

    out = tc.TestData.p560.accel(qz, qd, Q);
    expected_out = [  -9.3397 4.9666 1.6095 -5.4305 5.9885 -2.1228]';

    tc.verifyEqual(out, expected_out,'absTol',1e-4);

    out = tc.TestData.p560.accel([qz, qd,Q]);
    tc.verifyEqual(out, expected_out,'absTol',1e-4);

    out = tc.TestData.p560.accel([qz, qd,Q]');
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qd = [0.1 0.1 0.1 0.1 0.1 0.1;0.2 0.2 0.2 0.2 0.2 0.2];
    qz = [0 1 0 0 2 0;0 0.5 0 0 1 0];
    qd1 = [0.1 0.1 0.1 0.1 0.1 0.1];
    qd2 = [0.2 0.2 0.2 0.2 0.2 0.2];
    qz1 = [0 1 0 0 2 0];
    qz2 = [0 0.5 0 0 1 0];
    qn1 = [0 pi/4 -pi 1 pi/4 0];
    qn2 = [0 pi/2 -pi 1 pi/2 0];

    Q1 = tc.TestData.p560.rne(qn1,qz1,qz1);
    Q2 = tc.TestData.p560.rne(qn2,qz2,qz2);

    q3 = [Q1;Q2];

    out = tc.TestData.p560.accel(qz, qd,q3);
    expected_out = [
        -8.2760    5.8119    3.1487   -4.6392    6.9558   -1.6774
        -8.3467   -4.8514    6.0575   -4.9232    3.1244   -1.7861 ];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    tc.verifyError( @() tc.TestData.p560.accel(), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( [ 1 2]), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz, qz, qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz, qd1, qd1), 'RTB:accel:badarg');

    tc.verifyError( @() tc.TestData.p560.accel( [1 2], qd1, qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz1, [1 2], qd1), 'RTB:accel:badarg');
    tc.verifyError( @() tc.TestData.p560.accel( qz1, qz1, [1 2]), 'RTB:accel:badarg');

    tc.verifyError( @() tc.TestData.p560.accel( [ 1 2]), 'RTB:accel:badarg');

end

%        cinertia               - Cartesian manipulator inertia matrix
function cinertia_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.cinertia(qn);
    expected_out = [18.0741   -2.2763   -8.8446   -0.0283    0.7541   -0.4015
        -2.2763   11.2461    1.7238   -0.0750    0.2214   -0.4733
        -8.8446    1.7238   14.1135   -0.0300    0.7525   -0.4032
        -0.0283   -0.0750   -0.0300    0.1377   -0.0171    0.0492
        0.7541    0.2214    0.7525   -0.0171    0.4612   -0.2461
        -0.4015   -0.4733   -0.4032    0.0492   -0.2461    0.3457];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);;
end

%        coriolis               - centripetal/coriolis torque
function coriolis_test(tc)
    mdl_puma560;
    qd = 0.5 * [1 1 1 1 1 1];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.coriolis(qn, qd);
    expected_out = [
        -0.1336   -0.6458    0.0845    0.0005   -0.0008    0.0000
        0.3136    0.1922    0.3851   -0.0018   -0.0007    0.0000
        -0.1803   -0.1934   -0.0005   -0.0009   -0.0014    0.0000
        0.0007    0.0008    0.0003    0.0001    0.0003   -0.0000
        -0.0004    0.0005    0.0005   -0.0003   -0.0000   -0.0000
        0.0000    0.0000    0.0000   -0.0000    0.0000         0
        ];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qd = [0.1 0.1 0.1 0.1 0.1 0.1;0.2 0.2 0.2 0.2 0.2 0.2];
    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = p560.coriolis(qn, qd);
    expected_out(:,:,1) = [
        -0.0267   -0.1292    0.0169    0.0001   -0.0002    0.0000
        0.0627    0.0384    0.0770   -0.0004   -0.0001    0.0000
        -0.0361   -0.0387   -0.0001   -0.0002   -0.0003    0.0000
        0.0001    0.0002    0.0001    0.0000    0.0001   -0.0000
        -0.0001    0.0001    0.0001   -0.0001   -0.0000   -0.0000
        0.0000    0.0000    0.0000   -0.0000    0.0000         0
        ];
    expected_out(:,:,2) = [
        -0.0715   -0.1242   -0.0534    0.0008   -0.0001   -0.0000
        0.0731    0.0765    0.1535   -0.0009   -0.0007    0.0000
        -0.0023   -0.0772   -0.0003   -0.0004   -0.0007    0.0000
        0.0004    0.0006    0.0003    0.0000    0.0002   -0.0000
        0.0002    0.0004    0.0004   -0.0002    0.0000    0.0000
        -0.0000    0.0000    0.0000   -0.0000   -0.0000         0
        ];

    tc.verifyEqual(out,expected_out,'absTol',1e-4);
end

function gravload_test(tc)
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = tc.TestData.p560.gravload(qn);
    expected_out = [-0.0000 31.6334 6.0286 -0.0119 0.0218 0];
    tc.verifyEqual(out, expected_out, 'absTol',1e-4);

    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = tc.TestData.p560.gravload(qn);
    expected_out = [-0.0000   31.6334    6.0286   -0.0119    0.0218         0
        0.0000    7.7198    8.7439   -0.0238   -0.0000         0];
    tc.verifyEqual(out, expected_out, 'absTol', 1e-4);

    % zero gravity
    out = tc.TestData.p560.gravload(tc.TestData.p560.qn, [0 0 0]);
    tc.verifyEqual(out, [0 0 0 0 0 0], 'absTol', 1e-4);

    tc.verifyError( @() tc.TestData.p560.gravload([1 2 3]), 'RTB:SerialLink:gravload:badarg' );
end

function inertia_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.inertia(qn);
    expected_out = [3.6591   -0.4042    0.1013   -0.0021   -0.0015   -0.0000
        -0.4042    4.4128    0.3504   -0.0008    0.0017    0.0000
        0.1013    0.3504    0.9377   -0.0008    0.0008    0.0000
        -0.0021   -0.0008   -0.0008    0.1925    0.0000    0.0000
        -0.0015    0.0017    0.0008    0.0000    0.1713    0.0000
        -0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    qn = [0 pi/4 -pi 1 pi/4 0; 0 pi/2 -pi 1 pi/2 0];
    out = p560.inertia(qn);
    expected_out(:,:,1) = [3.6591   -0.4042    0.1013   -0.0021   -0.0015   -0.0000
        -0.4042    4.4128    0.3504   -0.0008    0.0017    0.0000
        0.1013    0.3504    0.9377   -0.0008    0.0008    0.0000
        -0.0021   -0.0008   -0.0008    0.1925    0.0000    0.0000
        -0.0015    0.0017    0.0008    0.0000    0.1713    0.0000
        -0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    expected_out(:,:,2) = [2.6621   -0.6880    0.0035   -0.0007   -0.0010    0.0000
        -0.6880    4.4114    0.3487   -0.0010    0.0015    0.0000
        0.0035    0.3487    0.9359   -0.0010    0.0003    0.0000
        -0.0007   -0.0010   -0.0010    0.1926    0.0000    0.0000
        -0.0010    0.0015    0.0003    0.0000    0.1713    0.0000
        0.0000    0.0000    0.0000    0.0000    0.0000    0.1941];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);
end

function dynamic_component_test(tc)
    % test that inertial components
    q = rand(1,6);
    qd = rand(1,6);
    qdd = rand(1,6);

    robot = tc.TestData.p560.nofriction('all');

    I = robot.inertia(q);
    C = robot.coriolis(q, qd);
    g = robot.gravload(q)';

    tau = robot.rne(q, qd, qdd)';

    tc.verifyEqual(norm(I*qdd'+C*qd'+g-tau), 0, 'absTol', 1e-8);
end

%        itorque                - inertia torque
function itorque_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    qdd = 0.5 * [1 1 1 1 1 1];
    out = p560.itorque(qn,qdd);
    expected_out = [1.6763    2.1799    0.6947    0.0944    0.0861    0.0971];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    %        rne                    - inverse dynamics
end
function rne2_test(tc)
    mdl_puma560;
    qz = [0 1 0 0 2 0];
    qn = [0 pi/4 -pi 1 pi/4 0];
    out = p560.rne(qn,qz,qz);
    expected_out = [-0.9748   59.1306    5.9889   -0.0108    1.8872    0.0001];
    tc.verifyEqual(out,expected_out,'absTol',1e-4);

    pnf = p560.nofriction('all');

    q = rand(1,6);
    qd = rand(1,6);
    qdd = rand(1,6);

    tau1 = pnf.rne(q,qd,qdd);
    tau2 = pnf.inertia(q)*qdd' +pnf.coriolis(q,qd)*qd' + pnf.gravload(q)';
    tc.verifyEqual(tau1', tau2, 'absTol', 1e-6);
end

%        fdyn                   - forward dynamics
function fdyn_test(tc)
    mdl_puma560;
    qn = [0 pi/4 -pi 1 pi/4 0];
    qd = 0.5 * [1 1 1 1 1 1];
    qdo = [0 0 0 0 0 0];
    T = 0.0001;

    p560 = p560.nofriction();

    [TI,Q,QD] = p560.fdyn(T, @(r,t,q,qd) zeros(1,6), qn, qdo);
end

function nofriction_test(tc)
    mdl_puma560;
    verifyFalse(tc,  p560.links(3).B == 0);
    verifyFalse(tc,  all(p560.links(3).Tc == 0) );

    nf = p560.nofriction();
    verifyFalse(tc,  nf.links(3).B == 0);
    tc.verifyTrue( all(nf.links(3).Tc == 0) );

    mdl_puma560;
    nf = p560.nofriction('all');
    tc.verifyTrue( nf.links(3).B == 0);
    tc.verifyTrue( all(nf.links(3).Tc == 0) );
end

function jtraj_test(tc)
    mdl_puma560
    qz = [0 0 0 0 0 0];
    qr = [0 pi/2 -pi/2 0 0 0];
    T1 = p560.fkine(qz);
    T2 = p560.fkine(qr);
    qt = p560.jtraj(T1, T2, 50, 'r');
    tc.verifyEqual( size(qt), [50 6]);

    %tc.verifyEqual(qt(1,:), qz, 'abstol', 1e-10);
    %tc.verifyEqual(qt(end,:), qr, 'abstol', 1e-10);
end

function edit_test(tc)
    robot = SerialLink(tc.TestData.p560);

    robot.edit()

    table = findobj('Type', 'uitable');
    table.Data(1,2) = 7;

    button = findobj('String', 'Save')
    f = button.Callback{1};
    f(button, [], table);

    tc.verifyEqual(robot.links(1).d, 7);
    close(gcf)

    robot.edit('dyn')

    table = findobj('Type', 'uitable');
    table.Data(1,9) = 7;

    button = findobj('String', 'Save')
    f = button.Callback{1};
    f(button, [], table);

    tc.verifyEqual(robot.links(1).m, 7);
    close(gcf)
end

function ellipse_test(tc)
    tc.TestData.p560.plot( tc.TestData.p560.qn );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, 'trans' );
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, 'rot' );

    clf
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, '2d' );

    close(gcf)

    tc.TestData.p560.plot( tc.TestData.p560.qn );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn, 'trans' );
    tc.TestData.p560.vellipse( tc.TestData.p560.qn, 'rot' );
    clf
    tc.TestData.p560.fellipse( tc.TestData.p560.qn, '2d' );

    close(gcf)
end

function dh_mdh_test(tc)

    tc.verifyTrue(tc.TestData.p560.isdh)
    tc.verifyFalse(tc.TestData.p560.ismdh)

    tc.verifyClass(tc.TestData.p560.isdh, 'logical')
    tc.verifyClass(tc.TestData.p560.ismdh, 'logical')

    tc.verifyFalse(tc.TestData.p560m.isdh)
    tc.verifyTrue(tc.TestData.p560m.ismdh)

    p560m = tc.TestData.p560.MDH;
    tc.verifyTrue(isa(p560m, 'SerialLink'));
    tc.verifyEqual(p560m.n, 6);
    tc.verifyTrue(p560m.ismdh);

    p560 = tc.TestData.p560m.DH;
    tc.verifyTrue(isa(p560, 'SerialLink'));
    tc.verifyEqual(p560.n, 6);
    tc.verifyTrue(p560.isdh);

    p560m = tc.TestData.p560.MDH;
    p560 = p560m.DH;  % should be the same robot

    T1 = tc.TestData.p560.fkine(tc.TestData.p560.qn);
    T2 = p560.fkine(tc.TestData.p560.qn);
    tc.verifyEqual(T1, T2);

end

function twists_test(tc)
    [tw,T0] = tc.TestData.p560.twists(tc.TestData.p560.qz);
    tc.verifyClass(tw, 'Twist');
    tc.verifyLength(tw, 6);
    tc.verifyClass(T0, 'SE3');
    tc.verifyLength(T0, 1);

    tc.verifyEqual( double(prod( [tw.exp(tc.TestData.p560.qn) T0] )), ...
        double(tc.TestData.p560.fkine(tc.TestData.p560.qn)), ...
        'absTol', 1e-6);
end

function predicates_test(tc)
    % isdh
    x = tc.TestData.p560.isdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560m.isdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);

    % ismdh
    x = tc.TestData.p560.ismdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.p560m.ismdh
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isspherical
    x = tc.TestData.p560.isspherical
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560.isspherical
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isprismatic
    x = tc.TestData.stanf.isprismatic
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 6); tc.verifyEqual(x, logical([0 0 1 0 0 0]));
    % isrevolute
    x = tc.TestData.stanf.isrevolute
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 6); tc.verifyEqual(x, ~logical([0 0 1 0 0 0]));

    % issym
    x = tc.TestData.p560.issym
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    mdl_twolink_sym;
    x = twolink.issym
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % isconfig
    x = tc.TestData.p560.isconfig('RRRRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);
    x = tc.TestData.p560.isconfig('RRPRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.stanf.isconfig('RRRRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyFalse(x);
    x = tc.TestData.stanf.isconfig('RRPRRR');
    tc.verifyClass(x, 'logical'); tc.verifyLength(x, 1); tc.verifyTrue(x);

    % islimit
    x = tc.TestData.p560.islimit([0 0 0 0 0 0]);
    tc.verifyClass(x, 'double'); tc.verifySize(x, [6 2]);
    tc.verifyEqual(x, zeros(6,2));
    x = tc.TestData.p560.islimit([0 10 0 0 0 0]);
    tc.verifyClass(x, 'double'); tc.verifySize(x, [6 2]);
    tc.verifyEqual(x, [0 1 0 0 0 0]'*[1 1]);
end

function test_get(tc)
    x = tc.TestData.p560.d;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [0  0  0.15005  0.4318  0  0], 'absTol', 1e-6);

    x = tc.TestData.p560.a;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [0 0.4318 0.0203  0  0 0], 'absTol', 1e-6);

    x = tc.TestData.p560.alpha;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 6);
    tc.verifyEqual(x, [1 0 -1 1 -1 0]*pi/2, 'absTol', 1e-6);

    x = tc.TestData.p560.theta;
    tc.verifyClass(x, 'double'); tc.verifyLength(x, 0);

end

function mat2str_test(tc)
    mdl_twolink_sym;
    twolink
end

function rad_deg_test(tc)
    q = [1 2 3 4 5 6];

    q2 = tc.TestData.p560.todegrees(q);
    tc.verifyEqual(q*180/pi, q2);
    q2 = tc.TestData.p560.toradians(q);
    tc.verifyEqual(q*pi/180, q2);

    q2 = tc.TestData.stanf.todegrees(q);
    qq = q*180/pi;
    qq(3) = q(3);
    tc.verifyEqual(qq, q2);
    q2 = tc.TestData.stanf.toradians(q);
    qq = q*pi/180;
    qq(3) = q(3);
    tc.verifyEqual(qq, q2);

end

function trchain_test(tc)
    % TODO need to test return values here
    s = tc.TestData.p560.trchain();
    tc.verifyClass(s, 'char');

    s = tc.TestData.p560m.trchain();
    tc.verifyClass(s, 'char');
end

function jointdynamics_test(tc)
    jd = tc.TestData.p560.jointdynamics( zeros(1,6), zeros(1,6));

    tc.verifySize(jd, [1 6]);
    tc.verifyClass(jd, 'tf');
end

author: petercorke

repo: robotics-toolbox-matlab


text:
function d = chord2arclen(d, R)
% CHORD2ARCLEN  Euclidean chord length distances to sphere arc length
% distances.
%
% DARCLEN = chord2arclen(DCHORD, R)
%
%   DCHORD is a scalar, vector, matrix or general array with Euclidean
%   chord length distances. Units are metres.
%
%   R is the radius of the sphere. Units are metres. By default, R=1.
%
%   DARCLEN has the same size as DCHORD, with the corresponding geodesic
%   (great circle) distances. Units are metres.
%
% See also: arclen2chord.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 2);
nargoutchk(0, 1);

% defaults
if (nargin < 2 || isempty(R))
    R = 1.0;
end

% convert chord length distances to arc length distances (both in metres)
d = 2*R * asin(d/(2*R));

author: vigente

repo: gerardus

save_path: github-repos/MATLAB/vigente-gerardus


text:
function [ d2q ] = gen_InverseDynamics(q,Pcii,Icii,mcii,dq,taw)
%% This function is used to calculate the inverse dynamics of the KUKA iiwa 7  R 800
% This function proposes that the robot is mounted with the base in the
% horizontal poistion.

% Arreguments:
%--------------------
% q: is 1x7 vector, joint nagles vector of the manipulator
% dq: is 1x7 vector, joint angular velocity vector
% taw: 1x7 vector, the torques on the joints due to the direct dynamics.
% Pcii: is 3X7 matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii: is (3x3x7) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii: is (1x7) vector, each element of which specifies a mass of one of
% the links

% Return value:
%--------------------
% d2q: is 7x1 vector, joint angular acceleration vector

% Copyright: Mohammad SAFEEA, 9th-April-2018

[M]=gen_MassMatrix(q,Pcii,Icii,mcii);
[B]=gen_CoriolisMatrix(q,Pcii,Icii,mcii,dq);
[ G ] = gen_GravityVector(q,Pcii,mcii);
% convert angular velocity/acceleration into column vectors
dq=columnVec(dq);
taw=columnVec(taw);
d2q=M\(taw-B*dq-G);
end

function y=columnVec(x)
if(size(x,2)==1)
    y=x;
else
    y=x';
end
end

repo: KST-Kuka-Sunrise-Toolbox


text:
function [eigVec, eigVal, mmPerVox] = dtiLoadTensor(vectorsBasename, tensorBasename, b0Basename)
% [eigVec, eigVal, mmPerVox] = dtiLoadTensor([vectorsBasename], [tensorBasename], [b0Basename])
%
% 'eigVec' is a 5d array- a 3d array of 3x3 eigenVectors.
% To get the eigenVector for a particular voxel, use: v = eigVec(x,y,z,:,:);
%
% If you also provide the 'Tensor.float' file, then the eigen values are
% also returned (eigVal).
%
% If you provide the B0 filename, then we can get some useful info from the
% header, like the voxel dimensions.
%
% Loads the 3x3 matrix from the tensorcalc-format 'Vectors.float' file.
% The Vectors.float files are 32-bit floating point arrays (raw data- no header)
% with 9 images per file:
%   X1, Y1, Z1 = (signed) components of the eigenvector corresponding to the
%                largest eigenvalue.
%   X2, Y2, Z2 = (signed) components of the eigenvector corresponding to the
%                middle eigenvalue.
%   X3, Y3, Z3 = (signed) components of the eigenvector corresponding to the
%                smallest eigenvalue.
% All vectors are unit vectors.
%
%
% HISTORY:
% 2002.08.16 RFD (bobd@stanford.edu) Wrote it.
% 2003.11.13 RFD changed to return eigen vectors and values separately.
% 2003.11.26 RFD- we now get some useful info from the B0 header, if it's available.
%
% TODO:
%
%
% EXAMPLE:
% [eigVec,eigVal] = dtiLoadTensor('Vectors.float','Tensor.float');
% meanDiffusivity = sum(eigVal,4)/3;
% stdevDiffusivity = sqrt(sum((eigVal-repmat(meanDiffusivity,[1,1,1,3])).^2,4));
% normDiffusivity = sqrt(sum(eigVal.^2,4));
% fa = sqrt(3/2).*(stdevDiffusivity./normDiffusivity);
%

if(~exist('vectorsBasename', 'var') | isempty(vectorsBasename))
    [f,p] = uigetfile({'*.001';'*.*'},'Select a slice from the Vectors file...');
    vectorsBasename = fullfile(p,f(1:end-3));
    tensorBasename = fullfile(p,'Tensor.float.');
    b0Basename = fullfile(p,'B0.');
end
if(~exist('tensorBasename', 'var') | isempty(tensorBasename))
    tensorBasename = fullfile(fileparts(vectorsBasename),'Tensor.float.');
else
    if(tensorBasename(end) ~= '.')
        tensorBasename = [tensorBasename '.'];
    end
end
if(~exist('b0Basename', 'var') | isempty(b0Basename))
    b0Basename = fullfile(fileparts(vectorsBasename),'B0.');
else
    if(b0Basename(end) ~= '.')
        b0Basename = [b0Basename '.'];
    end
end

if(vectorsBasename(end) ~= '.')
    vectorsBasename = [vectorsBasename '.'];
end

nSlices = length(dir([vectorsBasename,'*']));
if(~isempty(b0Basename))
    [su,ex,se,im] = readIfileHeader([b0Basename,'001']);
    imdim = [im.imatrix_X, im.imatrix_Y, nSlices];
    mmPerVox = [im.pixsize_X, im.pixsize_Y, im.slthick];
else
    warning('B0 file not provided- guessing parameters!');
    mmPerVox = [260/128 260/128 3];
    % Now try to guess the image size
    d = dir([vectorsBasename,'001']);
    % OK- there are 9*4=36 bytes per voxel -> d.bytes/36 voxels. log2 tells us
    % how many bits this requires. We assume that the bits are divided equally
    % between X and Y (thus the /2), and then convert from bits to decimal (2^)
    xySize = 2^(log2(d.bytes./36)./2);
    imdim = [xySize,xySize,nSlices];
    disp(['I''m guessing that the image dimension is [',num2str(imdim),'].']);
end

npix = imdim(1)*imdim(2);
eigVec = zeros([imdim,3,3]);
if(~isempty(tensorBasename))
    eigVal = zeros([imdim,3]);
else
    eigVal = [];
end
disp(['Reading ',num2str(nSlices),' slices...']);
for(ii=1:imdim(3))
    fname = sprintf('%s%03d', vectorsBasename, ii);
    fid = fopen(fname, 'rb', 'ieee-be');
    eigVec(:,:,ii,:,:) = reshape(fread(fid, inf, 'float32'),[imdim(1),imdim(2),1,3,3]);
    fclose(fid);
    % Unfortunately, tensorcalc stores only the unit vectors in the Vectors
    % file. So, we need to open up the corresponding Tensor file and get
    % the eigenvalues from there.
    if(~isempty(tensorBasename))
        fname = sprintf('%s%03d', tensorBasename, ii);
        % 2004.03.26 RFD: Data now in ieee-le format?
        fid = fopen(fname, 'rb', 'ieee-be');
        % The eigen values are in the 2:4 sections of the Tensor.float
        % file.  Here, we just read the first four sections.
        tmp = fread(fid, npix*4, 'float32');
        % Drop the first section, since it's the mean diffusivity.
        tmp = tmp(npix+1:end);
        fclose(fid);
        eigVal(:,:,ii,1) = reshape(tmp(1:npix), imdim(1), imdim(2));
        eigVal(:,:,ii,2) = reshape(tmp(npix+1:npix*2), imdim(1), imdim(2));
        eigVal(:,:,ii,3) = reshape(tmp(npix*2+1:npix*3), imdim(1), imdim(2));
    end
end
return;

% Genreal notes about tensorcalc data:
% Each slice is in a separate file. Included are two different data file types,
% Tensor.float and Vectors.float. Tensor.float contains most of the info that
% you would need to do fiber tracing, except that it does not contain the
% eigenvectors corresponding to the two smaller eigenvalues. Vectors.float
% contains all 3 eigenvectors. (For more info about these files, see
% http://sirl.stanford.edu/dwi/maj/)
%
% Tensor.float files are 32-bit floating point arrays (raw data- no header)
% containing the following maps:
%
% 1. Mean diffusivity (or Trace/3, units x106 mm2/s)
% 2. Maximum eigenvalue
% 3. Medium eigenvalue
% 4. Minimum eigenvalue
% 5. X component of the maximum eigenvector (values between -1 and 1)
% 6. Y component of the maximum eigenvector
% 7. Z component of the maximum eigenvector
% 8. Fractional Anisotropy, FA (multiplied by 1000)
% 9. Lattice index, Add (if asked for via -Add option)
% 10. Coherence index, CI (if asked for via -CI option)
% 11. b=0 image (arbitrary units)
%
% Sample matlab code to read these files:
%
% baseName = 'Tensor.float.';
% imDim = [128,128,38];
% meanDiffusion = zeros(imDim);
% maxEigVal = zeros(imDim);
% medEigVal = zeros(imDim);
% minEigVal = zeros(imDim);
% maxEigValX = zeros(imDim);
% maxEigValY = zeros(imDim);
% maxEigValZ = zeros(imDim);
% fa = zeros(imDim);
% for(ii=1:imDim(3))
%     fname = sprintf('%s%03d', baseName, ii);
%     fid = fopen(fname, 'rb', 'ieee-be');
%     tmp = reshape(fread(fid, inf, 'float32'),[imDim(1),imDim(2),11]);
%     fclose(fid);
%     meanDiffusion(:,:,ii) = tmp(:,:,1);
%     maxEigVal(:,:,ii) = tmp(:,:,2);
%     medEigVal(:,:,ii) = tmp(:,:,3);
%     minEigVal(:,:,ii) = tmp(:,:,4);
%     maxEigValX(:,:,ii) = tmp(:,:,5);
%     maxEigValY(:,:,ii) = tmp(:,:,6);
%     maxEigValZ(:,:,ii) = tmp(:,:,7);
%     fa(:,:,ii) = tmp(:,:,8);
%     b0(:,:,ii) = tmp(:,:,11);
% end