source: trunk/src/toolbox_calib/compute_extrinsic_refine.m @ 1024

Last change on this file since 1024 was 926, checked in by sommeria, 9 years ago

geometry cqlib updated

File size: 2.7 KB
RevLine 
[926]1function [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
35if nargin < 10,
36   thresh_cond = inf;
37end;
38
39
40if nargin < 9,
41   MaxIter = 20;
42end;
43
44if 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;
59end;
60
61
62% Initialization:
63
64omckk = omc_init;
65Tckk = Tc_init;
66
67
68% Final optimization (minimize the reprojection error in pixel):
69% through Gradient Descent:
70
71param = [omckk;Tckk];
72
73change = 1;
74
75iter = 0;
76
77%keyboard;
78
79%fprintf(1,'Gradient descent iterations: ');
80
81while (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   
110end;
111
112%fprintf(1,'\n');
113
114Rckk = rodrigues(omckk);
Note: See TracBrowser for help on using the repository browser.