-
Notifications
You must be signed in to change notification settings - Fork 96
/
campose4.m
100 lines (86 loc) · 2.88 KB
/
campose4.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
%CAMPOSE4 Camera pose estimation from 4 coplanar points.
%
% T = CAMPOSE4(D, ci)
% [T, UV0] = CAMPOSE4(D, ci)
%
% Input is a table of data points, D, with each row of the form [X Y u v]
% where (X, Y) are the world coordinate of the planar points with respect
% to some coordinate system whose origin lies within the plane,
% and (u, v) are the corresponding image plane coordinate.
%
% ci is a structure of camera intrinsic parameters that must contain
% focal length (f), and pixel sizes (sx, sy).
%
% Output is a homogeneous transformation, T, of the camera's pose with
% respect to the coordinate frame of the planar points.
% The optional result, UV0, is the coordinate of the principal point (in
% distance units).
%
% from Ganapathy "Camera Location Determination Problem",
% Bell Labs Tech. Memo 11358-841102-20-TM, Nov 2 1984
%
% SEE ALSO: CAMPOSE4, CAMCALP, CAMERA, CAMCALT, INVCAMCAL
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB. If not, see <http://www.gnu.org/licenses/>.
function [T, uv0] = campose4(uvXY, ci)
t = camcald4(uvXY);
k(1) = ci.f / ci.sx;
k(2) = ci.f / ci.sy;
% K = [k1 k2] contains the X and Y-direction scale
% factors: k1 = kx * F, k2 = ky * F, where F is the focal length.
%
lam1 = t(1,1)*t(3,2) - t(1,2)*t(3,1);
lam2 = t(2,1)*t(3,2) - t(2,2)*t(3,1);
if abs(lam1) < 1e-8,
lam1 = 0;
end
if abs(lam2) < 1e-8,
lam2 = 0;
end
if (lam1 == 0) & (lam2 == 0),
r2 = (k(1)^2 + k(2)^2) / sqrt(t(1,1)^2 + t(1,2)^2 + t(2,1)^2 + t(2,2)^2);
r = sqrt(r2);
disp('Problem with solution: lam1 = lam2 = 0');
i = 1;
f = 0;
c = 0;
g = 0;
h = 0;
u0 = ci.u0;
v0 = ci.v0;
else
r2 = (t(3,1)^2 + t(3,2)^2)/( (lam1/k(1))^2 + (lam2/k(2))^2 );
r = sqrt(r2);
i2 = 1 - r2*(t(3,1)^2 + t(3,2)^2);
i = sqrt(i2);
f = -lam1/k(1)*r2;
c = lam2/k(2)*r2;
u0 = (t(1,1)*t(3,1) + t(1,2)*t(3,2) + k(1)*c*i/r2) / (t(3,1)^2 + t(3,2)^2);
v0 = (t(2,1)*t(3,1) + t(2,2)*t(3,2) + k(2)*f*i/r2) / (t(3,1)^2 + t(3,2)^2);
g = t(3,1)*r;
h = t(3,2)*r;
end
a = (t(1,1)*r - u0*g) / k(1);
b = (t(1,2)*r - u0*h) / k(1);
d = (t(2,1)*r - v0*g) / k(2);
e = (t(2,2)*r - v0*h) / k(2);
p = (t(1,3)*r - u0*r) / k(1);
q = (t(2,3)*r - v0*r) / k(2);
R = [a b c;d e f;g h i];
P = [p q r];
T = [R P'; 0 0 0 1];
uv0 = [u0 v0];