-
Notifications
You must be signed in to change notification settings - Fork 96
/
camcald.m
72 lines (62 loc) · 2.23 KB
/
camcald.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
% CAMCALD Camera calibration from data points
%
% C = CAMCALD(D) is the camera matrix (3x4) determined by least squares
% from corresponding world and image-plane points. D is a table
% of points with rows of the form [X Y Z U V] where (X,Y,Z) is the
% coordinate of a world point and [U,V] is the corresponding image
% plane coordinate.
%
% [C,E] = CAMCALD(D) as above but E is the maximum residual error after
% back substitution [pixels].
%
% Notes:
% - This method assumes no lense distortion affecting the image plane
% coordinates.
%
% See also CentralCamera.
% 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 [C, resid] = camcald(XYZ, uv)
if numcols(XYZ) ~= numcols(uv)
error('must have same number of world and image-plane points');
end
n = numcols(XYZ);
if n < 6,
error('At least 6 points required for calibration');
end
%
% build the matrix as per Ballard and Brown p.482
%
% the row pair are one row at this point
%
A = [ XYZ' ones(n,1) zeros(n,4) -repmat(uv(1,:)', 1,3).*XYZ' ...
zeros(n,4) XYZ' ones(n,1) -repmat(uv(2,:)', 1,3).*XYZ' ];
%
% reshape the matrix, so that the rows interleave
%
A = reshape(A',11, n*2)';
if rank(A) < 11,
error('Rank deficient, perhaps points are coplanar or collinear');
end
B = reshape( uv, 1, n*2)';
C = A\B; % least squares solution
resid = max(max(abs(A * C - B)));
if resid > 1,
warning('Residual greater than 1 pixel');
end
fprintf('maxm residual %f pixels.\n', resid);
C = reshape([C;1]',4,3)';