1 | % the main script for the multicamera validation |
---|
2 | % reads the points and the camera matrices |
---|
3 | % and does 3D reconstructions |
---|
4 | % evaluates the reprojection errors |
---|
5 | % to check whether the P matrices still hold or not |
---|
6 | % |
---|
7 | % $Id: gorec.m,v 2.1 2005/05/23 16:22:51 svoboda Exp $ |
---|
8 | |
---|
9 | clear all; |
---|
10 | |
---|
11 | % add necessary paths |
---|
12 | addpath ../CommonCfgAndIO |
---|
13 | addpath ./CoreFunctions |
---|
14 | addpath ./InputOutputFunctions |
---|
15 | addpath ../RansacM; % ./Ransac for mex functions (it is significantly faster for noisy data) |
---|
16 | |
---|
17 | % get the configuration |
---|
18 | config = configdata(expname); |
---|
19 | |
---|
20 | UNDO_RADIAL = logical(config.cal.UNDO_RADIAL | config.cal.UNDO_HEIKK); |
---|
21 | |
---|
22 | if UNDO_RADIAL |
---|
23 | % add functions dealing with radial distortion |
---|
24 | addpath ../RadialDistortions |
---|
25 | end |
---|
26 | |
---|
27 | % read the input data |
---|
28 | loaded = loaddata(config); |
---|
29 | linear = loaded; % initalize the linear structure |
---|
30 | |
---|
31 | CAMS = size(config.cal.cams2use,2); |
---|
32 | FRAMES = size(loaded.IdMat,2); |
---|
33 | |
---|
34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
35 | % See the README how to compute data |
---|
36 | % for undoing of the radial distortion |
---|
37 | if config.cal.UNDO_RADIAL |
---|
38 | for i=1:CAMS, |
---|
39 | [K,kc] = readradfile(sprintf(config.files.rad,config.cal.cams2use(i))); |
---|
40 | xn = undoradial(loaded.Ws(i*3-2:i*3,:),K,[kc,0]); |
---|
41 | linear.Ws(i*3-2:i*3,:) = xn; |
---|
42 | end |
---|
43 | elseif config.cal.UNDO_HEIKK, |
---|
44 | for i=1:CAMS, |
---|
45 | heikkpar = load(sprintf(config.files.heikkrad,config.cal.cams2use(i)),'-ASCII'); |
---|
46 | xn = undoheikk(heikkpar(1:4),heikkpar(5:end),loaded.Ws(i*3-2:i*3-1,:)'); |
---|
47 | linear.Ws(i*3-2:i*3-1,:) = xn'; |
---|
48 | end |
---|
49 | end |
---|
50 | |
---|
51 | |
---|
52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
53 | % Detection of outliers |
---|
54 | % RANSAC is pairwise applied |
---|
55 | disp('RANSAC validation step running ...'); |
---|
56 | |
---|
57 | inl.IdMat = findinl(linear.Ws,linear.IdMat,config.cal.INL_TOL); |
---|
58 | |
---|
59 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
60 | %%% fill cam(i) structures |
---|
61 | for i=1:CAMS, |
---|
62 | cam(i).camId = config.cal.cams2use(i); |
---|
63 | cam(i).ptsLoaded = find(loaded.IdMat(i,:)); % loaded structure |
---|
64 | cam(i).ptsInl = find(inl.IdMat(i,:)); % survived initial pairwise validation |
---|
65 | cam(i).xgt = loaded.Ws(3*i-2:3*i,cam(i).ptsLoaded); |
---|
66 | cam(i).xlin = linear.Ws(3*i-2:3*i,cam(i).ptsLoaded); |
---|
67 | cam(i).xgtin = loaded.Ws(3*i-2:3*i,cam(i).ptsInl); |
---|
68 | cam(i).P = loaded.Pmat{i}; |
---|
69 | [cam(i).K, cam(i).R, cam(i).t, cam(i).C] = P2KRtC(cam(i).P); |
---|
70 | end |
---|
71 | |
---|
72 | % estimate the working volume which is |
---|
73 | % the intersection of the view cones |
---|
74 | disp('Computing maximal possible working volume') |
---|
75 | tic, |
---|
76 | [workingvolume.Xmat,workingvolume.idxisa] = workvolume(cam); |
---|
77 | toc |
---|
78 | % plot3(workingvolume.Xmat(workingvolume.idxisa,1),workingvolume.Xmat(workingvolume.idxisa,2),workingvolume.Xmat(workingvolume.idxisa,3),'.') |
---|
79 | Rmat =[]; |
---|
80 | for i=1:CAMS |
---|
81 | Rmat = [Rmat;cam(i).R]; |
---|
82 | end |
---|
83 | drawscene(workingvolume.Xmat(workingvolume.idxisa,:)',[cam(:).C],Rmat,3,'cloud','Maximal working volume',[cam(:).camId]); |
---|
84 | drawnow |
---|
85 | |
---|
86 | |
---|
87 | disp('***********************************************************') |
---|
88 | disp('Computing a robust 3D reconstruction via camera sampling ...') |
---|
89 | % compute a reconstruction robustly |
---|
90 | |
---|
91 | t1 = cputime; |
---|
92 | reconstructed = estimateX(linear,inl.IdMat,cam,config); |
---|
93 | reconstructed.CamIds = config.cal.cams2use(reconstructed.CamIdx); |
---|
94 | t2 = cputime; |
---|
95 | disp(sprintf('Elapsed time for 3D computation: %d minutes %d seconds',floor((t2-t1)/60), round(mod((t2-t1),60)))) |
---|
96 | |
---|
97 | % compute reprojections |
---|
98 | for i=1:CAMS, |
---|
99 | xe = linear.Pmat{i}*reconstructed.X; |
---|
100 | cam(i).xe = xe./repmat(xe(3,:),3,1); |
---|
101 | |
---|
102 | % these points were the input into Martinec and Pajdla filling |
---|
103 | mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far |
---|
104 | mask.vis = zeros(1,FRAMES); % maks of visible points |
---|
105 | mask.rec(reconstructed.ptsIdx) = 1; |
---|
106 | mask.vis(cam(i).ptsLoaded) = 1; |
---|
107 | mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera |
---|
108 | unmask.rec = cumsum(mask.rec); |
---|
109 | unmask.vis = cumsum(mask.vis); |
---|
110 | cam(i).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); |
---|
111 | cam(i).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); |
---|
112 | cam(i).err2d = sqrt(sum([cam(i).xe(1:2,cam(i).recandvis) - cam(i).xlin(1:2,cam(i).visandrec)].^2)); |
---|
113 | cam(i).mean2Derr = mean(cam(i).err2d); |
---|
114 | cam(i).std2Derr = std(cam(i).err2d); |
---|
115 | end |
---|
116 | |
---|
117 | % plot measured and reprojected 2D points |
---|
118 | for i=1:CAMS |
---|
119 | figure(i+10) |
---|
120 | clf |
---|
121 | plot(cam(i).xgt(1,:),cam(i).xgt(2,:),'ro'); |
---|
122 | hold on, grid on |
---|
123 | plot(cam(i).xgtin(1,:),cam(i).xgtin(2,:),'bo'); |
---|
124 | plot(cam(i).xlin(1,:),cam(i).xlin(2,:),'go'); |
---|
125 | plot(cam(i).xe(1,:),cam(i).xe(2,:),'k+') |
---|
126 | title(sprintf('measured, o, vs reprojected, +, 2D points (camera: %d)',config.cal.cams2use(i))); |
---|
127 | for j=1:size(cam(i).visandrec,2); % plot the reprojection errors |
---|
128 | line([cam(i).xlin(1,cam(i).visandrec(j)),cam(i).xe(1,cam(i).recandvis(j))],[cam(i).xlin(2,cam(i).visandrec(j)),cam(i).xe(2,cam(i).recandvis(j))],'Color','g'); |
---|
129 | end |
---|
130 | % draw the image boarder |
---|
131 | line([0 0 0 loaded.Res(i,1) loaded.Res(i,1) loaded.Res(i,1) loaded.Res(i,1) 0],[0 loaded.Res(i,2) loaded.Res(i,2) loaded.Res(i,2) loaded.Res(i,2) 0 0 0],'Color','k','LineWidth',2,'LineStyle','--') |
---|
132 | axis('equal') |
---|
133 | end |
---|
134 | |
---|
135 | % plot the 3D points |
---|
136 | figure(100), |
---|
137 | clf |
---|
138 | plot3(reconstructed.X(1,:),reconstructed.X(2,:),reconstructed.X(3,:),'*'); |
---|
139 | grid on |
---|
140 | |
---|
141 | figure(31) |
---|
142 | clf |
---|
143 | bar(config.cal.cams2use,[cam.mean2Derr;cam.std2Derr]',1.5) |
---|
144 | grid on |
---|
145 | xlabel('Id of the camera') |
---|
146 | title('2D error: mean (blue), std (red)') |
---|
147 | ylabel('pixels') |
---|
148 | |
---|
149 | %%% |
---|
150 | % print the results in a text form |
---|
151 | reconstructed |
---|
152 | |
---|
153 | %%% |
---|
154 | % save the data for non-linear estimation |
---|
155 | % the idea is to apply the caltech non-linear optimization |
---|
156 | % or any other alternative traditional calibration method to the |
---|
157 | % robustly reconstructed points. These 3D points will play the role |
---|
158 | % of a calibration grid. |
---|
159 | % |
---|
160 | % It is also assumed that the majority of the cameras are good to produce |
---|
161 | % acceptable 3D points. Othewise, we are trying to perform the calibration by using |
---|
162 | % a bad calibration grid |
---|
163 | % |
---|
164 | % This assumes sufficient overlap between cameras. No point filling applied |
---|
165 | % |
---|
166 | % 3D-2D correspondences is needed for each camera |
---|
167 | for i=1:CAMS, |
---|
168 | xe = loaded.Ws(i*3-2:i*3, reconstructed.ptsIdx(logical(loaded.IdMat(i,reconstructed.ptsIdx)))); |
---|
169 | Xe = reconstructed.X(:, logical(loaded.IdMat(i,reconstructed.ptsIdx))); |
---|
170 | corresp = [Xe',xe']; |
---|
171 | save(sprintf(config.files.points4cal,config.cal.cams2use(i)),'corresp','-ASCII'); |
---|
172 | end |
---|
173 | |
---|