gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+quaternion/rotationToQuaternion.m
function quaternion = rotationToQuaternion(R) % rotationToQuaternion Converts (orthogonal) rotation matrix R to (unit) quaternion. % % quaternion is a 4-by-1 vector % R is a 3x3 orthogonal matrix of corresponding rotation matrix % % Note % ---- % R is rotation of vectors anti-clockwise in a right-handed system by pre-multiplication % % Copyright 2014 The MathWorks, Inc. % References % ---------- % http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion Qxx = R(1,1); Qxy = R(1,2); Qxz = R(1,3); Qyx = R(2,1); Qyy = R(2,2); Qyz = R(2,3); Qzx = R(3,1); Qzy = R(3,2); Qzz = R(3,3); t = Qxx+Qyy+Qzz; if t >= 0, r = sqrt(1+t); s = 0.5/r; w = 0.5*r; x = (Qzy-Qyz)*s; y = (Qxz-Qzx)*s; z = (Qyx-Qxy)*s; else maxv = max(Qxx, max(Qyy, Qzz)); if maxv == Qxx r = sqrt(1+Qxx-Qyy-Qzz); s = 0.5/r; w = (Qzy-Qyz)*s; x = 0.5*r; y = (Qyx+Qxy)*s; z = (Qxz+Qzx)*s; elseif maxv == Qyy r = sqrt(1+Qyy-Qxx-Qzz); s = 0.5/r; w = (Qxz-Qzx)*s; x = (Qyx+Qxy)*s; y = 0.5*r; z = (Qzy+Qyz)*s; else r = sqrt(1+Qzz-Qxx-Qyy); s = 0.5/r; w = (Qyx-Qxy)*s; x = (Qxz+Qzx)*s; y = (Qzy+Qyz)*s; z = 0.5*r; end end quaternion = [w;x;y;z];