Lie group 实践

---

代码实践

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

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

旋转矩阵

则对应的绕x轴、y轴与z轴的三个旋转矩阵为：

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。

欧拉角求旋转矩阵

即

旋转矩阵求欧拉角

求解方程，可得
注：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];

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

给定四元数形式：其中， 为旋转轴。
是三维向量，它的模长和方向为、。令为长度为1的方向向量，则有

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;

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

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

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

给定四元数形式：对应的旋转矩阵：

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) );

---

总结：

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