source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/BlueCCal/MultiCamValidation/CoreFunctions/estimateX.m @ 37

% estimateX ... estimate 3D points robustly
% 
% reconstructed = estimateX(loaded,IdMat,cam)
%
% data   ... data structure, see LOADDATA
% IdMat  ... current point identification matrix
% cam    ... array of camera structures, see the main script GO
%
% reconstructed.ptdIdx ... indexes->data of points used for the reconstruction 
%              .X      ... reconstructed points, see u2PX
%              .CamIdx ... indexes->data of cameras used for the reconstruction
%
% $Id: estimateX.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $

function reconstructed = estimateX(data,IdMat,cam,config)

SS = config.cal.NTUPLES; % sample size
MIN_POINTS = config.cal.MIN_PTS_VAL; % minimal number of correnspondences in the sample

Ws   = data.Ws;
Pmat = data.Pmat;

CAMS   = size(IdMat,1);
FRAMES = size(IdMat,2);

% create indexes for all possible SS-tuples
if 1
  count = 1;
  if SS == 2;
        for i=1:CAMS,
          for j=(i+1):CAMS,
                sample(count).CamIds = [i,j];
                count = count+1;
          end
        end
  end
  if SS == 3;
        for i=1:CAMS,
          for j=(i+1):CAMS,
                for k=(j+1):CAMS,
                  sample(count).CamIds = [i,j,k];
                  count = count+1;
                end
          end
        end
  end
  if SS == 4;
        for i=1:CAMS,
          for j=(i+1):CAMS,
                for k=(j+1):CAMS,
                  for l=(k+1):CAMS,
                        sample(count).CamIds = [i,j,k,l];
                        count = count+1;
                  end
                end
          end
        end
  end
  if SS == 5;
        for i=1:CAMS,
          for j=(i+1):CAMS,
                for k=(j+1):CAMS,
                  for l=(k+1):CAMS,
                        for m=(l+1):CAMS,
                          sample(count).CamIds = [i,j,k,l,m];
                          count = count+1;
                        end
                  end
                end
          end
        end
  end
else
  sample(1).CamIds = [1:15];
  SS = size(sample(1).CamIds,2);
end

disp(sprintf('Computing recontruction from all %d camera %d-tuples',size(sample,2), SS));

% create triple indexes
for i=1:CAMS,
  tripleIdx{i} = [i*3-2:i*3];
end

%%%
% for all possible combination of SS-tuples of cameras
% do the linear 3D reconctruction if enough point avaialable
for i=1:size(sample,2),
  ptsIdx = find(sum(IdMat([sample(i).CamIds],:))==SS);
  if size(ptsIdx,2) > MIN_POINTS
        X = uP2X(Ws([tripleIdx{[sample(i).CamIds]}],ptsIdx), [Pmat{[sample(i).CamIds]}]);
        % compute the reprojections
        for j=1:CAMS,
          xe = Pmat{j}*X;
          cam(j).xe = xe./repmat(xe(3,:),3,1);
          % these points were the input into Martinec and Pajdla filling
          mask.rec = zeros(1,FRAMES);   % mask of points that survived validation so far
          mask.vis = zeros(1,FRAMES); % maks of visible points
          mask.rec(ptsIdx)  = 1;
          mask.vis(cam(j).ptsLoaded) = 1;
          mask.both             = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera
          unmask.rec    = cumsum(mask.rec);
          unmask.vis    = cumsum(mask.vis);
          cam(j).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec);
          cam(j).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec);
          cam(j).err2d     = sum([cam(j).xe(1:2,cam(j).recandvis) - cam(j).xgt(1:2,cam(j).visandrec)].^2);
          cam(j).mean2Derr = mean(cam(j).err2d);
          cam(j).std2Derr  = std(cam(j).err2d);
        end
        sample(i).mean2Derrs = [cam(:).mean2Derr];
        sample(i).std2Derrs  = sum([cam(:).std2Derr]);
        sample(i).mean2Derr = sum(sample(i).mean2Derrs);
  else
        sample(i).mean2Derr = 9e99;
        sample(i).std2Derrs     = 9e99;
  end
end

% find the best sample
[buff,idxBest] = min([sample(:).mean2Derr]+[sample(:).std2Derrs]);

% and recompute it the best values
reconstructed.ptsIdx = find(sum(IdMat(sample(idxBest).CamIds,:))==SS);
reconstructed.X          = uP2X(Ws([tripleIdx{[sample(idxBest).CamIds]}],reconstructed.ptsIdx), [Pmat{[sample(idxBest).CamIds]}]);
reconstructed.CamIdx = sample(idxBest).CamIds;

return;