[810] | 1 | %=======================================================================
|
---|
[908] | 2 | % Copyright 2008-2015, LEGI UMR 5519 / CNRS UJF G-INP, Grenoble, France
|
---|
[810] | 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 |
|
---|
[725] | 19 | function [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 |
|
---|
| 56 | if nargin < 8,
|
---|
| 57 | thresh_cond = inf;
|
---|
| 58 | end;
|
---|
| 59 |
|
---|
| 60 |
|
---|
| 61 | if nargin < 7,
|
---|
| 62 | MaxIter = 20;
|
---|
| 63 | end;
|
---|
| 64 |
|
---|
| 65 |
|
---|
| 66 | if 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;
|
---|
| 81 | end;
|
---|
| 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 |
|
---|
| 93 | H = [Rckk(:,1:2) Tckk];
|
---|
| 94 |
|
---|
| 95 | % Computes the reprojection error in pixels:
|
---|
| 96 |
|
---|
| 97 | x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
|
---|
| 98 |
|
---|
| 99 | ex = x_kk - x;
|
---|
| 100 |
|
---|
| 101 |
|
---|
| 102 | % Converts the homography in pixel units:
|
---|
| 103 |
|
---|
| 104 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];
|
---|
| 105 |
|
---|
| 106 | H = KK*H;
|
---|
| 107 |
|
---|
| 108 |
|
---|
| 109 |
|
---|
| 110 |
|
---|
| 111 | return;
|
---|
| 112 |
|
---|
| 113 |
|
---|
| 114 | % Test of compte extrinsic:
|
---|
| 115 |
|
---|
| 116 | Np = 4;
|
---|
| 117 | sx = 10;
|
---|
| 118 | sy = 10;
|
---|
| 119 | sz = 5;
|
---|
| 120 |
|
---|
| 121 | om = randn(3,1);
|
---|
| 122 | T = [0;0;100];
|
---|
| 123 |
|
---|
| 124 | noise = 2/1000;
|
---|
| 125 |
|
---|
| 126 | XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
|
---|
| 127 | xx = project_points(XX,om,T);
|
---|
| 128 |
|
---|
| 129 | xxn = 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 |
|
---|
| 136 | figure(3);
|
---|
| 137 | plot(xx(1,:),xx(2,:),'r+');
|
---|
| 138 | hold on;
|
---|
| 139 | plot(xxn(1,:),xxn(2,:),'g+');
|
---|
| 140 | hold off;
|
---|