[926] | 1 | function [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 | |
---|
| 38 | if nargin < 8, |
---|
| 39 | thresh_cond = inf; |
---|
| 40 | end; |
---|
| 41 | |
---|
| 42 | |
---|
| 43 | if nargin < 7, |
---|
| 44 | MaxIter = 20; |
---|
| 45 | end; |
---|
| 46 | |
---|
| 47 | |
---|
| 48 | if 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; |
---|
| 63 | end; |
---|
| 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 | |
---|
| 75 | H = [Rckk(:,1:2) Tckk]; |
---|
| 76 | |
---|
| 77 | % Computes the reprojection error in pixels: |
---|
| 78 | |
---|
| 79 | x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); |
---|
| 80 | |
---|
| 81 | ex = x_kk - x; |
---|
| 82 | |
---|
| 83 | |
---|
| 84 | % Converts the homography in pixel units: |
---|
| 85 | |
---|
| 86 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; |
---|
| 87 | |
---|
| 88 | H = KK*H; |
---|
| 89 | |
---|
| 90 | |
---|
| 91 | |
---|
| 92 | |
---|
| 93 | return; |
---|
| 94 | |
---|
| 95 | |
---|
| 96 | % Test of compte extrinsic: |
---|
| 97 | |
---|
| 98 | Np = 4; |
---|
| 99 | sx = 10; |
---|
| 100 | sy = 10; |
---|
| 101 | sz = 5; |
---|
| 102 | |
---|
| 103 | om = randn(3,1); |
---|
| 104 | T = [0;0;100]; |
---|
| 105 | |
---|
| 106 | noise = 2/1000; |
---|
| 107 | |
---|
| 108 | XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; |
---|
| 109 | xx = project_points(XX,om,T); |
---|
| 110 | |
---|
| 111 | xxn = 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 | |
---|
| 118 | figure(3); |
---|
| 119 | plot(xx(1,:),xx(2,:),'r+'); |
---|
| 120 | hold on; |
---|
| 121 | plot(xxn(1,:),xxn(2,:),'g+'); |
---|
| 122 | hold off; |
---|