Home > voicebox > rotro2qr.m

rotro2qr

PURPOSE

ROTRO2QR converts a 3x3 rotation matrix to a real quaternion

SYNOPSIS

function q=rotro2qr(r)

DESCRIPTION

```ROTRO2QR converts a 3x3 rotation matrix to a real quaternion
Inputs:

R(3,3)   Input rotation matrix

Outputs:

Q(4,1)   normalized real-valued quaternion

In the quaternion representation of a rotation, and q(1) = cos(t/2)
where t is the angle of rotation in the range 0 to 2pi
and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
a positive rotation about [0 0 1] takes the X axis towards the Y axis.```

CROSS-REFERENCE INFORMATION

This function calls:
This function is called by:
• rectifyhomog RECTIFYHOMOG Apply rectifying homographies to an image set

SOURCE CODE

```0001 function q=rotro2qr(r)
0002 %ROTRO2QR converts a 3x3 rotation matrix to a real quaternion
0003 % Inputs:
0004 %
0005 %     R(3,3)   Input rotation matrix
0006 %
0007 % Outputs:
0008 %
0009 %     Q(4,1)   normalized real-valued quaternion
0010 %
0011 % In the quaternion representation of a rotation, and q(1) = cos(t/2)
0012 % where t is the angle of rotation in the range 0 to 2pi
0013 % and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
0014 % a positive rotation about [0 0 1] takes the X axis towards the Y axis.
0015
0016 %      Copyright (C) Mike Brookes 2007
0017 %      Version: \$Id: rotro2qr.m 2186 2012-07-20 13:45:03Z dmb \$
0018 %
0019 %   VOICEBOX is a MATLAB toolbox for speech processing.
0021 %
0022 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0023 %   This program is free software; you can redistribute it and/or modify
0025 %   the Free Software Foundation; either version 2 of the License, or
0026 %   (at your option) any later version.
0027 %
0028 %   This program is distributed in the hope that it will be useful,
0029 %   but WITHOUT ANY WARRANTY; without even the implied warranty of
0030 %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0031 %   GNU General Public License for more details.
0032 %
0033 %   You can obtain a copy of the GNU General Public License from
0034 %   http://www.gnu.org/copyleft/gpl.html or by writing to
0035 %   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
0036 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0037
0038 % in the comments below, t is the rotation angle, a is the rotation axis
0039 q=zeros(4,1);
0040 t = 1 + r(1) + r(5) + r(9);     % 2(1+cos(t)) = 4(cos(t/2))^2 = 4 q(1)^2
0041 if t>1                       % for rotation angles less than 120 degrees
0042     s = sqrt(t) * 2;            % 4 cos(t/2) = 2 sin(t)/sin(t/2)
0043     q(2) = ( r(6) - r(8) ) / s;
0044     q(3) = ( r(7) - r(3) ) / s;
0045     q(4) = ( r(2) - r(4) ) / s;
0046     q(1) = 0.25 * s;            % cos(t/2)
0047 elseif ( r(1) > r(5) && r(1) > r(9) )            % row 1
0048     s  = sqrt( 1.0 + r(1) - r(5) - r(9) ) * 2;  % 4 a(1) sin(t/2) = 2 a(2) (1-cos(t))/sin(t/2)
0049     q(2) = 0.25 * s;
0050     q(3) = (r(2) + r(4) ) / s;
0051     q(4) = (r(7) + r(3) ) / s;
0052     q(1) = (r(6) - r(8) ) / s;
0053 elseif ( r(5) > r(9) )                           % row 2
0054     s  = sqrt( 1.0 + r(5) - r(1) - r(9) ) * 2;
0055     q(2) = (r(2) + r(4) ) / s;
0056     q(3) = 0.25 * s;
0057     q(4) = (r(6) + r(8) ) / s;
0058     q(1) = (r(7) - r(3) ) / s;
0059 else                                             % row 3
0060     s  = sqrt( 1.0 + r(9) - r(1) - r(5) ) * 2;
0061     q(2) = (r(7) + r(3) ) / s;
0062     q(3) = (r(6) + r(8) ) / s;
0063     q(4) = 0.25 * s;
0064     q(1) = (r(2) - r(4) ) / s;
0065 end
0066 f=find(q~=0);
0067 if (q(f(1))<0), q=-q; end           % force positive
0068 if abs(q'*q-1)>1e-8
0069     error('Input to rotro2qr must be a rotation matrix with det(r)=+1');
0070 end
0071```

Generated on Mon 06-Aug-2018 14:48:32 by m2html © 2003