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

Last change on this file since 810 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 3.8 KB
Line 
1%=======================================================================
2% Copyright 2008-2014, LEGI UMR 5519 / CNRS UJF G-INP, Grenoble, France
3%   http://www.legi.grenoble-inp.fr
4%   Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr
5%
6%     This file is part of the toolbox UVMAT.
7%
8%     UVMAT is free software; you can redistribute it and/or modify
9%     it under the terms of the GNU General Public License as published
10%     by the Free Software Foundation; either version 2 of the license,
11%     or (at your option) any later version.
12%
13%     UVMAT is distributed in the hope that it will be useful,
14%     but WITHOUT ANY WARRANTY; without even the implied warranty of
15%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16%     GNU General Public License (see LICENSE.txt) for more details.
17%=======================================================================
18
19function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),
20
21%compute_extrinsic
22%
23%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c)
24%
25%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
26%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
27%Works with planar and non-planar structures.
28%
29%INPUT: x_kk: Feature locations on the images
30%       X_kk: Corresponding grid coordinates
31%       fc: Camera focal length
32%       cc: Principal point coordinates
33%       kc: Distortion coefficients
34%       alpha_c: Skew coefficient
35%
36%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
37%        Tckk: 3D translation vector attached to the grid positions in space
38%        Rckk: 3D rotation matrices corresponding to the omc vectors
39%        H: Homography between points on the grid and points on the image plane (in pixel)
40%           This makes sense only if the planar that is used in planar.
41%        x: Reprojections of the points on the image plane
42%        ex: Reprojection error: ex = x_kk - x;
43%
44%Method: Computes the normalized point coordinates, then computes the 3D pose
45%
46%Important functions called within that program:
47%
48%normalize_pixel: Computes the normalize image point coordinates.
49%
50%pose3D: Computes the 3D pose of the structure given the normalized image projection.
51%
52%project_points.m: Computes the 2D image projections of a set of 3D points
53
54
55
56if nargin < 8,
57   thresh_cond = inf;
58end;
59
60
61if nargin < 7,
62   MaxIter = 20;
63end;
64
65
66if nargin < 6,
67   alpha_c = 0;
68        if nargin < 5,
69        kc = zeros(5,1);
70        if nargin < 4,
71        cc = zeros(2,1);
72        if nargin < 3,
73                fc = ones(2,1);
74                if nargin < 2,
75                error('Need 2D projections and 3D points (in compute_extrinsic.m)');
76                return;
77                end;
78        end;
79        end;
80        end;
81end;
82
83% Initialization:
84
85[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);
86
87% Refinement:
88[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);
89
90
91% computation of the homography (not useful in the end)
92
93H = [Rckk(:,1:2) Tckk];
94
95% Computes the reprojection error in pixels:
96
97x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
98
99ex = x_kk - x;
100
101
102% Converts the homography in pixel units:
103
104KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];
105
106H = KK*H;
107
108
109
110
111return;
112
113
114% Test of compte extrinsic:
115
116Np = 4;
117sx = 10;
118sy = 10;
119sz = 5;
120
121om = randn(3,1);
122T = [0;0;100];
123
124noise = 2/1000;
125
126XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
127xx = project_points(XX,om,T);
128
129xxn = xx + noise * randn(2,Np);
130
131[omckk,Tckk] = compute_extrinsic(xxn,XX);
132
133[om omckk om-omckk]
134[T Tckk T-Tckk]
135
136figure(3);
137plot(xx(1,:),xx(2,:),'r+');
138hold on;
139plot(xxn(1,:),xxn(2,:),'g+');
140hold off;
Note: See TracBrowser for help on using the repository browser.