estshhemm24.m | searchcode

/code/DTCWTregistration/estshhemm24.m

http://sparse-mri-reconstruction.googlecode.com/
MATLAB | 728 lines | 438 code | 74 blank | 216 comment | 41 complexity | ce9f3fd54091236e3a5534f419de6dc2 MD5 | raw file

function [shift1, shift2, shift_common,del1, del2, del_common,av1,av2,av_common, ...

    av1_s,av2_s,av_common_s,qvecs1,cond1,cond2,cond_common] = estshhemm24(refl, refh, prevl, prevh, ...

   w, nit, levelsel, search, debug, mode, qscale, avlevel, sizeqfilt, mask)

%TDR attempts to perform registration in both directions, ensuring that the

%error (optical flow) and error (data) is minimized.



% function [shift,delta,av,qvecs] = estshhemm22(refl, refh, prevl, prevh, ...

%    w, nit, levelsel, search, debug, mode, qscale, avlevel, sizeqfilt, mask)

%

% Estimate the shift between elements of the reference lowpass and

% highpass subimages, refl and refh, and the previous subimages, 

% prevl and prevh, at given levels of the dual-tree CWT.

% This version used Magnus Hemmendorf's affine vector field model

% from his 2002 Trans Med Imaging paper: 'Phase-based multi-dim volume

% registration'.

%

% refl and prevl are real lowpass subimages oversampled 2:1 each way.

% refh and prevh are cells of sets of 6 complex bandpass subimages in 3-D arrays.

% w(1:2) contains the expected phase rotations per sample 

% for the low and high band 1-D filters (in radians).

% nit is the number of iterations of the algorithm.

% levelsel selects the range of CWT levels used at each iteration:

% such that  levelsel(iter,1:2)  are the coarsest and finest levels

% used at iteration  iter.

% The default nit = 1.

% search  defines which offsets from the zero shift point will be tested

% in the initial search phase of the algorithm.  It is a column vector

% of complex elements representing the shift offsets.

%

% debug is an m x 2 matrix, specifying that element(s) debug(:,1),debug(:,2)

% in the subimages should be printed out for debugging. 

%

% shift is a complex matrix with 1+1j representing the lower right

% corner of a unit square (since vertical distances are measured

% downwards in images).  The shift vector points from a sampling

% point in ref to the equivalent point in prev.

% delta is a matrix giving the height of the error surface minimum

% at each point.

% av(:,:,1:6) is a 3-D array of affine vectors at each point.

% qvecs(:,:,1:28) is a 3-D array of q-vectors at each point. 

%

% mode(1)>0 causes a single affine vector to be used for the whole image

%  for iterations up to mode(1).

% mode(2)=1 removes the constraints for the outer edge of avecs

% mode(2)=2 removes the constraints for the outer edge of avecs on only the

%  subbands which are not normal to the edge of the image.

% mode(3)=1 removes constraints from bands 2 and 5 everywhere.

% mode(4)=0 for pure affine model, 1 for (affine + xy) model, 2 for

%  (affine + linear zoom) model, 3 for 10-term affine + linear zoom model,

% TDR 4 for translation-only model

% mode(5)=0.5 for modified zoom (1.0 otherwise).

% qscale defines the scale factor for the quiver plots, and equals 1 by default.

% avlevel is the level of the DTCWT subbands which have the same resolution

%  as the avec matrix.

% sizeqfilt is the size of the Q matrix smoothing filter 

%  (no. of taps = 2*sizeqfilt + 1).

% mask is a gain matrix for scaling the contraint vectors.  It should be

% the size of the coarsest subbands - 1.  It allows parts of the image to be ignored

% where mask = 0, and is usually = 1 elsewhere.

%

% esthemmm2 uses constraints2 for a more dense constraint vector field.

% The July 2004 version allows the avec field to be at finer resolution

%  than the coarsest DT CWT subband resolution.

%

% Nick Kingsbury, Cambridge University, July 2004.



if nargin < 14, mask = 1; end

if nargin < 13, sizeqfilt = 2; end

if nargin < 12, avlevel = length(refh); end

if nargin < 11, qscale = 1; end

if nargin < 10, mode = 0; end

if length(mode) < 5, mode = [mode 0 0 0 0]; end 



if nargin < 9, debug = []; end



if nargin < 8, search = 0; end



fig = gcf + 1;

plot_disp=0; %turns plotting of shift vectors on/off

% Check matrix sizes.

slo = size(refl);

shi = size(refh{end});

if any(size(prevl)~=slo | slo~=2*shi(1:2)) | any(size(prevh{end})~=shi) | (shi(3)~=6),

  error('Input matrices not correct sizes.');

end



% Mask matrix should be size of coarsest high bands - 1.

if length(mask)==1, mask = mask*ones(shi(1:2)-1); end

 

% Subimages are listed in order of increasing angle of rotation

% from 0 to pi, starting at 15 deg anticlockwise from a horizontal edge,

% and increasing by 30 deg each time.



nd = size(debug,1);

if nd > 0, di = debug(:,1) + (debug(:,2) - 1) * sc;

else       di = [];

end

% mat2scr([debug di],'%8d','ESTSHIFT debug at pel:')

% mat2scr(r6(di,:),'%8.2f','r6:')



% p6 = prevh;



% Calc the expected horiz and vert phase shifts for each subband.

w6 = [w(1)  w(2); w(2)  w(2); w(2)  w(1); w(2) -w(1); w(2) -w(2); w(1) -w(2)];



nsch = length(search);

%affine vector cell arrays for multiple shifts

av1_s=cell(nsch,1);

av2_s=cell(nsch,1);

av_common_s=cell(nsch,1);



if mode(4) == 0, lenavec = 6;

elseif mode(4) == 3, lenavec = 10;

elseif mode(4) == 4, lenavec = 2;

else lenavec = 8;

end



% Loop for each search offset.

for sch = 1:nsch,

   

   srefh = size(refh{avlevel});

   sc = srefh(1);

   sr = srefh(2);

   

   % Initialise affine vector to search offset.

   av1 = zeros(sc-1,sr-1,lenavec);

   av1(:,:,2) = real(search(sch));

   av1(:,:,1) = imag(search(sch));

  

   av2 = zeros(sc-1,sr-1,lenavec);

   av2(:,:,2) = -real(search(sch));

   av2(:,:,1) = -imag(search(sch));

   

   av_common = zeros(sc-1,sr-1,lenavec);

   av_common(:,:,2) = real(search(sch));

   av_common(:,:,1) = imag(search(sch));

   delta1 = [];

   

   % Perform nit iterations on shift and del so that del is more accurate.

   for it = 1:nit,

      levels = levelsel(min(it,size(levelsel,1)),:);

      qvecsum1 = 0;

      qvecsum2 = 0;

      qvecsum_common = 0;

      % Loop for each level at the current iteration.

      for lev = levels(1):-1:levels(2)

         

         

         % Calc. size of bandpass subimages in pels.

         srefh = size(refh{lev});

         sc = srefh(1);

         sr = srefh(2);

         ss = sc * sr;

         

         sh1 = affineshift2(av1,[sc sr],mode);  

         sh1 = affineshift2(av1,[sc sr],mode);  

         sh2 = affineshift2(av2,[sc sr],mode);  

         sh2 = affineshift2(av2,[sc sr],mode);  

         sh_common = affineshift2(av_common,[sc sr],mode);  

         sh_common = affineshift2(av_common,[sc sr],mode);  

         

         % Use sh (adjusted for pel units instead of half-image units) to shift prevh.

         if lev <= 1, method = 'lin'; else method = 'cub'; end

         prevhsh = shift_cwt_bands2(prevh{lev},real(sh1)*(sr/2) + imag(sh1)*(sqrt(-1)*sc/2),method,0);

         refhsh = shift_cwt_bands2(refh{lev},real(sh2)*(sr/2) + imag(sh2)*(sqrt(-1)*sc/2),method,0);

%          if real(search(sch))==0 && imag(search(sch))==0

%             disp('stopped here')

%             sh1

%             sh2

%             prevhsh-refhsh

%             

%             1+1

%          end

%          prevhsh-prevh{lev}

%          refhsh-refh{lev}

         % Calculate motion constraints at each point.

         % cvecs is a 4-D array in (y,x,band,cvec element).

         cvecs1 = zeros(sc-1,sr-1,6,lenavec+1);

         cvecs1(:,:,:,[1 2 lenavec+1]) = constraints2(refh{lev},prevhsh,w6,mode,mask);

         cvecs2 = zeros(sc-1,sr-1,6,lenavec+1);

         cvecs2(:,:,:,[1 2 lenavec+1]) = constraints2(prevh{lev},refhsh,w6,mode,mask);

        % cvecs

         % Add extra components which are equivalent to multiplying 

         % the constraint vectors cvecs by K.' matrices:

         % K = [1 0 y 0 x 0 xy 0 0; 0 1 0 y 0 x 0 xy 0; 0 0 0 0 0 0 0 0 1] 

         % for 2-D affine transform with extra xy terms.

         % yi and xi go from -1 to +1 across the image.

         % For the linear zoom model, columns 7 and 8 of K (the xy cols) are 

         % replaced by terms in [xy  x^2  0]' and [y^2  xy  0]' .

         s2 = sc - 1;

         for k=1:s2,

            yi = (2*k-sc) / sc;

            if mode(4)<=3 %TDR

                cvecs1(k,:,:,[3 4]) = cvecs1(k,:,:,[1 2]) * yi; 

                cvecs2(k,:,:,[3 4]) = cvecs2(k,:,:,[1 2]) * yi; 

            end

            if mode(4) == 3, 

               cvecs1(k,:,:,10) = cvecs1(k,:,:,3) * yi; 

               cvecs2(k,:,:,10) = cvecs2(k,:,:,3) * yi; 

            elseif mode(4) == 2, 

               cvecs1(k,:,:,8) = cvecs1(k,:,:,3) * (mode(5)*yi); 

               cvecs2(k,:,:,8) = cvecs2(k,:,:,3) * (mode(5)*yi); 

            end

         end

         s2 = sr - 1;

         for k=1:s2,

            xi = (2*k-sr) / sr;

            if mode(4)<=3 %TDR

                cvecs1(:,k,:,[5 6]) = cvecs2(:,k,:,[1 2]) * xi; 

                cvecs2(:,k,:,[5 6]) = cvecs2(:,k,:,[1 2]) * xi; 

            end

            if mode(4) == 3, % Linear zoom with separate x^2 and y^2 terms.

               cvecs1(:,k,:,7) = cvecs1(:,k,:,3) * xi;

               cvecs1(:,k,:,8) = cvecs1(:,k,:,4) * xi;

               cvecs1(:,k,:,9) = cvecs1(:,k,:,6) * xi;

               cvecs2(:,k,:,7) = cvecs2(:,k,:,3) * xi;

               cvecs2(:,k,:,8) = cvecs2(:,k,:,4) * xi;

               cvecs2(:,k,:,9) = cvecs2(:,k,:,6) * xi;

            elseif mode(4) == 2, % Linear zoom

               cvecs1(:,k,:,7) = cvecs1(:,k,:,3) * xi + cvecs1(:,k,:,6) * (mode(5)*xi);

               cvecs1(:,k,:,8) = cvecs1(:,k,:,4) * xi + cvecs1(:,k,:,8);

               cvecs2(:,k,:,7) = cvecs2(:,k,:,3) * xi + cvecs2(:,k,:,6) * (mode(5)*xi);

               cvecs2(:,k,:,8) = cvecs2(:,k,:,4) * xi + cvecs2(:,k,:,8);

            elseif mode(4) == 1,

               cvecs1(:,k,:,[7 8]) = cvecs1(:,k,:,[3 4]) * xi;

               cvecs2(:,k,:,[7 8]) = cvecs2(:,k,:,[3 4]) * xi;

            end

            

         end

         % cvecs now contains the terms  K' * c .

         %cvecs

         % Form the Q-matrices as vectors of the lower left half

         % of the symmetric Q-matrix.  Q = (K'*c*c'*K)

         % Combine all the constraints (subbands) at each location, ie all 6 directions.

         scv = size(cvecs1);

         c4 = scv(4);

         qvecs1 = zeros([scv(1:2) c4*(c4+1)/2]);

         qvecs2 = zeros([scv(1:2) c4*(c4+1)/2]);

         qvecs_common = zeros([scv(1:2) c4*(c4+1)/2]);

         

         

         

         iQ = zeros(c4,c4);

          

          

          

          

         k = 1;

         for k1 = 1:c4

            for k2 = k1:c4

               qvecs1(:,:,k) = sum(cvecs1(:,:,:,k1) .* cvecs1(:,:,:,k2),3); % Sum over all 6 directions.

               qvecs2(:,:,k) = sum(cvecs2(:,:,:,k1) .* cvecs2(:,:,:,k2),3); % Sum over all 6 directions.

               

               iQ(k1,k2) = k; iQ(k2,k1) = k; % indices for Q matrix and q vector.

               k = k + 1;

            end

         end

         

         %TDR extract first q matrix

          iQmat1 = iQ(1:(end-1),1:(end-1));  % Indices to convert qvec to Q.

          iqv1 = iQ(1:(end-1),end);  % Indices to convert qvec to q.

          iq01 = iQ(end,end);  % Index to convert qvec to q0.

          



          %TDR Extract second q matrix

          iQmat2 = iQ(1:(end-1),1:(end-1));  % Indices to convert qvec to Q.

          iqv2 = iQ(1:(end-1),end);  % Indices to convert qvec to q.

          iq02 = iQ(end,end);  % Index to convert qvec to q0.

         

         % Change the sample rate of qvecs, back to that of av, remembering

         % that the sizes are always odd and one less than the subband sizes.

         if lev < avlevel,

             % Reduce the sample rate.

             for k = lev:(avlevel-1)

                 % First reduce no of rows, using [0.5 1 0.5] filter.

                 t1 = 2:2:(size(qvecs1,1)-1);  

                 qvecs1 = 0.5*qvecs1(t1-1,:,:) + qvecs1(t1,:,:) + 0.5*qvecs1(t1+1,:,:);

                 qvecs2 = 0.5*qvecs2(t1-1,:,:) + qvecs2(t1,:,:) + 0.5*qvecs2(t1+1,:,:);

                 % Now reduce no of columns.

                 t2 = 2:2:(size(qvecs1,2)-1);  

                 qvecs1 = 0.5*qvecs1(:,t2-1,:,:) + qvecs1(:,t2,:) + 0.5*qvecs1(:,t2+1,:);

                 qvecs2 = 0.5*qvecs2(:,t2-1,:,:) + qvecs2(:,t2,:) + 0.5*qvecs2(:,t2+1,:);

             end

         elseif lev > avlevel,

             for k = (avlevel+1):lev

                 % First increase no of rows, using [0.5 1 0.5] filter.

                 sc = size(qvecs1,1);

                 qv1 = [];

                 qv2 = [];

                 qv1([0:sc]*2+1,:,:) = 0.5 * (qvecs1([1 1:sc],:,:) + qvecs1([1:sc sc],:,:));

                 qv1([1:sc]*2,:,:) = qvecs1;

                 qvecs1 = qv1;

                 

                 qv2([0:sc]*2+1,:,:) = 0.5 * (qvecs2([1 1:sc],:,:) + qvecs2([1:sc sc],:,:));

                 qv2([1:sc]*2,:,:) = qvecs2;

                 qvecs2 = qv2;

                 % Now increase no of columns.

                 sr = size(qvecs1,2);

                 qv1 = [];

                 qv2 = [];

                 qv1(:,[0:sr]*2+1,:) = 0.5 * (qvecs1(:,[1 1:sr],:) + qvecs1(:,[1:sr sr],:));

                 qv1(:,[1:sr]*2,:) = qvecs1;

                 qvecs1 = qv1;

                 qv2(:,[0:sr]*2+1,:) = 0.5 * (qvecs2(:,[1 1:sr],:) + qvecs2(:,[1:sr sr],:));

                 qv2(:,[1:sr]*2,:) = qvecs2;

                 qvecs2 = qv2;

             end

         end

          

         

        %TDR added this to attempt fixing bad estimates from coarse to fine 

       

         sqv1 = size(qvecs1);        

        sqv2 = size(qvecs2);  

         qvecs_common=zeros(sqv1);

if 0

       

        

        qf1 = zeros(sqv1(3),1);

        qf2 = zeros(sqv2(3),1);

        for row=1:sqv1(2)

            for col=1:sqv1(1)

                qf1(:) = qvecs1(col,row,:);

                Qmat1 = qf1(iQmat1);

                %TDR try masking bad Qmatrices from qvec accumulation

%                 if cond(-Qmat1)>1E3

%                     qvecs1(col,row,:);

%                     qvecs1(col,row,:)=zeros(size(qvecs1,3),1);

% %                     disp('bad condition here:qvec1');

%                     

%                 end

                qv1 = qf1(iqv1);

                q01 = qf1(iq01);



                qf2(:) = qvecs2(col,row,:);

                Qmat2 = qf2(iQmat2);

                %TDR try masking bad Qmatrices from qvec accumulation

%                 if cond(-Qmat2)>1E3

%                     qvecs2(col,row,:);

%                     qvecs2(col,row,:)=zeros(size(qvecs2,3),1);

% %                     disp('bad condition here:qvec2');

%                 end

%                 

                qv2 = qf2(iqv2);

                q02 = qf2(iq02);



                a1 = -Qmat1 \ qv1;                  

                avec1(col,row,:) = a1;

                a2 = -Qmat2 \ qv2;               

                avec2(col,row,:) = a2;

%                 a_common = -(Qmat1 + Qmat2) \ (qv1 - qv2);               

%                 avec_common(col,row,:) = a_common;

                

                

            end

        end

        avec1(find(isnan(avec1)))=0;

        avec2(find(isnan(avec2)))=0;

%         avec_common(find(isnan(avec_common)))=0;

        perform_reg_interp=1;

        bandlimreg=0;

        maxlevel=length(prevh);

        dtcwtparams=struct('filter','near_sym_b','qshift','qshift_d','leveldepth',maxlevel);

        [Ylf Yhf]= shift_cwt_bands_from_avec(prevl,prevh,perform_reg_interp,avec1,mode,bandlimreg);

        [Ylb Yhb]= shift_cwt_bands_from_avec(refl,refh,perform_reg_interp,-avec1,mode,bandlimreg);

        Yf=dtwaveifm2(Ylf,Yhf,dtcwtparams.filter,dtcwtparams.qshift);

        Yb=dtwaveifm2(Ylb,Yhb,dtcwtparams.filter,dtcwtparams.qshift);

%         error_surf1=abs(Yhf{avlevel}-Yhb{avlevel}).^2;        

        error_surf1=(Yf-Yb).^2;

        error_surf_sum1=error_surf1;

%         for d=1:6

%             if d==1

%                 error_surf_sum1=error_surf1(:,:,d);

%             else

%                 error_surf_sum1=error_surf_sum1+error_surf1(:,:,d);

%             end

%         end

        error_surf_sum1=refit_matrix(avec1,error_surf_sum1);

        [Ylf Yhf]= shift_cwt_bands_from_avec(prevl,prevh,perform_reg_interp,-avec2,mode,bandlimreg);

        [Ylb Yhb]= shift_cwt_bands_from_avec(refl,refh,perform_reg_interp,avec2,mode,bandlimreg);

        Yf=dtwaveifm2(Ylf,Yhf,dtcwtparams.filter,dtcwtparams.qshift);

        Yb=dtwaveifm2(Ylb,Yhb,dtcwtparams.filter,dtcwtparams.qshift);

%         error_surf2=abs(Yhf{avlevel}-Yhb{avlevel}).^2;            

        error_surf2=(Yf-Yb).^2;

        error_surf_sum2=error_surf2;

%         for d=1:6

%             if d==1

%                 error_surf_sum2=error_surf2(:,:,d);

%             else

%                 error_surf_sum2=error_surf_sum2+error_surf2(:,:,d);

%             end

%         end

        error_surf_sum2=refit_matrix(avec2,error_surf_sum2);

        

%         error_surf3=abs(refh{avlevel}-prevh{avlevel}).^2;

        Yf=dtwaveifm2(prevl,prevh,dtcwtparams.filter,dtcwtparams.qshift);

        Yb=dtwaveifm2(refl,refh,dtcwtparams.filter,dtcwtparams.qshift);

        error_surf3=(Yf-Yb).^2;

        error_surf_sum3=error_surf3;

%         for d=1:6

%             if d==1

%                 error_surf_sum3=error_surf3(:,:,d);

%             else

%                 error_surf_sum3=error_surf_sum3+error_surf3(:,:,d);

%             end

%         end

         error_surf_sum3=refit_matrix(avec2,error_surf_sum3);

        for row=1:sqv1(2)

            for col=1:sqv1(1)

                %make sure we've moved in the right direction at least...

                if 1%error_surf_sum3(col,row)>=error_surf_sum1(col,row) || error_surf_sum3(col,row)>=error_surf_sum2(col,row)

                    %pick the least data-erronious q-vec for each chi

                    if error_surf_sum1(col,row)<=error_surf_sum2(col,row)

                        qvecs_common(col,row,:)=qvecs1(col,row,:);

                    else

    %                     qvecs_common(col,row,:)=-qvecs2(col,row,:); 



                        qf2(:) = qvecs2(col,row,:);



                        Qmat2 = qf2(iQmat2);

                        qv2 = qf2(iqv2);



                        q02 = qf2(iq02);

                        a2 = -Qmat2 \ qv2;



                        if any(isnan(a2))

                            a2=zeros(size(a2,1),1);

                        else

                            a2=-a2;

                        end



                            %come-up with what qvecsum2(col,row,:) should be

                            %now

                            qv2=-Qmat2*a2;                       

                            qf2(iqv2)=qv2;

                            qvecs_common(col,row,:)=qf2(:);

    %                         qvecs1(col,row,:)-qvecs_common(col,row,:)

                    end

                else

                    %bad initial direction. hopefully the next level picks

                    %a good direction

%                     disp('bad estimate at coarse level here');

                    qvecs_common(col,row,:)=zeros(size(qvecs_common,3),1);

                end

            end

        end

       

end

        qvecsum_common = qvecsum_common + qvecs_common;

        qvecsum1 = qvecsum1 + qvecs1; % Accumulate qvecs across levels of CWT.#

        qvecsum2 = qvecsum2 + qvecs2; % Accumulate qvecs across levels of CWT.

         

      end  % End of loop for each level of CWT.

      

      % As a simple test, combine all qvecs into a single vector.

      qtot1 = squeeze(sum(sum(qvecsum1)));

      qtot2 = squeeze(sum(sum(qvecsum2)));

      qtot_common = squeeze(sum(sum(qvecsum_common)));

      

      % Filter the q vectors with a linear ramp filter.

      % Set up the 1-D filter impulse response.

      %TDR option to remove Q filter

%       if sizeqfilt(1)<=1

%         %do nothing to the q

%       else

          shq = sizeqfilt(min(it,end)) + 1; % 

          hq = [1:shq  (shq-1):-1:1].'; 

          hq = hq/sum(hq);

          for k=1:size(qvecsum1,3),

             qvecsum1(:,:,k) = colfilter(colfilter(qvecsum1(:,:,k),hq).',hq).';

             qvecsum2(:,:,k) = colfilter(colfilter(qvecsum2(:,:,k),hq).',hq).';

             qvecsum_common(:,:,k) = colfilter(colfilter(qvecsum_common(:,:,k),hq).',hq).';

             % qvecsum(:,:,k) = conv2(hqc,hqr,squeeze(qvecsum(:,:,k)),'same');

             if mode(1) >= it,

                qvecsum1(:,:,k) = qtot1(k); % Include this to use single Q matrix.

                qvecsum2(:,:,k) = qtot2(k); % Include this to use single Q matrix.

                qvecsum_common(:,:,k) = qtot_common(k); % Include this to use single Q matrix.

             end

          end

%       end

      % Now extract Q matrix, q vector and q0 scalar from each qvec vector, and

      % solve for the affine vector, a, which minimises the constraint error energy.

      iQmat1 = iQ(1:(end-1),1:(end-1));  % Indices to convert qvec to Q.

      iqv1 = iQ(1:(end-1),end);  % Indices to convert qvec to q.

      iq01 = iQ(end,end);  % Index to convert qvec to q0.

      sqv1 = size(qvecsum1);

      del1 = zeros(sqv1(1:2));

      avec1 = zeros([sqv1(1:2) lenavec]);

      avec2 = zeros([sqv1(1:2) lenavec]);

      avec_common = zeros([sqv1(1:2) lenavec]);

      qf1 = zeros(sqv1(3),1);

      

      %TDR Extract second q matrix

      iQmat2 = iQ(1:(end-1),1:(end-1));  % Indices to convert qvec to Q.

      iqv2 = iQ(1:(end-1),end);  % Indices to convert qvec to q.

      iq02 = iQ(end,end);  % Index to convert qvec to q0.

      sqv2 = size(qvecsum2);

      del2 = zeros(sqv2(1:2));

      avec2 = zeros([sqv2(1:2) lenavec]);

      qf2 = zeros(sqv2(3),1);

      

       iQmat_common = iQ(1:(end-1),1:(end-1));  % Indices to convert qvec to Q.

      iqv_common = iQ(1:(end-1),end);  % Indices to convert qvec to q.

      iq0_common = iQ(end,end);  % Index to convert qvec to q0.

      sqv_common = size(qvecsum_common);

      del_common = zeros(sqv_common(1:2));

      avec_common = zeros([sqv_common(1:2) lenavec]);

      qf_common = zeros(sqv_common(3),1);

      % Loops for each Q matrix locality.

%       zero_q_matrix_count=0; %count the numer of times a '0' constraint vector

      %causes poor conditioning of Q matrix

      cond_count1=0;

      cond_count2=0;

      cond_count3=0;

      scond=[size(avec1,1) size(avec1,2)];

      cond1=zeros(scond);

      cond2=zeros(scond);

      cond_common=zeros(scond);

      for row = 1:sqv1(2)

         for col = 1:sqv1(1)

            % Extract Q, q and q0.

            qf1(:) = qvecsum1(col,row,:);

            Qmat1 = qf1(iQmat1);

      		qv1 = qf1(iqv1);

      		q01 = qf1(iq01);

            

            qf2(:) = qvecsum2(col,row,:);

            Qmat2 = qf2(iQmat2);

      		qv2 = qf2(iqv2);

      		q02 = qf2(iq02);

            

            qf_common(:) = qvecsum_common(col,row,:);

            Qmat_common = qf_common(iQmat_common);

      		qv_common = qf_common(iqv_common);

      		q0_common = qf_common(iq0_common);

            

            % Solve for a_min: 

            %TDR

            %a = -Qmat \ qv;  

            %if (cond(Qmat)>1E3)

%             if any(isnan(a))

%                 cond(-Qmat)

%                 poor_condition_count=poor_condition_count+1;

%                 if sum(Qmat(:))<=1

%                     zero_q_matrix_count=zero_q_matrix_count+1;

%                 end

%             end

            %make sure matrix inversion is possible first..

            if cond(-Qmat1)>1E3

                a1=zeros(size(qv1));

%                 poor_condition_count=poor_condition_count+1;

            else

                a1 = -Qmat1 \ qv1;         		           

            end

%             a1 = -Qmat1 \ qv1;

            cond_count1=cond_count1+(cond(-Qmat1)>1E2);

            avec1(col,row,:) = a1;

            

            if cond(-Qmat2)>1E3

                a2=zeros(size(qv2));

%                 poor_condition_count=poor_condition_count+1;

            else

                a2 = -Qmat2 \ qv2;         		           

            end

%             a2 = -Qmat2 \ qv2;

            cond_count2=cond_count2+(cond(-Qmat2)>1E2);

            avec2(col,row,:) = a2;

            

            if cond(-Qmat_common)>1E3

                a_common=zeros(size(qv_common));

%                 poor_condition_count=poor_condition_count+1;

            else

                a_common = -Qmat_common \ qv_common;         		           

            end

%             a_common = -Qmat_common \ qv_common;

            cond_count3=cond_count3+(cond(-(Qmat_common))>1E2);

            %assumption of "exact-same forwards and reverse displacement

            %field"

%             a_common = -(Qmat1 + Qmat2) \ (qv1 - qv2);

%             cond_count3=cond_count3+(cond(-(Qmat1+Qmat2))>1E10);

            avec_common(col,row,:) = a_common;

            

            cond1(col,row)=cond(-Qmat1);

            cond2(col,row)=cond(-Qmat2);

            cond_common(col,row)=cond(-Qmat_common);

%             cond_common(col,row)=cond(-(Qmat1+Qmat2));

            % Calculate the error energy:

            del1(col,row) = a1.'*Qmat1*a1 + 2*qv1.'*a1 + q01;

            del2(col,row) = a2.'*Qmat2*a2 + 2*qv1.'*a2 + q02;

            del_common(col,row) = a_common.'*(Qmat_common)*a_common + 2*(qv_common).'*a_common+ (q0_common);

%             del_common(col,row) = a_common.'*(Qmat1+Qmat2)*a_common + 2*(qv1+qv2).'*a_common+ (q01+q02);

         end

      end

      %TDR display poor conditioning percentage

%       disp('percentage bad Q matrix from 0-constraint vector...');

%       poor_condition_count/(row*col)

%       zero_q_matrix_count/poor_condition_count

      % Update av.

      disp(['level: ' num2str(lev)]);

      disp(['ill-condition: 1 ' num2str(cond_count1)]);

      disp(['ill-condition: 2 ' num2str(cond_count2)]);

      disp(['ill-condition: 3 ' num2str(cond_count3)]);

      av1 = av1 + avec1;

      av2 = av2 + avec2;

      av_common = av_common + avec_common;

      

      % Create shift vector at same resolution as av.

      sav1 = size(av1) + [1 1 0];

      sh11 = affineshift2(avec1,sav1,mode);

	  maxsh11 = max(abs(sh11(:)));  % Display max extra shift on this iteration. 

      

      sav2 = size(av2) + [1 1 0];

      sh12 = affineshift2(avec2,sav2,mode);

	  maxsh12 = max(abs(sh12(:)));  % Display max extra shift on this iteration. 

      

      sav_common = size(av_common) + [1 1 0];

      sh1_common = affineshift2(avec_common,sav_common,mode);

	  maxsh1_common = max(abs(sh1_common(:)));  % Display max extra shift on this iteration. 

      

      % Update shift.

      sh1 = affineshift2(av1,sav1,mode);

      sh2 = affineshift2(av2,sav2,mode);

      sh_common = affineshift2(av_common,sav_common,mode);



      % Plot shift vectors.

      if (plot_disp) 

          figure(fig); set(gcf,'DefaultLineLineWidth',1.5);

          

          quiver(-real(sh)*sav(2)/2*qscale,-imag(sh)*sav(1)/2*qscale,0);

          set(gcf,'position',[700  30  500  672]);

          set(gca,'position',[0.01 0.01 .98 .98]);

          axis equal;

          axis ij;

          axis([-1 sav(2)+2 -1 sav(1)+2]);

          settitle(sprintf('%d iteration, output shift vectors',it));

          drawnow

          fig = fig + 1;

      end

      % Monitor the mean squared error and the mean affine vector (in pels).

      delta11(it)=mean(del1(:));

      avec11=squeeze(mean(mean(av1,1),2));

      avec11(1:2:end) = avec11(1:2:end)*size(refh{1},1);

      avec11(2:2:end) = avec11(2:2:end)*size(refh{1},2);

      

      delta12(it)=mean(del2(:));

      avec12=squeeze(mean(mean(av2,1),2));

      avec12(1:2:end) = avec12(1:2:end)*size(refh{1},1);

      avec12(2:2:end) = avec12(2:2:end)*size(refh{1},2);

      

      delta1_common(it)=mean(del_common(:));

      avec1_common=squeeze(mean(mean(av_common,1),2));

      avec1_common(1:2:end) = avec1_common(1:2:end)*size(refh{1},1);

      avec1_common(2:2:end) = avec1_common(2:2:end)*size(refh{1},2);

%        levels,delta1(it),avec1

      

      % Debug info:

%       mat2scr([real(sh(di))*16 imag(sh(di))*16 del(di)],'%10.4f',...

%          're(sh)*16  im(sh)*16  del:')

%       

   end  % End of iterations.

   

   % for k=1:6,

   %   t=[28:29 36:37];

   %   k,[r6(t,k)  p6i(t,k)  p61(t,k)]

   % end

   

   % Update shift, curv and delta at points where del < delta and sh is

   % close enough to shi.

   av1_s{sch}=avec1;

   av2_s{sch}=avec2;

   av_common_s{sch}=avec_common;

  

   

   

   

%    if sch == 1,

      shift1 = sh1;

      delta1 = del1;

      

      shift2 = sh2;

      delta2 = del2;

      

      shift_common = sh_common;

      delta_common = del_common;

%    else

%       if rem(mode,2) == 1,

%          dmin1 = find(del1./(sum(esurf(:,1:2)')'+1e-6) < ...

%             (delta1(:)./(sum(curv(:,1:2)')'+1e-6)-1e-5) & absl1(sh1(:) - shi(:)) < 0.6);

%          dmin2 = find(del2./(sum(esurf(:,1:2)')'+1e-6) < ...

%             (delta2(:)./(sum(curv(:,1:2)')'+1e-6)-1e-5) & absl1(sh2(:) - shi(:)) < 0.6);

%          dmin_common = find(del_common./(sum(esurf(:,1:2)')'+1e-6) < ...

%             (delta_common(:)./(sum(curv(:,1:2)')'+1e-6)-1e-5) & absl1(sh_common(:) - shi(:)) < 0.6);

%       else

%          dmin1 = find(del1 < (delta1(:) - 1e-2) & absl1(sh1(:) - shi(:)) < 0.6);

%          dmin2 = find(del2 < (delta2(:) - 1e-2) & absl1(sh2(:) - shi(:)) < 0.6);

%          dmin_common = find(del_common < (delta_common(:) - 1e-2) & absl1(sh_common(:) - shi(:)) < 0.6);

%       end

%       %    if ~isempty(dmin),

%       %      s=search(sch)

%       %      [dmin(:) shift(dmin)*16 sh(dmin)*16]

%       %      [dmin(:) delta(dmin) del(dmin)]

%       %    end

%       shift1(dmin1) = sh1(dmin1);

%       delta1(dmin1) = del1(dmin1);

%       

%       shift2(dmin2) = sh2(dmin2);

%       delta2(dmin2) = del2(dmin2);

%       

%       shift_common(dmin_common) = sh_common(dmin_common);

%       delta_common(dmin_common) = del_common(dmin_common);

%    end

   

%    mat2scr([real(shift(di))*16 imag(shift(di))*16 delta(di)],'%10.4f',...

%       'After min, re(shift)*16  im(shift)*16  delta:')

%    

end  % End of search offsets



% keyboard

% r6a=r6(28,:).';p6a=p6(4,[4:8:48]).';[r6a p6a r6a.*conj(p6a)/10]



% Uncomment next line to plot mean value of delta vs iteration.

% figure; semilogy(delta1); grid on; pause(0.1)



return