[37] | 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 | |
---|