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

Last change on this file since 1156 was 1000, checked in by g7moreau, 8 years ago
  • Remove tab
File size: 2.9 KB
Line 
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
36if nargin < 8,
37   thresh_cond = inf;
38end;
39
40if nargin < 7,
41   MaxIter = 20;
42end;
43
44if 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;
59end;
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
70H = [Rckk(:,1:2) Tckk];
71
72% Computes the reprojection error in pixels:
73
74x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
75
76ex = x_kk - x;
77
78% Converts the homography in pixel units:
79
80KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];
81
82H = KK*H;
83
84return;
85
86
87% Test of compte extrinsic:
88
89Np = 4;
90sx = 10;
91sy = 10;
92sz = 5;
93
94om = randn(3,1);
95T = [0;0;100];
96
97noise = 2/1000;
98
99XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
100xx = project_points(XX,om,T);
101
102xxn = 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
109figure(3);
110plot(xx(1,:),xx(2,:),'r+');
111hold on;
112plot(xxn(1,:),xxn(2,:),'g+');
113hold off;
Note: See TracBrowser for help on using the repository browser.