source: trunk/src/toolbox_calib/compute_extrinsic.m @ 986

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

geometry cqlib updated

File size: 2.8 KB
RevLine 
[926]1function [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
37
38if nargin < 8,
39   thresh_cond = inf;
40end;
41
42
43if nargin < 7,
44   MaxIter = 20;
45end;
46
47
48if nargin < 6,
49   alpha_c = 0;
50        if nargin < 5,
51        kc = zeros(5,1);
52        if nargin < 4,
53        cc = zeros(2,1);
54        if nargin < 3,
55                fc = ones(2,1);
56                if nargin < 2,
57                error('Need 2D projections and 3D points (in compute_extrinsic.m)');
58                return;
59                end;
60        end;
61        end;
62        end;
63end;
64
65% Initialization:
66
67[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);
68
69% Refinement:
70[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);
71
72
73% computation of the homography (not useful in the end)
74
75H = [Rckk(:,1:2) Tckk];
76
77% Computes the reprojection error in pixels:
78
79x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
80
81ex = x_kk - x;
82
83
84% Converts the homography in pixel units:
85
86KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];
87
88H = KK*H;
89
90
91
92
93return;
94
95
96% Test of compte extrinsic:
97
98Np = 4;
99sx = 10;
100sy = 10;
101sz = 5;
102
103om = randn(3,1);
104T = [0;0;100];
105
106noise = 2/1000;
107
108XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
109xx = project_points(XX,om,T);
110
111xxn = xx + noise * randn(2,Np);
112
113[omckk,Tckk] = compute_extrinsic(xxn,XX);
114
115[om omckk om-omckk]
116[T Tckk T-Tckk]
117
118figure(3);
119plot(xx(1,:),xx(2,:),'r+');
120hold on;
121plot(xxn(1,:),xxn(2,:),'g+');
122hold off;
Note: See TracBrowser for help on using the repository browser.