%% Extract Quaternions from Transformation Matrix
% If you have a 3-D transformation matrix created using functions such as rigid3d or affine3d, you can extract the rotation matrix from it and represent it in the form of a quaternion. However, before performing this conversion, you must first convert the rotation matrix from the postmultiply format to the premultiply format expected by quaternions.
%
%% Postmultiply Format
% To perform rotations using the rotation matrix part of a transformation matrix, multiply an (x, y, z) point by this rotation matrix.
% In point rotations, this point is rotated within a frame of reference.
% In frame rotations, the frame of reference is rotated around this point.
% Transformation matrices represented by rigid3d or affine3d objects use a postmultiply format. In this format, the point is multiplied by the rotation matrix, in that order. To satisfy the matrix multiplication, the point and its corresponding translation vector must be row vectors.
% This equation shows the postmultiply format for a rotation matrix, R, and a translation vector, t.
% \left\lbrack \begin{array}{ccc}
% x\prime  & y^{\prime }  & z^{\prime }
% \end{array}\right\rbrack =\left\lbrack \begin{array}{ccc}
% x & y & z
% \end{array}\right\rbrack \left\lbrack \begin{array}{ccc}
% R_{11}  & R_{12}  & R_{13} \\
% R_{21}  & R_{22}  & R_{23} \\
% R_{31}  & R_{32}  & R_{33}
% \end{array}\right\rbrack +\left\lbrack \begin{array}{ccc}
% t_x  & t_y  & t_z
% \end{array}\right\rbrack
% This format also applies when R and t are combined into a homogeneous transformation matrix. In this matrix, the 1 is used to satisfy the matrix multiplication and can be ignored.
% \left\lbrack \begin{array}{cccc}
% x^{\prime }  & y^{\prime }  & z^{\prime }  & 1
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cccc}
% x & y & z & 1
% \end{array}\right\rbrack \left\lbrack \begin{array}{cccc}
% R_{11}  & R_{12}  & R_{13}  & 0\\
% R_{21}  & R_{22}  & R_{23}  & 0\\
% R_{31}  & R_{32}  & R_{33}  & 0\\
% t_x  & t_y  & t_z  & 1
% \end{array}\right\rbrack
%
%% Premultiply Format
% In the premultiply format, the rotation matrix is multiplied by the point, in that order. To satisfy the matrix multiplication, the point and its corresponding translation vector must be column vectors.
% This equation shows the premultiply format, where R is the rotation matrix and t is the translation vector.
% \left\lbrack \begin{array}{c}
% x\prime \\
% y^{\prime } \\
% z^{\prime }
% \end{array}\right\rbrack =\left\lbrack \begin{array}{ccc}
% R_{11}  & R_{12}  & R_{13} \\
% R_{21}  & R_{22}  & R_{23} \\
% R_{31}  & R_{32}  & R_{33}
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% x\\
% y\\
% z
% \end{array}\right\rbrack +\left\lbrack \begin{array}{c}
% t_x \\
% t_y \\
% t_z
% \end{array}\right\rbrack
% As with the postmultiply case, this format also applies when R and t are combined into a homogeneous transformation matrix.
% \left\lbrack \begin{array}{c}
% x^{\prime } \\
% y^{\prime } \\
% z^{\prime } \\
% 1
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cccc}
% R_{11}  & R_{12}  & R_{13}  & t_x \\
% R_{21}  & R_{22}  & R_{23}  & t_y \\
% R_{31}  & R_{32}  & R_{33}  & t_z \\
% 0 & 0 & 0 & 1
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% x\\
% y\\
% z\\
% 1
% \end{array}\right\rbrack
%
%% Convert from Postmultiply to Premultiply Format
% To convert a rotation matrix to the premultiply format expected by quaternions, take the transpose of the rotation matrix.
% Create a 3-D rigid geometric transformation object from a rotation matrix and translation vector. The angle of rotation, \theta, is in degrees.

theta = 30;
rot = [ cosd(theta) sind(theta) 0; ...
       -sind(theta) cosd(theta) 0; ...
                  0           0 1];
trans = [2 3 4];

tform = rigid3d(rot,trans)

% The elements of the rotation matrix are ordered for rotating points using the postmultiply format. Convert the matrix to the premultiply format by taking its transpose. Notice that the second element of the first row and first column swap locations.

rotPost = tform.Rotation
rotPre = rotPost.'

% Create a quaternion from the premultiply version of the rotation matrix. Specify that the rotation matrix is configured for point rotations.

q = quaternion(rotPre,'rotmat','point')

%
% To verify that the premultiplied quaternion and the postmultiplied rotation matrix produce the same results, rotate a sample point using both approaches.

point = [1 2 3];
rotatedPointQuaternion = rotatepoint(q,point)
rotatedPointRotationMatrix = point * rotPost

%
% To convert back to the original rotation matrix, extract a rotation matrix from the quaternion. Then, create a rigid3d object by using the transpose of this rotation matrix.

R = rotmat(q,'point');
T = rigid3d(R',trans);
T.Rotation

% Copyright 2019 The MathWorks, Inc.