1 | function [p, polys, Mint, Mext] = projectDino(f, d, R, sclZ) |
---|
2 | %% [p, f, Mint, Mext] = projectDino(f, d, R, sclZ) |
---|
3 | %% |
---|
4 | %% Return the image positions of points on the Dino model |
---|
5 | %% as viewed from a camera with focal length f, and nodal point at d. |
---|
6 | %% Optional args: |
---|
7 | %% R is the 3x3 rotation matrix, or |
---|
8 | % if R =[] or R is omitted, |
---|
9 | %% then camera is rotated so that the optical axis passes through the |
---|
10 | %% mean of Dino's 3D point data. |
---|
11 | %% sclZ, Dino is scaled in its Z direction by |
---|
12 | %% the factor sclZ (default 1) before projection. |
---|
13 | %% On return, polys is a descriptor for individual polygons in the dino |
---|
14 | %% model. This is used in showWire. |
---|
15 | %% Also the intrinsic and extrinsic calibration |
---|
16 | %% matrices, Mint and Mext = [R, -R*d], respectively. |
---|
17 | %% |
---|
18 | %% This routine could generate divide by zero warnings depending on the |
---|
19 | %% placement of the camera. |
---|
20 | |
---|
21 | if nargin < 3 | size(R,1) == 0 |
---|
22 | computeR = 1; |
---|
23 | R = []; |
---|
24 | else |
---|
25 | computeR = 0; |
---|
26 | end |
---|
27 | if nargin < 4 |
---|
28 | sclZ = 1; |
---|
29 | end |
---|
30 | |
---|
31 | DEBUG = 0; |
---|
32 | |
---|
33 | %% Read Dino data set |
---|
34 | [v polys] = getHalfDino; |
---|
35 | |
---|
36 | %%% Set canonical 3D pose |
---|
37 | R0 = [1 0 0; 0 0 -1; 0 -1 0]; %% Rotate and reflect dino (concave away). |
---|
38 | mn0 = [0 0 0]'; %% Place Dino's mean at origin |
---|
39 | P0 = R0 * v' + repmat(mn0, 1, size(v,1)); |
---|
40 | if sclZ ~= 1.0 |
---|
41 | P0(3,:) = sclZ * P0(3,:); |
---|
42 | end |
---|
43 | |
---|
44 | if DEBUG |
---|
45 | figure(1); |
---|
46 | showWire(P0', polys); |
---|
47 | xlabel('Z');ylabel('Z');zlabel('Z'); |
---|
48 | title('3D Dino Model'); |
---|
49 | pause; |
---|
50 | end |
---|
51 | %% Build 3D homogeneous coordinates. |
---|
52 | P0 = [P0; ones(1,size(P0,2))]; |
---|
53 | |
---|
54 | if computeR |
---|
55 | %% Choose rotation to fixate mn0. |
---|
56 | %% That is solve: R * (mn0 - d) = [0 0 1]'; |
---|
57 | R = eye(3); |
---|
58 | R(:,3) = (mn0 - d)/norm(mn0 - d); |
---|
59 | R(:,2) = R(:,2) - (R(:,3)' * R(:,2)) * R(:,3); |
---|
60 | R(:,2) = R(:,2)/norm(R(:,2)); |
---|
61 | R(:,1) = R(:,1) - R(:,2:3) * (R(:,2:3)' * R(:,1)); |
---|
62 | R(:,1) = R(:,1)/norm(R(:,1)); |
---|
63 | R = R'; |
---|
64 | end |
---|
65 | |
---|
66 | %% Build intrinsic and extrinsic calibration matrices Mint and Mext. |
---|
67 | Mint = diag([f, f, 1]); |
---|
68 | Mext = [R, -R*d]; |
---|
69 | |
---|
70 | %% Construct projection matrix |
---|
71 | M = Mint * Mext; |
---|
72 | |
---|
73 | %% Compute the homogeneous image locations |
---|
74 | p = M * P0; |
---|
75 | |
---|
76 | %% Convert to image pixels |
---|
77 | p = p ./repmat(p(3,:), 3,1); |
---|