rotaxis.m

一个很好用的摄像机标定程序

function [point, direction, error, residua, cstat] = ...
rotaxis(normals, precision, verbose)
%[point, direction, error, residua, cstat] = ...
%ROTAXIS(normals, precision, verbose)
%
%Estimate a rotation axis of a surface of revolution
%
%Input:
%  NORMALS   - surface normals; cell with six arrays (points [x, y, z] and
%              unit directions [a, b, c])
%  PRECISION - required precision (see LEASTSQ and FOPTIONS, default 0.1)
%  VERBOSE   - 1 for verbose output during optimization (default 1)
%
%Output:
%  POINT     - the point lying on the rotation axis
%  DIRECTION - normalized direction of the rotation axis
%  ERROR     - error of the fitting (mean Eucleidan distance between the
%              surface normals and the estimated axis)
%  RESIDUA   - final residua
%  CSTAT     - computation statistics:
%		CSTAT(1) ... number of iterations
%		CSTAT(2) ... time spent in the optimization phase
%		CSTAT(3) ... total time spent in this function
%
%Notes:
%  - the axis is estimated by a Gauss-Newton non-linear least squares optimizer
%    LEASTSQ from Optimization Toolbox
%  - the objective function which is minimized is defined in ROTAXIS_FUN
%  - surface normals can be estimated by NORMALEST
%  - robust version of this estimation via M-estimators is available in
%    RROTAXIS
%
%See also:
%  leastsq, ROTAXIS_FUN, RROTAXIS, NORMALEST
%
%Radim Halir, Charles University Prague, [e-mail]
%Created: 21.3.1997
%Last modified 23.9.1998

% default parameters
if (nargin < 2)
  precision = 0.1;
end
if (nargin < 3)
  verbose = 1;
end

% initialization
starttime = clock;
[coords, dims] = data2cols(cat(3, normals{:}));
[coords, ind] = delnanrows(coords);
points = coords(:, 1 : 3);
normals = coords(:, 4 : 6);
npoints = size(points, 1);
center = sum(points) ./ npoints;
if (verbose)
  fprintf(1, 'Number of points: %d\n', npoints);

% finding principal direction
  fprintf(1, 'Finding initial direction: ');
end
directions = [0 0; 90 0; 0 90];
temp = size(directions, 1);
value = zeros(1, temp);
for i = 1 : temp
  value(i) = sum(rotaxis_fun(directions(i, :), points, normals, center) .^ 2);
  if (verbose)
    fprintf(1, '%.3g ', value(i));
  end
end
if (verbose)
  fprintf(1, '\n');
end
[temp, i] = min(value);
direction = directions(i, :);

% optimization
options(1) = (verbose > 0);		% verbose output?
options(2) = precision;			% termination tolerance for direction
options(3) = precision * temp / 200;	% terminal tolerance for value
	% 1 degree is about 0.5 per cent of direction
options(5) = 1;				% 0 for Lev-Marq, 1 for Gaus-Newton
	% Gauss-Newton is much faster than Levenberg-Marquardt in our case
opttime = clock;
[direction, options] = leastsq('rotaxis_fun', direction, options, [], ...
			       points, normals, center);
opttime = etime(clock, opttime);

% point and normal direction
[res, point, direction] = rotaxis_fun(direction, points, normals, center);
direction = vorient(direction);
if (verbose)
  temp = dir2angle(direction);
  fprintf(1, '\nNormal direction: (%.2f, %.2f) degrees\n', temp(1), temp(2));
end

% error and residua
temp = abs(res);
error = mean(temp);
residua = repmat(NaN, dims);
residua(ind) = temp;

% finalization
cstat = [options(10), opttime, etime(clock, starttime)];
if (verbose)
  fprintf(1, '%d iterations, optimization time: %.3f, total time: %.3f\n', ...
	  cstat(1), cstat(2), cstat(3));
end