1 | function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), |
---|
2 | |
---|
3 | %compute_extrinsic |
---|
4 | % |
---|
5 | %[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) |
---|
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 | % MaxIter: Maximum number of iterations |
---|
18 | % |
---|
19 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space |
---|
20 | % Tckk: 3D translation vector attached to the grid positions in space |
---|
21 | % Rckk: 3D rotation matrices corresponding to the omc vectors |
---|
22 | |
---|
23 | % |
---|
24 | %Method: Computes the normalized point coordinates, then computes the 3D pose |
---|
25 | % |
---|
26 | %Important functions called within that program: |
---|
27 | % |
---|
28 | %normalize_pixel: Computes the normalize image point coordinates. |
---|
29 | % |
---|
30 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. |
---|
31 | % |
---|
32 | %project_points.m: Computes the 2D image projections of a set of 3D points |
---|
33 | |
---|
34 | |
---|
35 | if nargin < 10, |
---|
36 | thresh_cond = inf; |
---|
37 | end; |
---|
38 | |
---|
39 | |
---|
40 | if nargin < 9, |
---|
41 | MaxIter = 20; |
---|
42 | end; |
---|
43 | |
---|
44 | if nargin < 8, |
---|
45 | alpha_c = 0; |
---|
46 | if nargin < 7, |
---|
47 | kc = zeros(5,1); |
---|
48 | if nargin < 6, |
---|
49 | cc = zeros(2,1); |
---|
50 | if nargin < 5, |
---|
51 | fc = ones(2,1); |
---|
52 | if nargin < 4, |
---|
53 | error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); |
---|
54 | return; |
---|
55 | end; |
---|
56 | end; |
---|
57 | end; |
---|
58 | end; |
---|
59 | end; |
---|
60 | |
---|
61 | |
---|
62 | % Initialization: |
---|
63 | |
---|
64 | omckk = omc_init; |
---|
65 | Tckk = Tc_init; |
---|
66 | |
---|
67 | |
---|
68 | % Final optimization (minimize the reprojection error in pixel): |
---|
69 | % through Gradient Descent: |
---|
70 | |
---|
71 | param = [omckk;Tckk]; |
---|
72 | |
---|
73 | change = 1; |
---|
74 | |
---|
75 | iter = 0; |
---|
76 | |
---|
77 | %keyboard; |
---|
78 | |
---|
79 | %fprintf(1,'Gradient descent iterations: '); |
---|
80 | |
---|
81 | while (change > 1e-10)&(iter < MaxIter), |
---|
82 | |
---|
83 | %fprintf(1,'%d...',iter+1); |
---|
84 | |
---|
85 | [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); |
---|
86 | |
---|
87 | ex = x_kk - x; |
---|
88 | |
---|
89 | %keyboard; |
---|
90 | |
---|
91 | JJ = [dxdom dxdT]; |
---|
92 | |
---|
93 | if cond(JJ) > thresh_cond, |
---|
94 | change = 0; |
---|
95 | else |
---|
96 | |
---|
97 | JJ2 = JJ'*JJ; |
---|
98 | |
---|
99 | param_innov = inv(JJ2)*(JJ')*ex(:); |
---|
100 | param_up = param + param_innov; |
---|
101 | change = norm(param_innov)/norm(param_up); |
---|
102 | param = param_up; |
---|
103 | iter = iter + 1; |
---|
104 | |
---|
105 | omckk = param(1:3); |
---|
106 | Tckk = param(4:6); |
---|
107 | |
---|
108 | end; |
---|
109 | |
---|
110 | end; |
---|
111 | |
---|
112 | %fprintf(1,'\n'); |
---|
113 | |
---|
114 | Rckk = rodrigues(omckk); |
---|