1 | function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), |
---|
2 | |
---|
3 | %compute_extrinsic |
---|
4 | % |
---|
5 | %[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c) |
---|
6 | % |
---|
7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection |
---|
8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. |
---|
9 | %Works with planar and non-planar structures. |
---|
10 | % |
---|
11 | %INPUT: x_kk: Feature locations on the images |
---|
12 | % X_kk: Corresponding grid coordinates |
---|
13 | % fc: Camera focal length |
---|
14 | % cc: Principal point coordinates |
---|
15 | % kc: Distortion coefficients |
---|
16 | % alpha_c: Skew coefficient |
---|
17 | % |
---|
18 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space |
---|
19 | % Tckk: 3D translation vector attached to the grid positions in space |
---|
20 | % Rckk: 3D rotation matrices corresponding to the omc vectors |
---|
21 | % H: Homography between points on the grid and points on the image plane (in pixel) |
---|
22 | % This makes sense only if the planar that is used in planar. |
---|
23 | % x: Reprojections of the points on the image plane |
---|
24 | % ex: Reprojection error: ex = x_kk - x; |
---|
25 | % |
---|
26 | %Method: Computes the normalized point coordinates, then computes the 3D pose |
---|
27 | % |
---|
28 | %Important functions called within that program: |
---|
29 | % |
---|
30 | %normalize_pixel: Computes the normalize image point coordinates. |
---|
31 | % |
---|
32 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. |
---|
33 | % |
---|
34 | %project_points.m: Computes the 2D image projections of a set of 3D points |
---|
35 | |
---|
36 | if nargin < 8, |
---|
37 | thresh_cond = inf; |
---|
38 | end; |
---|
39 | |
---|
40 | if nargin < 7, |
---|
41 | MaxIter = 20; |
---|
42 | end; |
---|
43 | |
---|
44 | if nargin < 6, |
---|
45 | alpha_c = 0; |
---|
46 | if nargin < 5, |
---|
47 | kc = zeros(5,1); |
---|
48 | if nargin < 4, |
---|
49 | cc = zeros(2,1); |
---|
50 | if nargin < 3, |
---|
51 | fc = ones(2,1); |
---|
52 | if nargin < 2, |
---|
53 | error('Need 2D projections and 3D points (in compute_extrinsic.m)'); |
---|
54 | return; |
---|
55 | end; |
---|
56 | end; |
---|
57 | end; |
---|
58 | end; |
---|
59 | end; |
---|
60 | |
---|
61 | % Initialization: |
---|
62 | |
---|
63 | [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); |
---|
64 | |
---|
65 | % Refinement: |
---|
66 | [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond); |
---|
67 | |
---|
68 | % computation of the homography (not useful in the end) |
---|
69 | |
---|
70 | H = [Rckk(:,1:2) Tckk]; |
---|
71 | |
---|
72 | % Computes the reprojection error in pixels: |
---|
73 | |
---|
74 | x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); |
---|
75 | |
---|
76 | ex = x_kk - x; |
---|
77 | |
---|
78 | % Converts the homography in pixel units: |
---|
79 | |
---|
80 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; |
---|
81 | |
---|
82 | H = KK*H; |
---|
83 | |
---|
84 | return; |
---|
85 | |
---|
86 | |
---|
87 | % Test of compte extrinsic: |
---|
88 | |
---|
89 | Np = 4; |
---|
90 | sx = 10; |
---|
91 | sy = 10; |
---|
92 | sz = 5; |
---|
93 | |
---|
94 | om = randn(3,1); |
---|
95 | T = [0;0;100]; |
---|
96 | |
---|
97 | noise = 2/1000; |
---|
98 | |
---|
99 | XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; |
---|
100 | xx = project_points(XX,om,T); |
---|
101 | |
---|
102 | xxn = xx + noise * randn(2,Np); |
---|
103 | |
---|
104 | [omckk,Tckk] = compute_extrinsic(xxn,XX); |
---|
105 | |
---|
106 | [om omckk om-omckk] |
---|
107 | [T Tckk T-Tckk] |
---|
108 | |
---|
109 | figure(3); |
---|
110 | plot(xx(1,:),xx(2,:),'r+'); |
---|
111 | hold on; |
---|
112 | plot(xxn(1,:),xxn(2,:),'g+'); |
---|
113 | hold off; |
---|