function [finalTrack] = finishTracks6(actFile, calFile)

warning off 'all';
display = false;

modelSize = 49;
minTrackInt = 2;
maxErr = 0.013;
maxDst = 0.124;
maxSmt = 0.233;

dirBase = dir([actFile '.1.*.pgm']);
numberFrames = size(dirBase, 1);

pointsName = [actFile '.points.mat'];
eval(['load '  pointsName ' humanBody ptNumber mkCode;']);

markerName = [actFile '.markers.mat'];
eval(['load '  markerName ' marker numMarkers;']);

for cam=1:8
  camFile = [calFile sprintf('%d', cam) '.Pmat.cal'];
  Pe(:, :, cam) = load(camFile);
end;  

% recover tracks from identification process
trackName = [actFile '.finalTrack.mat'];
eval(['load '  trackName ' finalTrack;']);

cleanName = [actFile '.cleanTrack.mat'];
eval(['load '  cleanName ' cleanTrack;']);

% find not detected markers
for trk=1:modelSize
  bin = (finalTrack(trk,:)>0);
  if (sum(bin) == 0)
    fprintf('------------------------\n', trk);      
    fprintf('Initializing Marker %2d.\n', trk);      
    fprintf('------------------------\n', trk);
    frm = 1;
    while (frm <= numberFrames)

      if (frm > 2)
        if ((finalTrack(trk,frm-1)>0 && finalTrack(trk,frm-2)>0)) break; end;
      end;
      fprintf('Frame %2d.\n', frm);
      
      clPt = 0;
      pass = 0;
      while (clPt < 2 && pass < 2)
        fprintf('Please, select at least two points.\n');          
        pC = zeros(2, 8);
        pass = pass+1;
        click = 0;
        for cam=1:8
          frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
          Io = double(imread(frmFile, 'pgm'));
          imshow(Io/255);
          pause;
          [xC, yC, button] = ginput(1);
          if (button == 3)
            pC(1, cam) = xC;
            pC(2, cam) = yC;
            click = click+1;
          elseif (button == 2)
            pass = 2;
            break;
          end;
          if (click == 3) break; end;
        end;
        mkBin = (pC(1,:) > 0);
        clPt = sum(mkBin);              
      end;

      mkBin = (pC(1,:) > 0);
      if (sum(mkBin) >= 2)
        camInd = find(mkBin);
        A1 = Pe(:, :, camInd(1)); 
        A = A1;
        b1(1, 1) = pC(1, camInd(1));
        b1(2, 1) = pC(2, camInd(1));
        b1(3, 1) = 1;
        b = b1;
        for ind=2:sum(mkBin)
          A1 = Pe(:, :, camInd(ind)); 
          A = [A A1];
          b1(1, 1) = pC(1, camInd(ind));
          b1(2, 1) = pC(2, camInd(ind));
          b1(3, 1) = 1;
          b = [b ; b1];
        end;
          
        x = uP2X(b, A);                    
        ptNumber(frm) = ptNumber(frm)+1;
        humanBody(:, ptNumber(frm), frm) = x;
        finalTrack(trk, frm) = ptNumber(frm);
        cleanTrack(trk, frm) = 1;
        if (sum(mkBin) > 2)
          mkCode(4, ptNumber(frm), frm) = camInd(1);
          mkCode(5, ptNumber(frm), frm) = 3;
        else
          mkCode(3, ptNumber(frm), frm) = camInd(1);            
          mkCode(4, ptNumber(frm), frm) = camInd(2);
          mkCode(5, ptNumber(frm), frm) = 2;          
        end;
           
        for cam=1:8            
          frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
          Io = imread(frmFile, 'pgm');
          dim = size(Io);
          subplot(2, 4, cam), imshow(rot90(Io));
          hold on;
          A1 = Pe(:, :, cam);
          
          p1 = A1*x;
          p1 = p1./(p1(3, 1));
          plot(p1(2,1), dim(2)-p1(1,1), 'ro');
       end;
       pause;
       clf;
       
      else
        finalTrack(trk, frm) = -1;
        cleanTrack(trk, frm) = 1;
      end;
      
      frm = frm+1;      
    end;
  end;
end;

% create model rigidity matrix
modelFile = ['model.dat'];
model = load(modelFile);
numEdge = size(model, 1); 
rigid = zeros(modelSize, modelSize);
for mk=1:numEdge
  rigid(model(mk, 1), model(mk, 2)) = 1;
end;

count = 0;
coverage = 0;
while (coverage < 1.0 && count < 20)
improve = false;
    
% create model distance matrix
links = zeros(modelSize, modelSize);
maxL = zeros(modelSize, modelSize);
minL = zeros(modelSize, modelSize);
for mk=1:numEdge
  bin1 = (finalTrack(model(mk, 1), :)>0);
  bin2 = (finalTrack(model(mk, 2), :)>0);
  binI = (bin1 & bin2);
  if (sum(binI) >= minTrackInt)
    dst = 0;
    tot = 0;
    for frm=1:numberFrames
      if (binI(frm) && finalTrack(model(mk, 1), frm)>0 && finalTrack(model(mk, 2), frm)>0)
        tot = tot + 1;          
        X(1) = humanBody(1, finalTrack(model(mk, 1), frm), frm);
        X(2) = humanBody(1, finalTrack(model(mk, 2), frm), frm);
        Y(1) = humanBody(2, finalTrack(model(mk, 1), frm), frm);
        Y(2) = humanBody(2, finalTrack(model(mk, 2), frm), frm);
        Z(1) = humanBody(3, finalTrack(model(mk, 1), frm), frm);
        Z(2) = humanBody(3, finalTrack(model(mk, 2), frm), frm);
        dx = X(1) - X(2);
        dy = Y(1) - Y(2);
        dz = Z(1) - Z(2);
        data(tot) = sqrt(dx*dx + dy*dy + dz*dz);
        dst = dst + sqrt(dx*dx + dy*dy + dz*dz);
      end;
    end;
    links(model(mk, 1), model(mk, 2)) = dst/tot;
    maxL(model(mk, 1), model(mk, 2)) = max(data(1:tot));
    minL(model(mk, 1), model(mk, 2)) = min(data(1:tot));    
  end;
end;

% symmetric model completes distance matrix
for mk1=1:numEdge
  edg = model(mk1, 3);
  if (links(model(mk1, 1), model(mk1, 2)))
    for mk2=1:numEdge
      if (model(mk2, 3) == edg)
        if (~links(model(mk2, 1), model(mk2, 2)))
          links(model(mk2, 1), model(mk2, 2)) = links(model(mk1, 1), model(mk1, 2));
          maxL(model(mk2, 1), model(mk2, 2)) = maxL(model(mk1, 1), model(mk1, 2));
          minL(model(mk2, 1), model(mk2, 2)) = minL(model(mk1, 1), model(mk1, 2));          
        end;
      end;
    end;
  end;
end;

for trk=1:modelSize
  bin = (finalTrack(trk,:) ~= 0);
  if (sum(bin)<numberFrames)

    fprintf('------------------------\n');
    fprintf('Finishing Marker %2d: ', trk);
    for f=1:numberFrames  fprintf('%d', bin(f)); end;
    fprintf('\n------------------------\n');
    pause;

    trackName = [actFile '.finalTrack.mat'];
    eval(['save ' trackName ' finalTrack;']);
    
    cleanName = [actFile '.cleanTrack.mat'];
    eval(['save ' cleanName ' cleanTrack;']);
    
    pointsName = [actFile '.points.mat'];
    eval(['save '  pointsName ' humanBody ptNumber mkCode;']);    
    
    minInd = min(find(bin));
    maxInd = max(find(bin));
    for frm = minInd-1:-1:1
      cur = frm+1;
      prv = cur+1;

      fprintf('Frame %2d.\n', frm);
      if (cleanTrack(trk, frm)) continue; end;
      
      % finds reconstructed 3D points already assigned to other tracks
      usedPt = zeros(1, ptNumber(frm));
      for t=1:modelSize
        if (finalTrack(t, frm)>0)
          usedPt(finalTrack(t, frm)) = 1;
        end;
      end;

      % compute distance between nonused points and smoothing prediction      
      smtAdj = zeros(1, ptNumber(frm));
      for pt=1:ptNumber(frm)
        if (~usedPt(pt) && finalTrack(trk, cur)>0 && finalTrack(trk, prv)>0)
            
          X(1) = humanBody(1, finalTrack(trk, cur), cur) + (humanBody(1, finalTrack(trk, cur), cur) - humanBody(1, finalTrack(trk, prv), prv));
          Y(1) = humanBody(2, finalTrack(trk, cur), cur) + (humanBody(2, finalTrack(trk, cur), cur) - humanBody(2, finalTrack(trk, prv), prv));
          Z(1) = humanBody(3, finalTrack(trk, cur), cur) + (humanBody(3, finalTrack(trk, cur), cur) - humanBody(3, finalTrack(trk, prv), prv));

          X(2) = humanBody(1, pt, frm);          
          Y(2) = humanBody(2, pt, frm);          
          Z(2) = humanBody(3, pt, frm);
          dx = X(1) - X(2);
          dy = Y(1) - Y(2);
          dz = Z(1) - Z(2);
          smtAdj(pt) = sqrt(dx*dx + dy*dy + dz*dz);
        else
          smtAdj(pt) = inf;
        end;
      end;            
      
      totAdj = 0;
      dstAdj = zeros(1, ptNumber(frm));      
      for adj = find(rigid(trk, :))
        if (links(trk, adj) && finalTrack(adj, frm)>0)
          for pt=1:ptNumber(frm)

            if (~usedPt(pt) && finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              X(2) = humanBody(1, pt, frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Y(2) = humanBody(2, pt, frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              Z(2) = humanBody(3, pt, frm);
              dx = X(1) - X(2);
              dy = Y(1) - Y(2);
              dz = Z(1) - Z(2);
              dst = abs(links(trk, adj) - sqrt(dx*dx + dy*dy + dz*dz));              
              
              dstAdj(pt) = dstAdj(pt) + dst;
            else
              dstAdj(pt) = inf;
            end;
          end;            
          totAdj = totAdj+1;
        end;          
      end;
           
      if (totAdj > 1)
        dstAdj = dstAdj/totAdj;
      else
        dstAdj = inf;
        if (count < 2)  
          break;
        else
          clPt = 0;
          pass = 0;
          while (clPt < 2 && pass < 2)
            fprintf('Please, select at least two points.\n');            
            pC = zeros(2, 8);
            pass = pass+1;
            click = 0;
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = double(imread(frmFile, 'pgm'));
              imshow(Io/255);
              pause;
              [xC, yC, button] = ginput(1);
              if (button == 3)
                pC(1, cam) = xC;
                pC(2, cam) = yC;
                click = click+1;
              elseif (button == 2)
                pass = 2;
                break;
              end;
              if (click == 3) break; end;
            end;
            mkBin = (pC(1,:) > 0);
            clPt = sum(mkBin);
          end;
          
          if (pass < 2)
            camInd = find(mkBin);
            A1 = Pe(:, :, camInd(1)); 
            A = A1;
            b1(1, 1) = pC(1, camInd(1));
            b1(2, 1) = pC(2, camInd(1));
            b1(3, 1) = 1;
            b = b1;
            for ind=2:sum(mkBin)
              A1 = Pe(:, :, camInd(ind)); 
              A = [A A1];
              b1(1, 1) = pC(1, camInd(ind));
              b1(2, 1) = pC(2, camInd(ind));
              b1(3, 1) = 1;
              b = [b ; b1];
            end;
          
            x = uP2X(b, A);                    
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = x;
            finalTrack(trk, frm) = ptNumber(frm);
            cleanTrack(trk, frm) = 1;
            if (sum(mkBin) > 2)
              mkCode(4, ptNumber(frm), frm) = camInd(1);
              mkCode(5, ptNumber(frm), frm) = 3;
            else
              mkCode(3, ptNumber(frm), frm) = camInd(1);            
              mkCode(4, ptNumber(frm), frm) = camInd(2);
              mkCode(5, ptNumber(frm), frm) = 2;          
            end;
           
            for cam=1:8            
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');
            end;

            pause;
            clf;
          else
            finalTrack(trk, frm) = -1;
            cleanTrack(trk, frm) = 1;
          end;
          continue;
        end;
      end;
      
      allAdj = dstAdj .* smtAdj;
      [minVal, minInd] = min(allAdj);
      if (minVal < maxErr && dstAdj(minInd) < maxDst && smtAdj(minInd) < maxSmt)

        finalTrack(trk, frm) = minInd;
        improve = true;
        %fprintf('Trk: %2d  Frm: %2d  Pt: %2d  Err: %f = (%f * %f)\n', trk, frm, minInd, minVal, dstAdj(minInd), smtAdj(minInd));   
        fprintf('Frame %2d finished automatically.\n', frm);    
        if (display)

          for cam=1:8
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
            x(1, 1) = humanBody(1, finalTrack(trk, frm), frm);
            x(2, 1) = humanBody(2, finalTrack(trk, frm), frm);
            x(3, 1) = humanBody(3, finalTrack(trk, frm), frm);
            x(4, 1) = 1;
            A1 = Pe(:, :, cam);
            p1 = A1*x;
            p1 = p1./(p1(3, 1));
            plot(p1(2,1), dim(2)-p1(1,1), 'ro');
          end;    
          pause;
          clf;
        end;          
      else
        %fprintf('(A) Stop at %d: %f %f %f\n', frm, minVal, dstAdj(minInd), smtAdj(minInd));

        if (finalTrack(trk, cur)>0 && finalTrack(trk, prv)>0)
          xT(1, 1) = humanBody(1, finalTrack(trk, cur), cur) + (humanBody(1, finalTrack(trk, cur), cur) - humanBody(1, finalTrack(trk, prv), prv));
          xT(2, 1) = humanBody(2, finalTrack(trk, cur), cur) + (humanBody(2, finalTrack(trk, cur), cur) - humanBody(2, finalTrack(trk, prv), prv));
          xT(3, 1) = humanBody(3, finalTrack(trk, cur), cur) + (humanBody(3, finalTrack(trk, cur), cur) - humanBody(3, finalTrack(trk, prv), prv));            
          xT(4, 1) = 1;
        else
          xT(1, 1) = 0;
          xT(2, 1) = 0;
          xT(3, 1) = 0;
          xT(4, 1) = 1;            
        end;
      
        % identify the cameras used to find point
        camUse = zeros(1, 8);
        for f=1:6
          if (frm+f <= numberFrames)
            if (finalTrack(trk, frm+f)>0)
              if (mkCode(5, finalTrack(trk, frm+f), frm+f) == 2)
                cam1 = mkCode(3, finalTrack(trk, frm+f), frm+f);
                cam2 = mkCode(4, finalTrack(trk, frm+f), frm+f);
                camUse(cam1) = camUse(cam1) + 0.75;
                camUse(cam2) = camUse(cam2) + 0.75;
              elseif (mkCode(5, finalTrack(trk, frm+f), frm+f) == 3)
                cam1 = mkCode(4, finalTrack(trk, frm+f), frm+f);
                cam2 = cam1+1;
                if (cam2 > 8) cam2 = cam2-8; end;
                cam3 = cam1+2;
                if (cam3 > 8) cam3 = cam3-8; end;
                camUse(cam1) = camUse(cam1) + 0.5;
                camUse(cam2) = camUse(cam2) + 1.0;
                camUse(cam3) = camUse(cam3) + 0.5;            
              end;
            end;
          end;
        end;        
        [minVal, minInd] = sort(-camUse);        
        for cam=1:8
          %fprintf('C%d (%f) ', minInd(cam), camUse(minInd(cam)));
        end;
        %fprintf('\n');
        camSet = minInd(1:3);
          
        % use only three closest joints to check for valid points
        totAdj = 0;        
        for adj = find(rigid(trk, :))
          if (links(trk, adj) && finalTrack(adj, frm)>0)
            totAdj = totAdj+1;
            val(totAdj) = links(trk, adj);
            dat(totAdj) = adj;
          end;
        end;
        if (totAdj < 4)
          adjSet = dat;
        else
          [minVal, minInd] = sort(val(1:totAdj));
          for i=1:3
            adjSet(i) = dat(minInd(i));
          end;
        end;        

        % compute a valid bounding box
        Rx = [-inf inf];
        Ry = [-inf inf];
        Rz = [-inf inf];
        for adj = adjSet
          if (finalTrack(adj, frm)>0)
            rx = [humanBody(1, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(1, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            ry = [humanBody(2, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(2, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            rz = [humanBody(3, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(3, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            
            if (rx(1) > Rx(1)) Rx(1) = rx(1); end;
            if (rx(2) < Rx(2)) Rx(2) = rx(2); end;
            if (ry(1) > Ry(1)) Ry(1) = ry(1); end;
            if (ry(2) < Ry(2)) Ry(2) = ry(2); end;
            if (rz(1) > Rz(1)) Rz(1) = rz(1); end;
            if (rz(2) < Rz(2)) Rz(2) = rz(2); end;
          end;
        end;
        % compute points satisfying geometric constraints
        numTot = 0;
        inc = ((Rx(2)-Rx(1))*(Ry(2)-Ry(1))*(Rz(2)-Rz(1))/100000)^(1/3);
        for xx=Rx(1):inc:Rx(2)
        for yy=Ry(1):inc:Ry(2)
        for zz=Rz(1):inc:Rz(2)
              
          interior = true;
          rigDst = 0;
          rigTot = 0;
          for adj = adjSet
            if (finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              dx = X(1) - xx;
              dy = Y(1) - yy;
              dz = Z(1) - zz;
            
              dst = sqrt(dx*dx + dy*dy + dz*dz);
              rigDst = rigDst + abs(links(trk, adj) - dst);
              rigTot = rigTot+1;
              if (dst > maxL(trk, adj) || dst < minL(trk, adj))
                interior = false;
              end;
            end;
          end;

          if (interior)
            numTot = numTot+1;
            xAll(1, numTot) = xx;
            xAll(2, numTot) = yy;
            xAll(3, numTot) = zz;
            xAll(4, numTot) = 1;
            geoRig(numTot) = rigDst/rigTot;
            dx = xT(1,1) - xx;
            dy = xT(2,1) - yy;
            dz = xT(3,1) - zz;
            trkSmt(numTot) = sqrt(dx*dx + dy*dy + dz*dz);
          end;
        end;
        end;
        end;
       
        valid = ones(1, numTot);
        feasible = zeros(1, numTot);
        intLevel = 32;
        for cam= camSet
          frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
          Io = imread(frmFile, 'pgm');
          dim = size(Io);          
          I = rot90(Io < intLevel);
          se = strel('disk',6);
          I2 = imdilate(I,se);
          I1 = I2-I;
       
          A1 = Pe(:, :, cam);
          for ix=1:numTot
            p1 = A1*xAll(:,ix);
            p1 = p1./(p1(3, 1));
            
            if (round(dim(2)-p1(1,1)) > 0 && round(p1(2,1)) > 0 && round(dim(2)-p1(1,1)) <= size(I2, 1) && round(p1(2,1)) <= size(I2, 2))
              if (~I2(round(dim(2)-p1(1,1)), round(p1(2,1))))
                valid(ix) = 0;
              end;
            
              if (I1(round(dim(2)-p1(1,1)), round(p1(2,1))))
                feasible(ix) = feasible(ix)+1;
              end;
            else
              valid(ix) = 0;  
            end;
          end;
        end;        
        maxFeas = max(feasible(1:numTot));
        fprintf('IK Points with %d adjacent markers (Valid, Feasible): (%d, %d)\n', totAdj, numTot, sum(feasible==maxFeas));
        
        % compute markers not used
        mkUsed = zeros(max(numMarkers(:, frm)), 8);
        for t=1:modelSize
          if (finalTrack(t, frm)>0)
            if (mkCode(5, finalTrack(t, frm), frm) == 2)
              mk1 = mkCode(1, finalTrack(t, frm), frm);
              mk2 = mkCode(2, finalTrack(t, frm), frm);
              cam1 = mkCode(3, finalTrack(t, frm), frm);
              cam2 = mkCode(4, finalTrack(t, frm), frm);
              if (mk1) mkUsed(mk1, cam1)= 1; end;
              if (mk2) mkUsed(mk2, cam2)= 1; end;
            else
              mk1 = mkCode(1, finalTrack(t, frm), frm);
              mk2 = mkCode(2, finalTrack(t, frm), frm);
              mk3 = mkCode(3, finalTrack(t, frm), frm);
              cam1 = mkCode(4, finalTrack(t, frm), frm);
              cam2 = cam1+1;
              if (cam2 > 8) cam2 = cam2-8; end;
              cam3 = cam1+2;
              if (cam3 > 8) cam3 = cam3-8; end;
              if (mk1) mkUsed(mk1, cam1)= 1; end;
              if (mk2) mkUsed(mk2, cam2)= 1; end;
              if (mk3) mkUsed(mk3, cam3)= 1; end;
            end;            
          end
        end        

        frmFile = [actFile '.' sprintf('%d', 1)  '.' sprintf('%05d', frm-1) '.pgm'];
        Io = imread(frmFile, 'pgm');
        dim = size(Io);                  
        geoMap = zeros(dim(2), dim(1), 8);
        mkDetec = zeros(1, 8);
        for cam=camSet
           if (display)
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
          end;
          A1 = Pe(:, :, cam);

          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              p1 = A1*xAll(:,ix);
              p1 = p1./(p1(3, 1));
              if (display)
                if (feasible(ix) == 1) plot(p1(2,1), dim(2)-p1(1,1), 'g.'); end;
                if (feasible(ix) == 2) plot(p1(2,1), dim(2)-p1(1,1), 'y.'); end;
                if (feasible(ix) == 3) plot(p1(2,1), dim(2)-p1(1,1), 'r.'); end;              
              end;
              geoMap(round(dim(2)-p1(1,1)), round(p1(2,1)), cam) = 1;              
            end;
          end;
          geoDst(:, :, cam) = bwdist(geoMap(:,:,cam));
          
          p1 = A1*xT;
          p1 = p1./(p1(3, 1));
          if (display) plot(p1(2,1), dim(2)-p1(1,1), 'bo'); end;
          
          dstTrkMin = inf;
          dstGeoMin = inf;
          ptGeoMin = 0;
          for pt=1:numMarkers(cam, frm)
            if (~mkUsed(pt, cam))
              dx = marker(2, pt, cam, frm)-p1(2,1);
              dy = marker(1, pt, cam, frm)-p1(1,1);
              dst = sqrt(dx*dx + dy*dy);
              if (dst < dstTrkMin)
                dstTrkMin = dst;
                ptTrkMin = pt;
              end;
              dst = geoDst(round(dim(2)-marker(1, pt, cam, frm)), round(marker(2, pt, cam, frm)), cam);
              if (dst < dstGeoMin)
                dstGeoMin = dst;
                ptGeoMin = pt;
              end;              
            end;
          end;
          if (ptGeoMin)
            dx = marker(2, ptTrkMin, cam, frm)-marker(2, ptGeoMin, cam, frm);
            dy = marker(1, ptTrkMin, cam, frm)-marker(1, ptGeoMin, cam, frm);
            if (sqrt(dx*dx + dy*dy)<2.0 && min([dstTrkMin dstGeoMin])<1.5)
              if (display)
                plot(marker(2, ptTrkMin, cam, frm), dim(2)-marker(1, ptTrkMin, cam, frm), 'rx');
                plot(marker(2, ptGeoMin, cam, frm), dim(2)-marker(1, ptGeoMin, cam, frm), 'ro');
              end;
              %fprintf('C%d (%f %f) ', cam, dstTrkMin, dstGeoMin);            
              mkDetec(cam) = ptGeoMin;
            end;
          end;
        end;
        if (display)
          pause;
          clf;        
        end;
        
        mkBin = (mkDetec > 0);
        if (sum(mkBin) == 3)
          camInd = find(mkBin);
          cam1 = camInd(1);
          cam2 = camInd(2);
          cam3 = camInd(3);
          A1 = Pe(:, :, cam1);
          A2 = Pe(:, :, cam2);
          A3 = Pe(:, :, cam3);
          A = [A1 A2 A3];
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          b2(1, 1) = marker(1, mkDetec(cam2), cam2, frm);
          b2(2, 1) = marker(2, mkDetec(cam2), cam2, frm);
          b2(3, 1) = 1;
          b3(1, 1) = marker(1, mkDetec(cam3), cam3, frm);
          b3(2, 1) = marker(2, mkDetec(cam3), cam3, frm);
          b3(3, 1) = 1;

          b = [b1 ; b2 ; b3];
          x = uP2X(b, A);
                    
          ptNumber(frm) = ptNumber(frm)+1;
          humanBody(:, ptNumber(frm), frm) = x;
          mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
          mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
          mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
          mkCode(4, ptNumber(frm), frm) = cam1;          
          mkCode(5, ptNumber(frm), frm) = 3;
          finalTrack(trk, frm) = ptNumber(frm);           

          for cam=1:8           
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
            A1 = Pe(:, :, cam);
          
            p1 = A1*x;
            p1 = p1./(p1(3, 1));
            plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
          end;
          pause;
          clf;
          
        elseif (sum(mkBin) == 2)

          camInd = find(mkBin);
          cam1 = camInd(1);
          cam2 = camInd(2);
          A1 = Pe(:, :, cam1);
          A2 = Pe(:, :, cam2);
          A = [A1 A2];
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          b2(1, 1) = marker(1, mkDetec(cam2), cam2, frm);
          b2(2, 1) = marker(2, mkDetec(cam2), cam2, frm);
          b2(3, 1) = 1;

          b = [b1 ; b2];
          x = uP2X(b, A);
          
          rigDst = 0;
          rigTot = 0;
          for adj = adjSet
            if (finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              dx = X(1) - x(1, 1);
              dy = Y(1) - x(2, 1);
              dz = Z(1) - x(3, 1);
            
              dst = sqrt(dx*dx + dy*dy + dz*dz);
              rigDst = rigDst + abs(links(trk, adj) - dst);
              rigTot = rigTot+1;
            end;
          end;
          rigDst = rigDst/rigTot;
          
          dx = xT(1, 1) - x(1, 1);
          dy = xT(2, 1) - x(2, 1);
          dz = xT(3, 1) - x(3, 1);
          smtDst = sqrt(dx*dx + dy*dy + dz*dz);
          
          if (smtDst < 0.111)                    
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = x;
            mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
            mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
            mkCode(3, ptNumber(frm), frm) = cam1;          
            mkCode(4, ptNumber(frm), frm) = cam2;          
            mkCode(5, ptNumber(frm), frm) = 2;
            finalTrack(trk, frm) = ptNumber(frm);
            
            fprintf('Finished automatically with two detected markers.\n');            
            %fprintf('Two markers case. (ts: %f gr: %f)\n', smtDst, rigDst);
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;
            
          else

            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = x;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
              mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
              mkCode(3, ptNumber(frm), frm) = cam1;          
              mkCode(4, ptNumber(frm), frm) = cam2;          
              mkCode(5, ptNumber(frm), frm) = 2;
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);                                            
              mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
              mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
              mkCode(3, ptNumber(frm), frm) = cam1;          
              mkCode(4, ptNumber(frm), frm) = cam2;          
              mkCode(5, ptNumber(frm), frm) = 2;              
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;
              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;              
              else
                finalTrack(trk, frm) = -1;
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
            end;
            %%% HELP
          end;
          
        elseif (sum(mkBin) == 1)

          camInd = find(mkBin);
          cam1 = camInd(1);
          A1 = Pe(:, :, cam1);            
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          
          minCst = inf;
          minIx = 0;
          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              p1 = A1*xAll(:,ix);
              p1 = p1./(p1(3, 1));
              
              dx = p1(1,1)-b1(1, 1);
              dy = p1(2,1)-b1(2, 1);
              epiCst(ix) = sqrt(dx*dx + dy*dy);             
              overallCst = (epiCst(ix)^2)*trkSmt(ix);
              if (minCst > overallCst)
                minCst = overallCst;
                minIx = ix;
              end;
            end;
          end;
          
          if (minIx) tmpVal = trkSmt(minIx); else tmpVal = inf; end;
          if (tmpVal < 0.049)
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
            finalTrack(trk, frm) = ptNumber(frm);            
            mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
            mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
            mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
            mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
            mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);

            fprintf('Finished automatically with one detected marker.\n');            
            %fprintf('One marker case. (ts: %f gr: %f fe: %d ec: %f)\n', trkSmt(minIx), geoRig(minIx), feasible(minIx), epiCst(minIx));          
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;          
            
          else
              
            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              if (minIx)
                p1 = A1*xAll(:,minIx);
                p1 = p1./(p1(3, 1));
                plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              end;
            
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);              
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);                            
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;

              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
              else
                finalTrack(trk, frm) = -1;
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
              
            end;
            %%% HELP
            
          end;
           
        else

          minCst = inf;
          minIx = 0;
          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              overallCst = trkSmt(ix);
              if (minCst > overallCst)
                minCst = overallCst;
                minIx = ix;
              end;
            end;
          end;
          
          if (minCst < 0.100)
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
            finalTrack(trk, frm) = ptNumber(frm);              
            mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
            mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
            mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
            mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
            mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);                
            
            fprintf('Finished automatically with no detected marker.\n');            
            %fprintf('No marker case. (ts: %f gr: %f fe: %d)\n', trkSmt(minIx), geoRig(minIx), feasible(minIx));
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;                    
            
          else
            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              if (minIx)
                p1 = A1*xAll(:,minIx);
                p1 = p1./(p1(3, 1));
                plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              end;
            
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);                              
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm+1), frm+1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm+1), frm+1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm+1), frm+1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm+1), frm+1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm+1), frm+1);                                            
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;

              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
              else
                finalTrack(trk, frm) = -1;  
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
              
            end;
            %%% HELP
            
          end;          
        end;            
        
        %fprintf('\n');

        clear dat;
        clear adjSet;
      end;
    end;

    for frm = maxInd+1:numberFrames
      cur = frm-1;
      prv = cur-1;
      
      fprintf('Frame %2d.\n', frm);
      if (cleanTrack(trk, frm)) continue; end;
      
      % compute distance between points and adjacent tracks    
      usedPt = zeros(1, ptNumber(frm));
      for t=1:modelSize
        if (finalTrack(t, frm)>0)
          usedPt(finalTrack(t, frm)) = 1;
        end;
      end;
      
      smtAdj = zeros(1, ptNumber(frm));
      for pt=1:ptNumber(frm)
        if (~usedPt(pt) && finalTrack(trk, cur)>0 && finalTrack(trk, prv)>0)
            
          X(1) = humanBody(1, finalTrack(trk, cur), cur) + (humanBody(1, finalTrack(trk, cur), cur) - humanBody(1, finalTrack(trk, prv), prv));
          Y(1) = humanBody(2, finalTrack(trk, cur), cur) + (humanBody(2, finalTrack(trk, cur), cur) - humanBody(2, finalTrack(trk, prv), prv));
          Z(1) = humanBody(3, finalTrack(trk, cur), cur) + (humanBody(3, finalTrack(trk, cur), cur) - humanBody(3, finalTrack(trk, prv), prv));            
            
          X(2) = humanBody(1, pt, frm);
          Y(2) = humanBody(2, pt, frm);
          Z(2) = humanBody(3, pt, frm);
          dx = X(1) - X(2);
          dy = Y(1) - Y(2);
          dz = Z(1) - Z(2);
          smtAdj(pt) = sqrt(dx*dx + dy*dy + dz*dz);
        else
          smtAdj(pt) = inf;
        end;
      end;            
      
      totAdj = 0;
      dstAdj = zeros(1, ptNumber(frm));    
      for adj = find(rigid(trk, :))
        if (links(trk, adj) && finalTrack(adj, frm)>0)            
          for pt=1:ptNumber(frm)

            if (~usedPt(pt) && finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              X(2) = humanBody(1, pt, frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Y(2) = humanBody(2, pt, frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              Z(2) = humanBody(3, pt, frm);
              dx = X(1) - X(2);
              dy = Y(1) - Y(2);
              dz = Z(1) - Z(2);
              dst = abs(links(trk, adj) - sqrt(dx*dx + dy*dy + dz*dz));              
              
              dstAdj(pt) = dstAdj(pt) + dst;
            else
              dstAdj(pt) = inf;
            end;
          end;            
          totAdj = totAdj+1;
        end;          
      end;

      if (totAdj > 1)
        dstAdj = dstAdj/totAdj;
      else
        dstAdj = inf;
        if (count < 2)  
          break;
        else
          clPt = 0;
          pass = 0;
          while (clPt < 2 && pass < 2)
            fprintf('Please, select at least two points.\n');                          
            pC = zeros(2, 8);
            pass = pass+1;
            click = 0;
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = double(imread(frmFile, 'pgm'));
              imshow(Io/255);
              pause;
              [xC, yC, button] = ginput(1);
              if (button == 3)
                pC(1, cam) = xC;
                pC(2, cam) = yC;
                click = click+1;
              elseif (button == 2)
                pass = 2;
                break;
              end;
              if (click == 3) break; end;
            end;
            mkBin = (pC(1,:) > 0);
            clPt = sum(mkBin);
          end;

          if (pass < 2)
            camInd = find(mkBin);
            A1 = Pe(:, :, camInd(1)); 
            A = A1;
            b1(1, 1) = pC(1, camInd(1));
            b1(2, 1) = pC(2, camInd(1));
            b1(3, 1) = 1;
            b = b1;
            for ind=2:sum(mkBin)
              A1 = Pe(:, :, camInd(ind)); 
              A = [A A1];
              b1(1, 1) = pC(1, camInd(ind));
              b1(2, 1) = pC(2, camInd(ind));
              b1(3, 1) = 1;
              b = [b ; b1];
            end;
          
            x = uP2X(b, A);                    
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = x;
            finalTrack(trk, frm) = ptNumber(frm);
            cleanTrack(trk, frm) = 1;
            if (sum(mkBin) > 2)
              mkCode(4, ptNumber(frm), frm) = camInd(1);
              mkCode(5, ptNumber(frm), frm) = 3;
            else
              mkCode(3, ptNumber(frm), frm) = camInd(1);            
              mkCode(4, ptNumber(frm), frm) = camInd(2);
              mkCode(5, ptNumber(frm), frm) = 2;          
            end;
           
            for cam=1:8            
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');
            end;

            pause;
            clf;       
          else
            finalTrack(trk, frm) = -1;
            cleanTrack(trk, frm) = 1;
          end;
          continue;
        end;
      end;

      allAdj = dstAdj .* smtAdj;
      [minVal, minInd] = min(allAdj);
      if (minVal < maxErr && dstAdj(minInd) < maxDst && smtAdj(minInd) < maxSmt)

        finalTrack(trk, frm) = minInd;
        improve = true;
        %fprintf('Trk: %2d  Frm: %2d  Pt: %2d  Err: %f = (%f * %f)\n', trk, frm, minInd, minVal, dstAdj(minInd), smtAdj(minInd));    
        fprintf('Frame %2d finished automatically.\n', frm);    
        if (display)
          for cam=1:8
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
            x(1, 1) = humanBody(1, finalTrack(trk, frm), frm);
            x(2, 1) = humanBody(2, finalTrack(trk, frm), frm);
            x(3, 1) = humanBody(3, finalTrack(trk, frm), frm);
            x(4, 1) = 1;
            A1 = Pe(:, :, cam);
            p1 = A1*x;
            p1 = p1./(p1(3, 1));
            plot(p1(2,1), dim(2)-p1(1,1), 'ro');
          end;    
          pause;
          clf;
        end;          
      else
        %fprintf('(B) Stop at %d: %f %f %f\n', frm, minVal, dstAdj(minInd), smtAdj(minInd));

        if (finalTrack(trk, cur)>0 && finalTrack(trk, prv)>0)
          xT(1, 1) = humanBody(1, finalTrack(trk, cur), cur) + (humanBody(1, finalTrack(trk, cur), cur) - humanBody(1, finalTrack(trk, prv), prv));
          xT(2, 1) = humanBody(2, finalTrack(trk, cur), cur) + (humanBody(2, finalTrack(trk, cur), cur) - humanBody(2, finalTrack(trk, prv), prv));
          xT(3, 1) = humanBody(3, finalTrack(trk, cur), cur) + (humanBody(3, finalTrack(trk, cur), cur) - humanBody(3, finalTrack(trk, prv), prv));            
          xT(4, 1) = 1;
        else
          xT(1, 1) = 0;
          xT(2, 1) = 0;
          xT(3, 1) = 0;
          xT(4, 1) = 1;          
        end;
      
        camUse = zeros(1, 8);
        for f=1:6
          if (frm-f>0)
            if (finalTrack(trk, frm-f)>0)
              if (mkCode(5, finalTrack(trk, frm-f), frm-f) == 2)
                cam1 = mkCode(3, finalTrack(trk, frm-f), frm-f);
                cam2 = mkCode(4, finalTrack(trk, frm-f), frm-f);
                camUse(cam1) = camUse(cam1) + 0.75;
                camUse(cam2) = camUse(cam2) + 0.75;
              elseif (mkCode(5, finalTrack(trk, frm-f), frm-f) == 3)
                cam1 = mkCode(4, finalTrack(trk, frm-f), frm-f);
                cam2 = cam1+1;
                if (cam2 > 8) cam2 = cam2-8; end;
                cam3 = cam1+2;
                if (cam3 > 8) cam3 = cam3-8; end;
                camUse(cam1) = camUse(cam1) + 0.5;
                camUse(cam2) = camUse(cam2) + 1.0;
                camUse(cam3) = camUse(cam3) + 0.5;            
              end;
            end;
          end;
        end;

        [minVal, minInd] = sort(-camUse);        
        for cam=1:8
            %fprintf('C%d (%f) ', minInd(cam), camUse(minInd(cam)));
        end;
        %fprintf('\n');
        camSet = minInd(1:3);
        
        totAdj = 0;        
        for adj = find(rigid(trk, :))
          if (links(trk, adj) && finalTrack(adj, frm)>0)
            totAdj = totAdj+1;
            val(totAdj) = links(trk, adj);
            dat(totAdj) = adj;
          end;
        end;
        if (totAdj < 4)
          adjSet = dat;
        else
          [minVal, minInd] = sort(val(1:totAdj));
          for i=1:3
            adjSet(i) = dat(minInd(i));
          end;
        end;
    
        Rx = [-inf inf];
        Ry = [-inf inf];
        Rz = [-inf inf];
        for adj = adjSet
          if (finalTrack(adj, frm)>0)
            rx = [humanBody(1, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(1, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            ry = [humanBody(2, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(2, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            rz = [humanBody(3, finalTrack(adj, frm), frm)-maxL(trk, adj) humanBody(3, finalTrack(adj, frm), frm)+maxL(trk, adj)];
            
            if (rx(1) > Rx(1)) Rx(1) = rx(1); end;
            if (rx(2) < Rx(2)) Rx(2) = rx(2); end;
            if (ry(1) > Ry(1)) Ry(1) = ry(1); end;
            if (ry(2) < Ry(2)) Ry(2) = ry(2); end;
            if (rz(1) > Rz(1)) Rz(1) = rz(1); end;
            if (rz(2) < Rz(2)) Rz(2) = rz(2); end;
          end;
        end;
        %fprintf('totAdj: %d\n', totAdj);

        numTot = 0;
        inc = ((Rx(2)-Rx(1))*(Ry(2)-Ry(1))*(Rz(2)-Rz(1))/100000)^(1/3);        
        for xx=Rx(1):inc:Rx(2)
        for yy=Ry(1):inc:Ry(2)
        for zz=Rz(1):inc:Rz(2)
              
          interior = true;
          rigDst = 0;
          rigTot = 0;
          for adj = adjSet
            if (finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              dx = X(1) - xx;
              dy = Y(1) - yy;
              dz = Z(1) - zz;
            
              dst = sqrt(dx*dx + dy*dy + dz*dz);
              rigDst = rigDst + abs(links(trk, adj) - dst);
              rigTot = rigTot+1;
              if (dst > maxL(trk, adj) || dst < minL(trk, adj))
                interior = false;
              end;
            end;
          end;              
              
          if (interior)
            numTot = numTot+1;
            xAll(1, numTot) = xx;
            xAll(2, numTot) = yy;
            xAll(3, numTot) = zz;
            xAll(4, numTot) = 1;
            geoRig(numTot) = rigDst/rigTot;
            dx = xT(1,1) - xx;
            dy = xT(2,1) - yy;
            dz = xT(3,1) - zz;
            trkSmt(numTot) = sqrt(dx*dx + dy*dy + dz*dz);            
          end;
        end;
        end;
        end;          

        valid = ones(1, numTot);
        feasible = zeros(1, numTot);
        intLevel = 32;
        for cam= camSet
          frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
          Io = imread(frmFile, 'pgm');
          dim = size(Io);          
          I = rot90(Io < intLevel);
          se = strel('disk',6);
          I2 = imdilate(I,se);
          I1 = I2-I;
          
          A1 = Pe(:, :, cam);
          for ix=1:numTot
            p1 = A1*xAll(:,ix);
            p1 = p1./(p1(3, 1));
            if (round(dim(2)-p1(1,1)) > 0 && round(p1(2,1)) > 0 && round(dim(2)-p1(1,1)) <= size(I2, 1) && round(p1(2,1)) <= size(I2, 2))            
              if (~I2(round(dim(2)-p1(1,1)), round(p1(2,1))))
                valid(ix) = 0;
              end;
            
              if (I1(round(dim(2)-p1(1,1)), round(p1(2,1))))
                feasible(ix) = feasible(ix)+1;
              end;
            else
              valid(ix) = 0;  
            end;
          end;
        end;    

        maxFeas = max(feasible(1:numTot));
        fprintf('IK Points with %d adjacent markers (Valid, Feasible): (%d, %d)\n', totAdj, numTot, sum(feasible==maxFeas));        
        % compute markers not used
        mkUsed = zeros(max(numMarkers(:, frm)), 8);
        for t=1:modelSize
          if (finalTrack(t, frm)>0)
            if (mkCode(5, finalTrack(t, frm), frm) == 2)
              mk1 = mkCode(1, finalTrack(t, frm), frm);
              mk2 = mkCode(2, finalTrack(t, frm), frm);
              cam1 = mkCode(3, finalTrack(t, frm), frm);
              cam2 = mkCode(4, finalTrack(t, frm), frm);
              if (mk1) mkUsed(mk1, cam1)= 1; end;
              if (mk2) mkUsed(mk2, cam2)= 1; end;
            else
              mk1 = mkCode(1, finalTrack(t, frm), frm);
              mk2 = mkCode(2, finalTrack(t, frm), frm);
              mk3 = mkCode(3, finalTrack(t, frm), frm);
              cam1 = mkCode(4, finalTrack(t, frm), frm);
              cam2 = cam1+1;
              if (cam2 > 8) cam2 = cam2-8; end;
              cam3 = cam1+2;
              if (cam3 > 8) cam3 = cam3-8; end;
              if (mk1) mkUsed(mk1, cam1)= 1; end;
              if (mk2) mkUsed(mk2, cam2)= 1; end;
              if (mk3) mkUsed(mk3, cam3)= 1; end;
            end;            
          end
        end        

        frmFile = [actFile '.' sprintf('%d', 1)  '.' sprintf('%05d', frm-1) '.pgm'];
        Io = imread(frmFile, 'pgm');
        dim = size(Io);                  
        geoMap = zeros(dim(2), dim(1), 8);
        mkDetec = zeros(1, 8);
        for cam=camSet
          if (display)
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
          end;
          A1 = Pe(:, :, cam);

          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              p1 = A1*xAll(:,ix);
              p1 = p1./(p1(3, 1));
              if (display)
                if (feasible(ix) == 1) plot(p1(2,1), dim(2)-p1(1,1), 'g.'); end;
                if (feasible(ix) == 2) plot(p1(2,1), dim(2)-p1(1,1), 'y.'); end;
                if (feasible(ix) == 3) plot(p1(2,1), dim(2)-p1(1,1), 'r.'); end;                            
              end;
              geoMap(round(dim(2)-p1(1,1)), round(p1(2,1)), cam) = 1;
            end;
          end;          
          geoDst(:, :, cam) = bwdist(geoMap(:, :, cam));
          
          p1 = A1*xT;
          p1 = p1./(p1(3, 1));
          if (display) plot(p1(2,1), dim(2)-p1(1,1), 'bo'); end;

          dstTrkMin = inf;
          dstGeoMin = inf;
          ptGeoMin = 0;
          for pt=1:numMarkers(cam, frm)
            if (~mkUsed(pt, cam))
              dx = marker(2, pt, cam, frm)-p1(2,1);
              dy = marker(1, pt, cam, frm)-p1(1,1);
              dst = sqrt(dx*dx + dy*dy);
              if (dst < dstTrkMin)
                dstTrkMin = dst;
                ptTrkMin = pt;
              end;
              dst = geoDst(round(dim(2)-marker(1, pt, cam, frm)), round(marker(2, pt, cam, frm)), cam);
              if (dst < dstGeoMin)
                dstGeoMin = dst;
                ptGeoMin = pt;
              end;                            
            end;
          end;
          if (ptGeoMin)
            dx = marker(2, ptTrkMin, cam, frm)-marker(2, ptGeoMin, cam, frm);
            dy = marker(1, ptTrkMin, cam, frm)-marker(1, ptGeoMin, cam, frm);
            if (sqrt(dx*dx + dy*dy)<2.0 && min([dstTrkMin dstGeoMin])<1.5)
              if (display)
                plot(marker(2, ptTrkMin, cam, frm), dim(2)-marker(1, ptTrkMin, cam, frm), 'rx');
                plot(marker(2, ptGeoMin, cam, frm), dim(2)-marker(1, ptGeoMin, cam, frm), 'ro');          
              end;
              %fprintf('C%d (%f %f) ', cam, dstTrkMin, dstGeoMin);            
              mkDetec(cam) = ptGeoMin;
            end;
          end;
        end;
        if (display)
          pause;
          clf;
        end;

        mkBin = (mkDetec > 0);
        if (sum(mkBin) == 3)
          camInd = find(mkBin);
          cam1 = camInd(1);
          cam2 = camInd(2);
          cam3 = camInd(3);
          A1 = Pe(:, :, cam1);
          A2 = Pe(:, :, cam2);
          A3 = Pe(:, :, cam3);
          A = [A1 A2 A3];
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          b2(1, 1) = marker(1, mkDetec(cam2), cam2, frm);
          b2(2, 1) = marker(2, mkDetec(cam2), cam2, frm);
          b2(3, 1) = 1;
          b3(1, 1) = marker(1, mkDetec(cam3), cam3, frm);
          b3(2, 1) = marker(2, mkDetec(cam3), cam3, frm);
          b3(3, 1) = 1;

          b = [b1 ; b2 ; b3];
          x = uP2X(b, A);
                    
          ptNumber(frm) = ptNumber(frm)+1;
          humanBody(:, ptNumber(frm), frm) = x;
          mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
          mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
          mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
          mkCode(4, ptNumber(frm), frm) = cam1;          
          mkCode(5, ptNumber(frm), frm) = 3;
          finalTrack(trk, frm) = ptNumber(frm);           

          for cam=1:8
            frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
            Io = imread(frmFile, 'pgm');
            dim = size(Io);
            subplot(2, 4, cam), imshow(rot90(Io));
            hold on;
            A1 = Pe(:, :, cam);
          
            p1 = A1*x;
            p1 = p1./(p1(3, 1));
            plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
          end;
          pause;
          clf;
            
        elseif (sum(mkBin) == 2)

          camInd = find(mkBin);
          cam1 = camInd(1);
          cam2 = camInd(2);
          A1 = Pe(:, :, cam1);
          A2 = Pe(:, :, cam2);
          A = [A1 A2];
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          b2(1, 1) = marker(1, mkDetec(cam2), cam2, frm);
          b2(2, 1) = marker(2, mkDetec(cam2), cam2, frm);
          b2(3, 1) = 1;

          b = [b1 ; b2];
          x = uP2X(b, A);

          rigDst = 0;
          rigTot = 0;
          for adj = adjSet
            if (finalTrack(adj, frm)>0)
              X(1) = humanBody(1, finalTrack(adj, frm), frm);
              Y(1) = humanBody(2, finalTrack(adj, frm), frm);
              Z(1) = humanBody(3, finalTrack(adj, frm), frm);
              dx = X(1) - x(1, 1);
              dy = Y(1) - x(2, 1);
              dz = Z(1) - x(3, 1);
            
              dst = sqrt(dx*dx + dy*dy + dz*dz);
              rigDst = rigDst + abs(links(trk, adj) - dst);
              rigTot = rigTot+1;
            end;
          end;    
          rigDst = rigDst/rigTot;

          dx = xT(1, 1) - x(1, 1);
          dy = xT(2, 1) - x(2, 1);
          dz = xT(3, 1) - x(3, 1);
          smtDst = sqrt(dx*dx + dy*dy + dz*dz);
          
          if (smtDst < 0.111)                    
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = x;
            mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
            mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
            mkCode(3, ptNumber(frm), frm) = cam1;          
            mkCode(4, ptNumber(frm), frm) = cam2;          
            mkCode(5, ptNumber(frm), frm) = 2;
            finalTrack(trk, frm) = ptNumber(frm);
            
            fprintf('Finished automatically with two detected markers.\n');                      
            %fprintf('Two markers case. (ts: %f gr: %f)\n', smtDst, rigDst);
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;
            
          else
              
            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = x;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
              mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
              mkCode(3, ptNumber(frm), frm) = cam1;          
              mkCode(4, ptNumber(frm), frm) = cam2;          
              mkCode(5, ptNumber(frm), frm) = 2;              
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);                                            
              mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
              mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
              mkCode(3, ptNumber(frm), frm) = cam1;          
              mkCode(4, ptNumber(frm), frm) = cam2;          
              mkCode(5, ptNumber(frm), frm) = 2;                            
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                                                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;
              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
              else
                finalTrack(trk, frm) = -1;
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
              
            end;
            %%% HELP
          end;
                                
        elseif (sum(mkBin) == 1)

          camInd = find(mkBin);
          cam1 = camInd(1);
          A1 = Pe(:, :, cam1);            
          b1(1, 1) = marker(1, mkDetec(cam1), cam1, frm);
          b1(2, 1) = marker(2, mkDetec(cam1), cam1, frm);
          b1(3, 1) = 1;
          
          minCst = inf;
          minIx = 0;
          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              p1 = A1*xAll(:,ix);
              p1 = p1./(p1(3, 1));
              
              dx = p1(1,1)-b1(1, 1);
              dy = p1(2,1)-b1(2, 1);
              epiCst(ix) = sqrt(dx*dx + dy*dy);             
              overallCst = (epiCst(ix)^2)*trkSmt(ix);
              if (minCst > overallCst)
                minCst = overallCst;
                minIx = ix;
              end;
            end;
          end;
          
          if (minIx) tmpVal = trkSmt(minIx); else tmpVal = inf; end;          
          if (tmpVal < 0.049)
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
            finalTrack(trk, frm) = ptNumber(frm);            
            mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
            mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
            mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
            mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
            mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);
            
            fprintf('Finished automatically with one detected marker.\n');                    
            %fprintf('One marker case. (ts: %f gr: %f fe: %d c: %f)\n', trkSmt(minIx), geoRig(minIx), feasible(minIx), epiCst(minIx));
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;          
            
          else
              
            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
              
              if (minIx)
                p1 = A1*xAll(:,minIx);
                p1 = p1./(p1(3, 1));
                plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              end;
              
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);                        
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);                                      
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                                                                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;
              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
              else
                finalTrack(trk, frm) = -1;  
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
              
            end;
            %%% HELP
            
          end;
          
        else

          minCst = inf;
          minIx = 0;
          for ix=find(valid)
            if (feasible(ix)==maxFeas)
              overallCst = trkSmt(ix);
              if (minCst > overallCst)
                minCst = overallCst;
                minIx = ix;
              end;
            end;
          end;
          
          if (minCst < 0.100)
            ptNumber(frm) = ptNumber(frm)+1;
            humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
            finalTrack(trk, frm) = ptNumber(frm);                          
            mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
            mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
            mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
            mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
            mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);
            
            fprintf('Finished automatically with no detected marker.\n');                    
            %fprintf('No marker case. (ts: %f gr: %f fe: %d)\n', trkSmt(minIx), geoRig(minIx), feasible(minIx));
            x = humanBody(:, ptNumber(frm), frm);
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              p1 = A1*x;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
            end;
            pause;
            clf;                    
            
          else
            
            %%% HELP
            for cam=1:8
              frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
              Io = imread(frmFile, 'pgm');
              dim = size(Io);
              subplot(2, 4, cam), imshow(rot90(Io));
              hold on;
              A1 = Pe(:, :, cam);
          
              if (minIx)
                p1 = A1*xAll(:,minIx);
                p1 = p1./(p1(3, 1));
                plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
              end;
            
              p1 = A1*xT;
              p1 = p1./(p1(3, 1));
              plot(p1(2,1), dim(2)-p1(1,1), 'bo');                        
            end;            
            in = input('(0-Red | 1-Blue | 2-None): ');
            clf;                    

            if (in == 0)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xAll(:,minIx);
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);                        
            elseif (in == 1)
              ptNumber(frm) = ptNumber(frm)+1;
              humanBody(:, ptNumber(frm), frm) = xT;
              finalTrack(trk, frm) = ptNumber(frm);
              mkCode(1, ptNumber(frm), frm) = mkCode(1, finalTrack(trk, frm-1), frm-1);            
              mkCode(2, ptNumber(frm), frm) = mkCode(2, finalTrack(trk, frm-1), frm-1);            
              mkCode(3, ptNumber(frm), frm) = mkCode(3, finalTrack(trk, frm-1), frm-1);            
              mkCode(4, ptNumber(frm), frm) = mkCode(4, finalTrack(trk, frm-1), frm-1);            
              mkCode(5, ptNumber(frm), frm) = mkCode(5, finalTrack(trk, frm-1), frm-1);              
            else
              clPt = 0;
              pass = 0;
              while (clPt < 2 && pass < 2)
                fprintf('Please, select at least two points.\n');                                                                                                          
                pC = zeros(2, 8);
                pass = pass+1;
                click = 0;
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = double(imread(frmFile, 'pgm'));
                  imshow(Io/255);
                  pause;
                  [xC, yC, button] = ginput(1);
                  if (button == 3)
                    pC(1, cam) = xC;
                    pC(2, cam) = yC;
                    click = click+1;
                  elseif (button == 2)
                    pass = 2;
                    break;
                  end;
                  if (click == 3) break; end;
                end;
                mkBin = (pC(1,:) > 0);
                clPt = sum(mkBin);
              end;
              %%% RESET    
                
              mkBin = (pC(1,:) > 0);
              if (sum(mkBin) >= 3)
                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                cam3 = camInd(3);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A3 = Pe(:, :, cam3);
                A = [A1 A2 A3];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;
                b3(1, 1) = pC(1, cam3);
                b3(2, 1) = pC(2, cam3);
                b3(3, 1) = 1;

                b = [b1 ; b2 ; b3];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = mkDetec(cam3);          
                mkCode(4, ptNumber(frm), frm) = cam1;          
                mkCode(5, ptNumber(frm), frm) = 3;
                finalTrack(trk, frm) = ptNumber(frm);           

                %fprintf('Reset with Three markers.\n');
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
          
              elseif (sum(mkBin) == 2)

                camInd = find(mkBin);
                cam1 = camInd(1);
                cam2 = camInd(2);
                A1 = Pe(:, :, cam1);
                A2 = Pe(:, :, cam2);
                A = [A1 A2];
                b1(1, 1) = pC(1, cam1);
                b1(2, 1) = pC(2, cam1);
                b1(3, 1) = 1;
                b2(1, 1) = pC(1, cam2);
                b2(2, 1) = pC(2, cam2);
                b2(3, 1) = 1;

                b = [b1 ; b2];
                x = uP2X(b, A);
                    
                ptNumber(frm) = ptNumber(frm)+1;
                humanBody(:, ptNumber(frm), frm) = x;
                mkCode(1, ptNumber(frm), frm) = mkDetec(cam1);          
                mkCode(2, ptNumber(frm), frm) = mkDetec(cam2);          
                mkCode(3, ptNumber(frm), frm) = cam1;          
                mkCode(4, ptNumber(frm), frm) = cam2;          
                mkCode(5, ptNumber(frm), frm) = 2;
                finalTrack(trk, frm) = ptNumber(frm);

                %fprintf('Reset with Two markers.\n');
                x = humanBody(:, ptNumber(frm), frm);
                for cam=1:8
                  frmFile = [actFile '.' sprintf('%d', cam)  '.' sprintf('%05d', frm-1) '.pgm'];
                  Io = imread(frmFile, 'pgm');
                  dim = size(Io);
                  subplot(2, 4, cam), imshow(rot90(Io));
                  hold on;
                  A1 = Pe(:, :, cam);
          
                  p1 = A1*x;
                  p1 = p1./(p1(3, 1));
                  plot(p1(2,1), dim(2)-p1(1,1), 'ro');          
                end;
                pause;
                clf;
              else
                finalTrack(trk, frm) = -1;  
              end;
              
              %%% RESET

              cleanTrack(trk, frm) = 1;           
              
              trackName = [actFile '.finalTrack.mat'];
              eval(['save ' trackName ' finalTrack;']);
    
              cleanName = [actFile '.cleanTrack.mat'];
              eval(['save ' cleanName ' cleanTrack;']);
    
              pointsName = [actFile '.points.mat'];
              eval(['save '  pointsName ' humanBody ptNumber mkCode;']);                  
              
            end;
            %%% HELP
            
          end;          
        end;
        %fprintf('\n');
        
        clear dat;
        clear adjSet;
      end;            
    end;    
  end;
end;

finalBin = (finalTrack>0);
coverage = sum(sum(finalBin))/(modelSize*numberFrames);
fprintf('Finished %f.\n', 100*coverage);

if (~improve)
  count = count+1;
else
  count = 0;
end;
end;

finalBin = (finalTrack>0);
for mk=1:modelSize
  fprintf('Mrk %2d = ', mk);  
  for f=1:numberFrames
    fprintf('%d', finalBin(mk,f));
  end;
  fprintf(' => %2d\n', sum(finalBin(mk,:)));
end;
coverage = sum(sum(finalBin))/(modelSize*numberFrames);
fprintf('Finishing done.\n');

pointsName = [actFile '.points.mat'];
eval(['save ' pointsName ' humanBody ptNumber mkCode;']);

trackName = [actFile '.finalTrack.mat'];
eval(['save ' trackName ' finalTrack;']);    

cleanName = [actFile '.cleanTrack.mat'];
eval(['save ' cleanName ' cleanTrack;']);