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