代码实践

【一】旋转矩阵 --> 欧拉角

https://blog.csdn.net/u012423865/article/details/78219787?locationNum=9&fps=1

旋转矩阵 $\mathbf{R}=\left[\begin{matrix} r_{11}& r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{matrix}\right]$

则对应的绕x轴、y轴与z轴的三个旋转矩阵为：
$\mathbf{R}_x(\theta)=\left[\begin{matrix} 1& 0 & 0\\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{matrix}\right]、\mathbf{R}_y(\theta)=\left[\begin{matrix} cos(\theta)& 0 & sin(\theta)\\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{matrix}\right]$ $\mathbf{R}_z(\theta)=\left[\begin{matrix} cos(\theta) & -sin(\theta) & 0\\ sin(\theta) & cos(\theta) & 0\\ 0 & 0 & 1 \end{matrix}\right]$
任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。

欧拉角求旋转矩阵

$\mathbf{R}(\theta_x, \theta_y, \theta_z)=\mathbf{R}_x(\theta_x)\mathbf{R}_y(\theta_y)\mathbf{R}_z(\theta_z)$ 即
$\mathbf{R}(\theta_x, \theta_y, \theta_z)=\left[\begin{matrix} c_yc_z & c_zs_xs_y & s_xs_z+c_xc_zs_y\\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_z\\ -s_y & c_ys_z & c_xc_y \end{matrix}\right]$

旋转矩阵求欧拉角

$\mathbf{R}(\theta_x, \theta_y, \theta_z)=\left[\begin{matrix} r_{11}& r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{matrix}\right]=\left[\begin{matrix} c_yc_z & c_zs_xs_y & s_xs_z+c_xc_zs_y\\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_z\\ -s_y & c_ys_z & c_xc_y \end{matrix}\right]$ 求解方程，可得 $\begin{align} & \theta_{x}=atan2(r_{32}, r_{33})\\ & \theta_{y}=atan2(-r_{31}, \sqrt{r_{32}^2+r_{33}^2})\\ & \theta_{z}=atan2(r_{21}, r_{11}) \end{align}$
注：atan2()为C++中的函数，atan2(y, x) 求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。

function Eul = RotMat2Euler(R)

% Code provided by Graham Taylor, Geoff Hinton and Sam Roweis 
%
% Finds one of two equivalent Euler angle representations for a Direction
% Cosine Matrix
% Assumes the DCM is in 'zyx' order
% Given R, the rotation matrix
% Returns a vector of Euler angles (in radians)
%  the first about x axis, the second about y axis, the third about z axis
% Based on an article by Gregory G. Slabaugh
%
% Usage Eul = RotMat2Euler(R)

%Note we need to treat the case of cos(E2) = +- pi/2 separately
%This corresponds to element R(1,3) = +- 1

if R(1,3) == 1 | R(1,3) == -1   % 这里对应上面的(3, 1) 
  %special case
  E3 = 0; %set arbitrarily
  dlta = atan2(R(1,2),R(1,3));
  if R(1,3) == -1
    E2 = pi/2;
    E1 = E3 + dlta;
  else
    E2 = -pi/2;
    E1 = -E3 + dlta;
  end
else
  E2 = - asin(R(1,3));
  E1 = atan2(R(2,3)/cos(E2), R(3,3)/cos(E2));
  E3 = atan2(R(1,2)/cos(E2), R(1,1)/cos(E2));
end

Eul = [E1 E2 E3];

【二】四元数 --> 指数映射

给定四元数形式： $\mathbf{q}=[\cos(\frac{\theta}{2}), n_x \sin(\frac{\theta}{2}), n_y \sin(\frac{\theta}{2}), n_z \sin(\frac{\theta}{2})]$ 其中， $\mathbf{n}$ 为旋转轴。
$\begin{aligned} &\theta=2\arccos q_0\\ &[n_x, n_y, n_z]^T=[q_1, q_2, q_3]^T/sin{\frac{\theta}{2}} \end{aligned}$ $\mathbf{n}$ 是三维向量，它的模长和方向为 $\theta$ 、 $\mathbf{n_0}$ 。令 $\mathbf{n_0}$ 为长度为1的方向向量，则有 $\mathbf{n}=\theta\mathbf{n_0}$

function [r]=quat2expmap(q)

% Software provided by Hao Zhang
% http://www.cs.berkeley.edu/~nhz/software/rotations
%
% function [r]=quat2expmap(q)
% convert quaternion q into exponential map r
% 
% denote the axis of rotation by unit vector r0, the angle by theta
% 这里的四元数是单位四元数
% q is of the form (cos(theta/2), r0*sin(theta/2))
% r is of the form r0*theta
  
  % 四元数范数要为1 
  if (abs(norm(q)-1)>1E-3)
    error('quat2expmap: input quaternion is not norm 1');
  end
  sinhalftheta=norm(q(2:4));     % 后三维求范数sin(theta/2)
  coshalftheta=q(1);             % 第一维就是cos(theta/2)
  r0=q(2:4)/(norm(q(2:4))+eps);  % 这一步的得到(n_x, n_y, n_z)，即旋转轴
  % 四元数求theta角
  % atan2(y, x) Return the arc tangent (measured in radians) of y/x
  % 因为已经求得sin(x/2)和cos(x/2)，所以可以反解出x
  theta=2*atan2(sinhalftheta,coshalftheta);
  % 将角度调整到[0, 2pi)
  theta=mod(theta+2*pi,2*pi);
  if (theta>pi)     % 若角度大于180，则方向取反
    theta=2*pi-theta; 
    r0=-r0; 
  end
  r=r0*theta;

【三】指数映射 --> 旋转矩阵

$\mathbf{R}=exp(\mathbf{\phi}^{\wedge})=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{a}\mathbf{a}^T+\sin{\theta}\mathbf{a}^{\wedge}$

def expmap2rotmat(r):
  """
  Converts an exponential map angle to a rotation matrix
  Matlab port to python for evaluation purposes
  I believe this is also called Rodrigues' formula
  https://github.com/asheshjain399/RNNexp/blob/srnn/structural_rnn/CRFProblems/H3.6m/mhmublv/Motion/expmap2rotmat.m

  Args
    r: 1x3 exponential map
  Returns
    R: 3x3 rotation matrix
  """
  theta = np.linalg.norm( r )
  r0  = np.divide( r, max(theta, np.finfo(np.float32).eps) )
  r0x = np.array([0, -r0[2], r0[1], 0, 0, -r0[0], 0, 0, 0]).reshape(3,3)
  r0x = r0x - r0x.T
  R = np.eye(3,3) + np.sin(theta)*r0x + (1-np.cos(theta))*(r0x).dot(r0x);
  return R

【四】旋转矩阵 --> 四元数

$\mathbf{q}=q_0+q_1i+q_2j+q_3k$ 给定四元数形式： $\mathbf{q}=[\cos(\frac{\theta}{2}), n_x \sin(\frac{\theta}{2}), n_y \sin(\frac{\theta}{2}), n_z \sin(\frac{\theta}{2})]$ 对应的旋转矩阵 $\mathbf{R}$ ： $q_0\! = \!\frac{\sqrt{\mathbf{tr}(\mathbf(R))\!+\!1}}{2},q_1\!=\!\frac{m_{32}\!-\!m_{23}}{4q_0},q_2\!=\!\frac{m_{13}\!-\!m_{31}}{4q_0},\ \ q_3\!=\!\frac{m_{21}\!-\!m_{12}}{4q_0}$

function q=rotmat2quat(R)
% function q=rotmat2quat(R)
% convert a rotation matrix R into unit quaternion q
%  
% denote the axis of rotation by unit vector r0, the angle by theta
 % q is of the form (cos(theta/2), r0*sin(theta/2))

  % 如果 (norm(R*R'-eye(3,3))>1E-10 || det(R)<0)
  % rotmat2quat: input matrix is not a rotation matrix
  d=R-R';
  r(1)=-d(2,3);   % m23-m32
  r(2)=d(1,3);    % m31-m13
  r(3)=-d(1,2);   % m12-m32

  % norm(r)的结果是
  sintheta=norm(r)/2;   
  r0=r/(norm(r)+eps);  % 后三项的范数为sin(theta/2)，归一化就是r0=(nx,ny,nz)
  % 直接求第一项q0，就等于单位四元数第一项cos(theta)
  costheta=(trace(R)-1)/2;
  
  theta=atan2(sintheta,costheta);
  q=[cos(theta/2) r0*sin(theta/2)];

【五】旋转矩阵 --> 指数映射

先转四元数，再转指数映射

def rotmat2expmap(R):
  return quat2expmap( rotmat2quat(R) );

总结：

欧拉角<->旋转矩阵->四元数->指数映射->旋转矩阵

Lie group 实践