quart.m

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

function rotmat = quart(rotAx, rotAng)
%DEVELOPMENT PHASE
%
% rotmat = quart(rotAx, rotAng)
%
% computes rotation matrix of quarternions
%
% rotmat - rotation matrix
%
% rotAx - rotation axis [xr, yr, zr]
% rotAng - rotation angle

m1 = cos( rotAng/2 );
m2 = sin(( rotAng/2 ) * rotAx(1,1));
m3 = sin(( rotAng/2 ) * rotAx(1,2));
m4 = sin(( rotAng/2 ) * rotAx(1,3));

m = [m1, m2, m3, m4]'

rotmat = [1 - 2*(m3^2 + m4^2), 2 * (m2*m3 + m1*m4), 2 * (m2*m4 - m1*m3);
	2 * (m2*m3 - m1*m4), 1 - 2 * (m2^2 + m4^2), 2 * (m3*m4 + m1*m2);
	2 * (m2*m4 + m1*m3), 2 * (m3*m4 - m1*m2), 1 - 2 * (m2^2 + m3^2) ];

rotmat = rotmat ./ vsize(m);