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 | |
---|