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

Last change on this file since 849 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 3.6 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,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),
20
21%compute_extrinsic
22%
23%[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter)
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%       MaxIter: Maximum number of iterations
36%
37%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
38%        Tckk: 3D translation vector attached to the grid positions in space
39%        Rckk: 3D rotation matrices corresponding to the omc vectors
40
41%
42%Method: Computes the normalized point coordinates, then computes the 3D pose
43%
44%Important functions called within that program:
45%
46%normalize_pixel: Computes the normalize image point coordinates.
47%
48%pose3D: Computes the 3D pose of the structure given the normalized image projection.
49%
50%project_points.m: Computes the 2D image projections of a set of 3D points
51
52
53if nargin < 10,
54   thresh_cond = inf;
55end;
56
57
58if nargin < 9,
59   MaxIter = 20;
60end;
61
62if nargin < 8,
63    alpha_c = 0;
64    if nargin < 7,
65        kc = zeros(5,1);
66        if nargin < 6,
67            cc = zeros(2,1);
68            if nargin < 5,
69                fc = ones(2,1);
70                if nargin < 4,
71                    error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)');
72                    return;
73                end;
74            end;
75        end;
76    end;
77end;
78
79
80% Initialization:
81
82omckk = omc_init;
83Tckk = Tc_init;
84
85
86% Final optimization (minimize the reprojection error in pixel):
87% through Gradient Descent:
88
89param = [omckk;Tckk];
90
91change = 1;
92
93iter = 0;
94
95%keyboard;
96
97%fprintf(1,'Gradient descent iterations: ');
98
99while (change > 1e-10)&(iter < MaxIter),
100   
101    %fprintf(1,'%d...',iter+1);
102   
103    [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
104   
105    ex = x_kk - x;
106   
107    %keyboard;
108   
109    JJ = [dxdom dxdT];
110   
111    if cond(JJ) > thresh_cond,
112        change = 0;
113    else
114       
115        JJ2 = JJ'*JJ;
116       
117        param_innov = inv(JJ2)*(JJ')*ex(:);
118        param_up = param + param_innov;
119        change = norm(param_innov)/norm(param_up);
120        param = param_up;
121        iter = iter + 1;
122       
123        omckk = param(1:3);
124        Tckk = param(4:6);
125       
126    end;
127   
128end;
129
130%fprintf(1,'\n');
131
132Rckk = rodrigues(omckk);
Note: See TracBrowser for help on using the repository browser.