dir2rot.m

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

function [out1, out2] = dir2rot(in1, in2)
%[out1, out2] = DIR2ROT(in1, in2)
%
%Convert between direction and rotation matrix. Variants:
%  [rotangle, rotaxis] = DIR2ROT(origdir, finaldir)
%  rotmatrix = DIR2ROT(origdir, finaldir)
%  rotmatrix = DIR2ROT(rotangle, rotaxis)
%  rotmatrix = DIR2ROT(rotangle, rotaz)
%
%Variables:
%  ORIGDIR   - original direction
%  FINALDIR  - final direction (default [0, 0, 1])
%  ROTANGLE  - angle of rotation (in degrees)
%  ROTAXIS   - axis of rotation (default [0, 0, 1])
%  ROTAZ     - azimuth of rotation (in degrees)
%  ROTMATRIX - 3x3 rotation matrix 
%
%See also:
%  ROTMATRIX, TRANSFORM
%
%Radim Halir, Charles University Prague, [e-mail]
%Created: 17.11.1996
%Last modified 14.1.1998

% default parameters
if (nargin == 1)
  in2 = [0, 0, 1];
end

if (length(in1) == 1)
% input: angle rotaxis/azimuth
  rotangle = in1;
  if (length(in2) == 1)
    % azimuth
    in2 = in2 / 180 * pi;
    rotaxis = [- sin(in2), cos(in2), 0];
  else
    % rotation axis
    rotaxis = in2;
  end
else
% input: original_direction final_direction
  in1 = vnorm(in1(:)');
  in2 = vnorm(in2(:)');
  
  if( cross(in1, in2) == [0,0,0] )
  %oba kolme
    if(nargout > 1)
      out1 = 0;
      out2 = in2;
      return;
    else
      out1 = eye(3);
      return;
    end
  end
  
  rotangle = acos(dot(in1, in2)) / pi * 180;
  rotaxis = vnorm(cross(in1, in2));
  if (nargout > 1)
    out1 = rotangle;
    out2 = rotaxis;
    return
  end
end

% check the angle
if (rem(rotangle, 360) == 0)
  out1 = eye(3);
  return
end
  
% create quaternion
rotangle = (rotangle / 180 * pi) / 2;
m = [cos(rotangle) sin(rotangle) * vnorm(rotaxis(:)')];

% quaternion -> rotation matrix
m = m' * m;
m01 = m(1, 2);
m02 = m(1, 3);
m03 = m(1, 4);
m11 = m(2, 2);
m12 = m(2, 3);
m13 = m(2, 4);
m22 = m(3, 3);
m23 = m(3, 4);
m33 = m(4, 4);
rot = 2 * [0.5 - (m22 + m33), m12 - m03, m13 + m02; ...
           m12 + m03, 0.5 - (m11 + m33), m23 - m01; ...
           m13 - m02, m23 + m01, 0.5 - (m11 + m22)];
out1 = rot;