[37] | 1 | function [Xmat,idxisa] = workvolume(cam,room,imres,idxcams) |
---|
| 2 | |
---|
| 3 | STEP = 0.1; |
---|
| 4 | |
---|
| 5 | if nargin < 4 |
---|
| 6 | idxcams = [1:size(cam,2)]; |
---|
| 7 | end |
---|
| 8 | |
---|
| 9 | if nargin < 3 |
---|
| 10 | imres = repmat([640 480],size(idxcams,2),1); |
---|
| 11 | end |
---|
| 12 | imres = imres(idxcams,:); |
---|
| 13 | |
---|
| 14 | if nargin < 2 |
---|
| 15 | % room [x_min, x_max, y_min, y_max, z_min, z_max] |
---|
| 16 | room = [-3 3 -3 3 0 3]; |
---|
| 17 | end |
---|
| 18 | |
---|
| 19 | % compose Pmat containing all P matrices |
---|
| 20 | Pmat = []; |
---|
| 21 | for i=idxcams, |
---|
| 22 | Pmat = [Pmat; cam(i).P]; |
---|
| 23 | end |
---|
| 24 | |
---|
| 25 | % create points |
---|
| 26 | |
---|
| 27 | zcoor = room(5):STEP:room(6); |
---|
| 28 | znum = size(zcoor,2); |
---|
| 29 | ycoor = room(3):STEP:room(4); |
---|
| 30 | ynum = size(ycoor,2); |
---|
| 31 | xcoor = room(1):STEP:room(2); |
---|
| 32 | xnum = size(xcoor,2); |
---|
| 33 | |
---|
| 34 | |
---|
| 35 | buff = repmat(ycoor,znum,1); |
---|
| 36 | yvec = reshape(buff,prod(size(buff)),1); |
---|
| 37 | zvec = repmat(zcoor',ynum,1); |
---|
| 38 | |
---|
| 39 | buff = repmat(xcoor,znum*ynum,1); |
---|
| 40 | xvec = reshape(buff,prod(size(buff)),1); |
---|
| 41 | |
---|
| 42 | Xmat = [xvec,repmat([yvec,zvec],xnum,1)]; |
---|
| 43 | Xmat = [Xmat,ones(size(Xmat(:,1)))]; |
---|
| 44 | |
---|
| 45 | clear buff |
---|
| 46 | |
---|
| 47 | size(Xmat) |
---|
| 48 | umat = Pmat*Xmat'; |
---|
| 49 | |
---|
| 50 | % normalize projected points in umat |
---|
| 51 | scalemat = []; |
---|
| 52 | for i=1:size(idxcams,2), |
---|
| 53 | scalemat = [scalemat; repmat(umat(3*i,:),3,1)]; |
---|
| 54 | end |
---|
| 55 | umat = umat./scalemat; |
---|
| 56 | clear scalemat; |
---|
| 57 | mask = zeros(size(umat)); |
---|
| 58 | mask(1:3:end,:) = umat(1:3:end,:)<repmat(imres(:,1),1,size(mask,2)); |
---|
| 59 | mask(2:3:end,:) = umat(2:3:end,:)<repmat(imres(:,2),1,size(mask,2)); |
---|
| 60 | mask(3:3:end,:) = 1; |
---|
| 61 | mask = mask.*(umat>0); |
---|
| 62 | idxisa = find(sum(mask)==size(mask,1)); |
---|
| 63 | |
---|
| 64 | return; |
---|
| 65 | |
---|