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