7 files changed, 703 insertions, 0 deletions

diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m
new file mode 100755
index 0000000..3ddb232
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m
@@ -0,0 +1,26 @@
+%Functions related to the assignment problem.
+%Version 1.0, 25-May-1999.
+%
+%Copyright (c) 1995-1999 Niclas Borlin, Dept. of Computing Science, 
+%                        Umea University, SE-901 87 UMEA, Sweden.
+%                        Niclas.Borlin@cs.umu.se. 
+%                        www.cs.umu.se/~niclas.
+%
+%All standard disclaimers apply.
+%
+%You are free to use this code as you wish.  If you use it for a
+%publication or in a commercial package, please include an
+%acknowledgement and/or at least send me an email. (Looks good in my CV :-).
+%
+%Main functions:
+%   hungarian - calculate a solution of the square assignment
+%               problem. See HELP for a reference.
+%   condass   - calculate a condition number of the solution to the
+%               assignment problem. See HELP for a reference.
+%   allcosts  - calculate the costs of all possible assignments.
+%   allperms  - calculate all possible permutations/assignments of a
+%               given problem size.
+%
+%Test/demo functions.
+%   demo      - short demonstration of the main functions.
+%   test      - test/verification of the main functions.
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m
new file mode 100755
index 0000000..ffdb8b9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m
@@ -0,0 +1,17 @@
+function [c,p]=allcosts(C)
+%ALLCOSTS Calculate all costs for an assignment problem.
+%
+%[c,p]=allcosts(C)
+%c returns the costs, p the corresponding permutations.
+
+% v1.0  95-07-18. Niclas Borlin, niclas@cs.umu.se.
+
+p=allperm(size(C,1));
+
+c=zeros(size(p,1),1);
+
+I=eye(size(C,1));
+
+for i=1:size(p,1)
+        c(i)=sum(C(logical(sparse(p(i,:),1:size(C,1),1))));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m
new file mode 100755
index 0000000..b8d419e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m
@@ -0,0 +1,17 @@
+function p=allperm(n)
+%ALLPERM All permutation matrix.
+%
+%p=allperm(n)
+%Returns a matrix with all permutations of 1:n stored row-wise.
+
+% v1.0  95-07-18. Niclas Borlin, niclas@cs.umu.se.
+
+if (n<=1)
+        p=1;
+else
+        q=allperm(n-1);
+        p=[];
+        for i=1:n
+                p=[p;i*ones(size(q,1),1) q+(q>=i)];
+        end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m
new file mode 100755
index 0000000..82552e7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m
@@ -0,0 +1,54 @@
+function [k,C1,T1,C2,T2]=condass(A)
+%CONDASS Calculate condition number of the assigment problem.
+%
+%[k,C1,T1,C2,T2]=condass(A)
+%A  - A square cost matrix.
+%k  - The condition number of the assigment problem.
+%C1 - The best assigment.
+%T1 - The lowest cost.
+%C2 - The second best assignment.
+%T2 - The second lowest cost.
+%
+%The condition number is calculated as the relative difference between
+%the best and second best solutions, as described in Nystrom, Soderkvist,
+%and Wedin, "A Note on some Identification Problems Arising in Roentgen
+%Stereo Photogrammetric Analysis", J of Biomechanics, 27(10):1291-1294,
+%1994.
+
+% v1.0  96-09-14. Niclas Borlin, niclas@cs.umu.se.
+
+% A substantial effort was put into this code. If you use it for a
+% publication or otherwise, please include an acknowledgement and notify
+% me by email. /Niclas
+
+% Create a large number used to block selected assignments.
+big=sum(sum(A))+1;
+
+% Get best assigment.
+[C1,T1]=hungarian(A);
+
+% Initialize second best solution.
+T2=inf;
+C2=zeros(size(C1));
+
+% Create a work matrix.
+B=A;
+for i=1:length(C1)
+    % Block assigment in column i.
+    B(C1(i),i)=big;
+    % Get best assigment with this one blocked.
+    [C,T]=hungarian(B);
+    if (T<T2)
+                % Remember it if it's the best so far.
+                T2=T;
+                C2=C;
+    end
+    % Remove blocking in column i.
+    B(C1(i),i)=A(C1(i),i);
+end
+
+% Calculate difference...
+mu=T2-T1;
+
+% ...and condition number.
+k=T1/mu;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m
new file mode 100755
index 0000000..2d34e37
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m
@@ -0,0 +1,38 @@
+A=magic(10);
+B=A(4:7,4:7);
+disp('Cost matrix:')
+disp(B)
+disp('Calculating best assignment...');
+[c,t]=hungarian(B);
+disp('Best assignment (as row indices):')
+disp(c)
+disp('Best assignment (as logical matrix):')
+disp(logical(full(sparse(c,1:4,1))))
+disp('Lowest cost:')
+disp(t)
+
+disp(sprintf('\nCalculating condition number for solution...'));
+[k,c1,t1,c2,t2]=condass(B);
+disp('Lowest cost (should be same as above): ')
+disp(t1)
+disp('corresponding assignment (should be same as above):')
+disp(c1)
+disp('Second lowest cost: ')
+disp(t2)
+disp('corresponding assignment:')
+disp(c2)
+disp('Condition number for solution:')
+disp(k)
+
+disp(sprintf('\nCalculating all possible costs...'));
+[c,p]=allcosts(B);
+% Sort by cost.
+[y,i]=sort(c);
+disp('The three lowest costs:')
+disp(c(i(1:3)))
+disp('Corresponding assignments:')
+disp(p(i(1:3),:))
+disp('The three highest costs:')
+disp(c(i(end+[-2:0])))
+disp('Corresponding assignments:')
+disp(p(i(end+[-2:0]),:))
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m
new file mode 100755
index 0000000..0b493f0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m
@@ -0,0 +1,464 @@
+function [C,T]=hungarian(A)
+%HUNGARIAN Solve the Assignment problem using the Hungarian method.
+%
+%[C,T]=hungarian(A)
+%A - a square cost matrix.
+%C - the optimal assignment.
+%T - the cost of the optimal assignment.
+
+% Adapted from the FORTRAN IV code in Carpaneto and Toth, "Algorithm 548:
+% Solution of the assignment problem [H]", ACM Transactions on
+% Mathematical Software, 6(1):104-111, 1980.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+%                 Department of Computing Science, Umeå University,
+%                 Sweden. 
+%                 All standard disclaimers apply.
+
+% A substantial effort was put into this code. If you use it for a
+% publication or otherwise, please include an acknowledgement or at least
+% notify me by email. /Niclas
+
+[m,n]=size(A);
+
+if (m~=n)
+    error('HUNGARIAN: Cost matrix must be square!');
+end
+
+% Save original cost matrix.
+orig=A;
+
+% Reduce matrix.
+A=hminired(A);
+
+% Do an initial assignment.
+[A,C,U]=hminiass(A);
+
+% Repeat while we have unassigned rows.
+while (U(n+1))
+    % Start with no path, no unchecked zeros, and no unexplored rows.
+    LR=zeros(1,n);
+    LC=zeros(1,n);
+    CH=zeros(1,n);
+    RH=[zeros(1,n) -1];
+    
+    % No labelled columns.
+    SLC=[];
+    
+    % Start path in first unassigned row.
+    r=U(n+1);
+    % Mark row with end-of-path label.
+    LR(r)=-1;
+    % Insert row first in labelled row set.
+    SLR=r;
+    
+    % Repeat until we manage to find an assignable zero.
+    while (1)
+        % If there are free zeros in row r
+        if (A(r,n+1)~=0)
+            % ...get column of first free zero.
+            l=-A(r,n+1);
+            
+            % If there are more free zeros in row r and row r in not
+            % yet marked as unexplored..
+            if (A(r,l)~=0 & RH(r)==0)
+                % Insert row r first in unexplored list.
+                RH(r)=RH(n+1);
+                RH(n+1)=r;
+                
+                % Mark in which column the next unexplored zero in this row
+                % is.
+                CH(r)=-A(r,l);
+            end
+        else
+            % If all rows are explored..
+            if (RH(n+1)<=0)
+                % Reduce matrix.
+                [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
+            end
+            
+            % Re-start with first unexplored row.
+            r=RH(n+1);
+            % Get column of next free zero in row r.
+            l=CH(r);
+            % Advance "column of next free zero".
+            CH(r)=-A(r,l);
+            % If this zero is last in the list..
+            if (A(r,l)==0)
+                % ...remove row r from unexplored list.
+                RH(n+1)=RH(r);
+                RH(r)=0;
+            end
+        end
+        
+        % While the column l is labelled, i.e. in path.
+        while (LC(l)~=0)
+            % If row r is explored..
+            if (RH(r)==0)
+                % If all rows are explored..
+                if (RH(n+1)<=0)
+                    % Reduce cost matrix.
+                    [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
+                end
+                
+                % Re-start with first unexplored row.
+                r=RH(n+1);
+            end
+            
+            % Get column of next free zero in row r.
+            l=CH(r);
+            
+            % Advance "column of next free zero".
+            CH(r)=-A(r,l);
+            
+            % If this zero is last in list..
+            if(A(r,l)==0)
+                % ...remove row r from unexplored list.
+                RH(n+1)=RH(r);
+                RH(r)=0;
+            end
+        end
+        
+        % If the column found is unassigned..
+        if (C(l)==0)
+            % Flip all zeros along the path in LR,LC.
+            [A,C,U]=hmflip(A,C,LC,LR,U,l,r);
+            % ...and exit to continue with next unassigned row.
+            break;
+        else
+            % ...else add zero to path.
+            
+            % Label column l with row r.
+            LC(l)=r;
+            
+            % Add l to the set of labelled columns.
+            SLC=[SLC l];
+            
+            % Continue with the row assigned to column l.
+            r=C(l);
+            
+            % Label row r with column l.
+            LR(r)=l;
+            
+            % Add r to the set of labelled rows.
+            SLR=[SLR r];
+        end
+    end
+end
+
+% Calculate the total cost.
+T=sum(orig(logical(sparse(C,1:size(orig,2),1))));
+
+
+function A=hminired(A)
+%HMINIRED Initial reduction of cost matrix for the Hungarian method.
+%
+%B=assredin(A)
+%A - the unreduced cost matris.
+%B - the reduced cost matrix with linked zeros in each row.
+
+% v1.0  96-06-13. Niclas Borlin, niclas@cs.umu.se.
+
+[m,n]=size(A);
+
+% Subtract column-minimum values from each column.
+colMin=min(A);
+A=A-colMin(ones(n,1),:);
+
+% Subtract row-minimum values from each row.
+rowMin=min(A')';
+A=A-rowMin(:,ones(1,n));
+
+% Get positions of all zeros.
+[i,j]=find(A==0);
+
+% Extend A to give room for row zero list header column.
+A(1,n+1)=0;
+for k=1:n
+    % Get all column in this row. 
+    cols=j(k==i)';
+    % Insert pointers in matrix.
+    A(k,[n+1 cols])=[-cols 0];
+end
+
+
+function [A,C,U]=hminiass(A)
+%HMINIASS Initial assignment of the Hungarian method.
+%
+%[B,C,U]=hminiass(A)
+%A - the reduced cost matrix.
+%B - the reduced cost matrix, with assigned zeros removed from lists.
+%C - a vector. C(J)=I means row I is assigned to column J,
+%              i.e. there is an assigned zero in position I,J.
+%U - a vector with a linked list of unassigned rows.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+[n,np1]=size(A);
+
+% Initalize return vectors.
+C=zeros(1,n);
+U=zeros(1,n+1);
+
+% Initialize last/next zero "pointers".
+LZ=zeros(1,n);
+NZ=zeros(1,n);
+
+for i=1:n
+    % Set j to first unassigned zero in row i.
+        lj=n+1;
+        j=-A(i,lj);
+
+    % Repeat until we have no more zeros (j==0) or we find a zero
+        % in an unassigned column (c(j)==0).
+    
+        while (C(j)~=0)
+                % Advance lj and j in zero list.
+                lj=j;
+                j=-A(i,lj);
+        
+                % Stop if we hit end of list.
+                if (j==0)
+                        break;
+                end
+        end
+
+        if (j~=0)
+                % We found a zero in an unassigned column.
+                
+                % Assign row i to column j.
+                C(j)=i;
+                
+                % Remove A(i,j) from unassigned zero list.
+                A(i,lj)=A(i,j);
+
+                % Update next/last unassigned zero pointers.
+                NZ(i)=-A(i,j);
+                LZ(i)=lj;
+
+                % Indicate A(i,j) is an assigned zero.
+                A(i,j)=0;
+        else
+                % We found no zero in an unassigned column.
+
+                % Check all zeros in this row.
+
+                lj=n+1;
+                j=-A(i,lj);
+                
+                % Check all zeros in this row for a suitable zero in another row.
+                while (j~=0)
+                        % Check the in the row assigned to this column.
+                        r=C(j);
+                        
+                        % Pick up last/next pointers.
+                        lm=LZ(r);
+                        m=NZ(r);
+                        
+                        % Check all unchecked zeros in free list of this row.
+                        while (m~=0)
+                                % Stop if we find an unassigned column.
+                                if (C(m)==0)
+                                        break;
+                                end
+                                
+                                % Advance one step in list.
+                                lm=m;
+                                m=-A(r,lm);
+                        end
+                        
+                        if (m==0)
+                                % We failed on row r. Continue with next zero on row i.
+                                lj=j;
+                                j=-A(i,lj);
+                        else
+                                % We found a zero in an unassigned column.
+                        
+                                % Replace zero at (r,m) in unassigned list with zero at (r,j)
+                                A(r,lm)=-j;
+                                A(r,j)=A(r,m);
+                        
+                                % Update last/next pointers in row r.
+                                NZ(r)=-A(r,m);
+                                LZ(r)=j;
+                        
+                                % Mark A(r,m) as an assigned zero in the matrix . . .
+                                A(r,m)=0;
+                        
+                                % ...and in the assignment vector.
+                                C(m)=r;
+                        
+                                % Remove A(i,j) from unassigned list.
+                                A(i,lj)=A(i,j);
+                        
+                                % Update last/next pointers in row r.
+                                NZ(i)=-A(i,j);
+                                LZ(i)=lj;
+                        
+                                % Mark A(r,m) as an assigned zero in the matrix . . .
+                                A(i,j)=0;
+                        
+                                % ...and in the assignment vector.
+                                C(j)=i;
+                                
+                                % Stop search.
+                                break;
+                        end
+                end
+        end
+end
+
+% Create vector with list of unassigned rows.
+
+% Mark all rows have assignment.
+r=zeros(1,n);
+rows=C(C~=0);
+r(rows)=rows;
+empty=find(r==0);
+
+% Create vector with linked list of unassigned rows.
+U=zeros(1,n+1);
+U([n+1 empty])=[empty 0];
+
+
+function [A,C,U]=hmflip(A,C,LC,LR,U,l,r)
+%HMFLIP Flip assignment state of all zeros along a path.
+%
+%[A,C,U]=hmflip(A,C,LC,LR,U,l,r)
+%Input:
+%A   - the cost matrix.
+%C   - the assignment vector.
+%LC  - the column label vector.
+%LR  - the row label vector.
+%U   - the 
+%r,l - position of last zero in path.
+%Output:
+%A   - updated cost matrix.
+%C   - updated assignment vector.
+%U   - updated unassigned row list vector.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+n=size(A,1);
+
+while (1)
+    % Move assignment in column l to row r.
+    C(l)=r;
+    
+    % Find zero to be removed from zero list..
+    
+    % Find zero before this.
+    m=find(A(r,:)==-l);
+    
+    % Link past this zero.
+    A(r,m)=A(r,l);
+    
+    A(r,l)=0;
+    
+    % If this was the first zero of the path..
+    if (LR(r)<0)
+        ...remove row from unassigned row list and return.
+        U(n+1)=U(r);
+        U(r)=0;
+        return;
+    else
+        
+        % Move back in this row along the path and get column of next zero.
+        l=LR(r);
+        
+        % Insert zero at (r,l) first in zero list.
+        A(r,l)=A(r,n+1);
+        A(r,n+1)=-l;
+        
+        % Continue back along the column to get row of next zero in path.
+        r=LC(l);
+    end
+end
+
+
+function [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
+%HMREDUCE Reduce parts of cost matrix in the Hungerian method.
+%
+%[A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
+%Input:
+%A   - Cost matrix.
+%CH  - vector of column of 'next zeros' in each row.
+%RH  - vector with list of unexplored rows.
+%LC  - column labels.
+%RC  - row labels.
+%SLC - set of column labels.
+%SLR - set of row labels.
+%
+%Output:
+%A   - Reduced cost matrix.
+%CH  - Updated vector of 'next zeros' in each row.
+%RH  - Updated vector of unexplored rows.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+n=size(A,1);
+
+% Find which rows are covered, i.e. unlabelled.
+coveredRows=LR==0;
+
+% Find which columns are covered, i.e. labelled.
+coveredCols=LC~=0;
+
+r=find(~coveredRows);
+c=find(~coveredCols);
+
+% Get minimum of uncovered elements.
+m=min(min(A(r,c)));
+
+% Subtract minimum from all uncovered elements.
+A(r,c)=A(r,c)-m;
+
+% Check all uncovered columns..
+for j=c
+    % ...and uncovered rows in path order..
+    for i=SLR
+        % If this is a (new) zero..
+        if (A(i,j)==0)
+            % If the row is not in unexplored list..
+            if (RH(i)==0)
+                % ...insert it first in unexplored list.
+                RH(i)=RH(n+1);
+                RH(n+1)=i;
+                % Mark this zero as "next free" in this row.
+                CH(i)=j;
+            end
+            % Find last unassigned zero on row I.
+            row=A(i,:);
+            colsInList=-row(row<0);
+            if (length(colsInList)==0)
+                % No zeros in the list.
+                l=n+1;
+            else
+                l=colsInList(row(colsInList)==0);
+            end
+            % Append this zero to end of list.
+            A(i,l)=-j;
+        end
+    end
+end
+
+% Add minimum to all doubly covered elements.
+r=find(coveredRows);
+c=find(coveredCols);
+
+% Take care of the zeros we will remove.
+[i,j]=find(A(r,c)<=0);
+
+i=r(i);
+j=c(j);
+
+for k=1:length(i)
+    % Find zero before this in this row.
+    lj=find(A(i(k),:)==-j(k));
+    % Link past it.
+    A(i(k),lj)=A(i(k),j(k));
+    % Mark it as assigned.
+    A(i(k),j(k))=0;
+end
+
+A(r,c)=A(r,c)+m;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m
new file mode 100755
index 0000000..4c238f2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m
@@ -0,0 +1,87 @@
+disp('Testing hungarian...');
+A=magic(10);
+B=A(4:7,4:7);
+[c,t]=hungarian(B);
+if (any(c~=[2 1 3 4]))
+        disp('Wrong coupling!');
+elseif (t~=77)
+        disp('Wrong solution!');
+else
+        disp('Hungarian appears OK.');
+end
+
+disp('Testing condass...');
+[k,c1,t1,c2,t2]=condass(B);
+if (any(c1~=[2 1 3 4]))
+        disp('Wrong best coupling!');
+elseif (t1~=77)
+        disp('Wrong lowest cost!');
+elseif (any(c2~=[1 2 3 4]))
+        disp('Wrong second best coupling!');
+elseif (t2~=102)
+        disp('Wrong second lowest cost!');
+elseif (k~=t1/(t2-t1))
+        disp('Wrong condition number!');
+else
+        disp('condass appears OK.');
+end
+
+disp('Testing allcosts...');
+[c,p]=allcosts(B);
+cTrue=[ 102
+                127
+                202
+                227
+                222
+                222
+                77
+                102
+                182
+                202
+                202
+                197
+                177
+                202
+                182
+                202
+                302
+                297
+                202
+                202
+                207
+                202
+                307
+                302
+];
+pTrue=[ 1     2     3     4
+                1     2     4     3
+                1     3     2     4
+                1     3     4     2
+                1     4     2     3
+                1     4     3     2
+                2     1     3     4
+                2     1     4     3
+                2     3     1     4
+                2     3     4     1
+                2     4     1     3
+                2     4     3     1
+                3     1     2     4
+                3     1     4     2
+                3     2     1     4
+                3     2     4     1
+                3     4     1     2
+                3     4     2     1
+                4     1     2     3
+                4     1     3     2
+                4     2     1     3
+                4     2     3     1
+                4     3     1     2
+                4     3     2     1
+          ];
+if (any(c~=cTrue))
+        disp('Wrong costs!');
+elseif (any(p~=pTrue))
+        disp('Wrong permutations!');
+else    
+        disp('allcosts appears OK.');
+end