rotaxis_fun.m

来自「一个很好用的摄像机标定程序」· M 代码 · 共 26 行

function [residua, point, normal] = ...
rotaxis_fun(direction, points, normals, center, weights)
%[residua, point, normal] = ...
%ROTAXIS_FUN(direction, points, normals, center, weights)
%
%Function which is minimized in ROTAXIS/RROTAXIS via leastsq
%
%Radim Halir, Charles University Prague, [e-mail]
%Created: 21.3.1997
%Last modified 23.9.1998

normal = angle2dir(direction);
vectors = vnorm(vcross(normal, normals));
diffs = dot(points, vectors, 2);
if (nargin < 5)
  evectors = [vectors; normal];
  ediffs = [diffs;  center * normal'];
else
  evectors = [mvop(vectors, weights, '.*'); normal];
  ediffs = [diffs .* weights;  center * normal'];
end  
% the following computation is MUCH faster than point = evectors \ ediffs;
point = (evectors' * evectors) \ (evectors' * ediffs);
residua = vectors * point - diffs;
point = point';