[37] | 1 | % estimateX ... estimate 3D points robustly |
---|
| 2 | % |
---|
| 3 | % reconstructed = estimateX(loaded,IdMat,cam) |
---|
| 4 | % |
---|
| 5 | % data ... data structure, see LOADDATA |
---|
| 6 | % IdMat ... current point identification matrix |
---|
| 7 | % cam ... array of camera structures, see the main script GO |
---|
| 8 | % |
---|
| 9 | % reconstructed.ptdIdx ... indexes->data of points used for the reconstruction |
---|
| 10 | % .X ... reconstructed points, see u2PX |
---|
| 11 | % .CamIdx ... indexes->data of cameras used for the reconstruction |
---|
| 12 | % |
---|
| 13 | % $Id: estimateX.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $ |
---|
| 14 | |
---|
| 15 | function reconstructed = estimateX(data,IdMat,cam,config) |
---|
| 16 | |
---|
| 17 | SS = config.cal.NTUPLES; % sample size |
---|
| 18 | MIN_POINTS = config.cal.MIN_PTS_VAL; % minimal number of correnspondences in the sample |
---|
| 19 | |
---|
| 20 | Ws = data.Ws; |
---|
| 21 | Pmat = data.Pmat; |
---|
| 22 | |
---|
| 23 | CAMS = size(IdMat,1); |
---|
| 24 | FRAMES = size(IdMat,2); |
---|
| 25 | |
---|
| 26 | % create indexes for all possible SS-tuples |
---|
| 27 | if 1 |
---|
| 28 | count = 1; |
---|
| 29 | if SS == 2; |
---|
| 30 | for i=1:CAMS, |
---|
| 31 | for j=(i+1):CAMS, |
---|
| 32 | sample(count).CamIds = [i,j]; |
---|
| 33 | count = count+1; |
---|
| 34 | end |
---|
| 35 | end |
---|
| 36 | end |
---|
| 37 | if SS == 3; |
---|
| 38 | for i=1:CAMS, |
---|
| 39 | for j=(i+1):CAMS, |
---|
| 40 | for k=(j+1):CAMS, |
---|
| 41 | sample(count).CamIds = [i,j,k]; |
---|
| 42 | count = count+1; |
---|
| 43 | end |
---|
| 44 | end |
---|
| 45 | end |
---|
| 46 | end |
---|
| 47 | if SS == 4; |
---|
| 48 | for i=1:CAMS, |
---|
| 49 | for j=(i+1):CAMS, |
---|
| 50 | for k=(j+1):CAMS, |
---|
| 51 | for l=(k+1):CAMS, |
---|
| 52 | sample(count).CamIds = [i,j,k,l]; |
---|
| 53 | count = count+1; |
---|
| 54 | end |
---|
| 55 | end |
---|
| 56 | end |
---|
| 57 | end |
---|
| 58 | end |
---|
| 59 | if SS == 5; |
---|
| 60 | for i=1:CAMS, |
---|
| 61 | for j=(i+1):CAMS, |
---|
| 62 | for k=(j+1):CAMS, |
---|
| 63 | for l=(k+1):CAMS, |
---|
| 64 | for m=(l+1):CAMS, |
---|
| 65 | sample(count).CamIds = [i,j,k,l,m]; |
---|
| 66 | count = count+1; |
---|
| 67 | end |
---|
| 68 | end |
---|
| 69 | end |
---|
| 70 | end |
---|
| 71 | end |
---|
| 72 | end |
---|
| 73 | else |
---|
| 74 | sample(1).CamIds = [1:15]; |
---|
| 75 | SS = size(sample(1).CamIds,2); |
---|
| 76 | end |
---|
| 77 | |
---|
| 78 | disp(sprintf('Computing recontruction from all %d camera %d-tuples',size(sample,2), SS)); |
---|
| 79 | |
---|
| 80 | % create triple indexes |
---|
| 81 | for i=1:CAMS, |
---|
| 82 | tripleIdx{i} = [i*3-2:i*3]; |
---|
| 83 | end |
---|
| 84 | |
---|
| 85 | %%% |
---|
| 86 | % for all possible combination of SS-tuples of cameras |
---|
| 87 | % do the linear 3D reconctruction if enough point avaialable |
---|
| 88 | for i=1:size(sample,2), |
---|
| 89 | ptsIdx = find(sum(IdMat([sample(i).CamIds],:))==SS); |
---|
| 90 | if size(ptsIdx,2) > MIN_POINTS |
---|
| 91 | X = uP2X(Ws([tripleIdx{[sample(i).CamIds]}],ptsIdx), [Pmat{[sample(i).CamIds]}]); |
---|
| 92 | % compute the reprojections |
---|
| 93 | for j=1:CAMS, |
---|
| 94 | xe = Pmat{j}*X; |
---|
| 95 | cam(j).xe = xe./repmat(xe(3,:),3,1); |
---|
| 96 | % these points were the input into Martinec and Pajdla filling |
---|
| 97 | mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far |
---|
| 98 | mask.vis = zeros(1,FRAMES); % maks of visible points |
---|
| 99 | mask.rec(ptsIdx) = 1; |
---|
| 100 | mask.vis(cam(j).ptsLoaded) = 1; |
---|
| 101 | mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera |
---|
| 102 | unmask.rec = cumsum(mask.rec); |
---|
| 103 | unmask.vis = cumsum(mask.vis); |
---|
| 104 | cam(j).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); |
---|
| 105 | cam(j).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); |
---|
| 106 | cam(j).err2d = sum([cam(j).xe(1:2,cam(j).recandvis) - cam(j).xgt(1:2,cam(j).visandrec)].^2); |
---|
| 107 | cam(j).mean2Derr = mean(cam(j).err2d); |
---|
| 108 | cam(j).std2Derr = std(cam(j).err2d); |
---|
| 109 | end |
---|
| 110 | sample(i).mean2Derrs = [cam(:).mean2Derr]; |
---|
| 111 | sample(i).std2Derrs = sum([cam(:).std2Derr]); |
---|
| 112 | sample(i).mean2Derr = sum(sample(i).mean2Derrs); |
---|
| 113 | else |
---|
| 114 | sample(i).mean2Derr = 9e99; |
---|
| 115 | sample(i).std2Derrs = 9e99; |
---|
| 116 | end |
---|
| 117 | end |
---|
| 118 | |
---|
| 119 | % find the best sample |
---|
| 120 | [buff,idxBest] = min([sample(:).mean2Derr]+[sample(:).std2Derrs]); |
---|
| 121 | |
---|
| 122 | % and recompute it the best values |
---|
| 123 | reconstructed.ptsIdx = find(sum(IdMat(sample(idxBest).CamIds,:))==SS); |
---|
| 124 | reconstructed.X = uP2X(Ws([tripleIdx{[sample(idxBest).CamIds]}],reconstructed.ptsIdx), [Pmat{[sample(idxBest).CamIds]}]); |
---|
| 125 | reconstructed.CamIdx = sample(idxBest).CamIds; |
---|
| 126 | |
---|
| 127 | return; |
---|
| 128 | |
---|