Geometry Rotations

Rotation Representations

Axis-Angle

三维旋转可以通过一个单位向量即旋转轴，和绕该轴的旋转角来表达，即这里所说的轴角表达方式 (Axis-Angle Representation)。
轴角可能是最容易理解的表达三维旋转的方式，但是需要注意的两点是，如果要结合两个旋转，需要将轴角表达转化为矩阵或者四元数形式，另外，在0和180度时，微小的改变会使旋转轴突然变化很大 (there are two singularities at 0° and 180° where the axis can jump suddenly for a small change in input.)。在旋转角为0时，旋转轴可以是任意的单位向量；在旋转角为180度时，旋转轴有两个方向。

Euler Angles

当将三维旋转分解为三个独立的旋转角来表达时，一般称为欧拉角 (Euler Angle)。因为在结合多个旋转时，作用顺序至关重要，所以有多种不同的欧拉角。在使用欧拉角时，以下几点必须明白：

结合旋转角的顺序 (Order of combining angles)

假设用x,y,z分别表示绕x,y,z坐标轴的一个旋转，所以三维旋转的顺序有xyz, yzx, zxy，颠倒顺序有zyx, xzy, yxz，共计6种方式。但由于绕每个轴旋转并不是独立的，三维旋转还可以结合两个平面的旋转来构成，即xyx, xzx, yxy, yzy, zxz and zyz。所以一共有12种可能的顺序。
相对于旋转物体还是世界坐标系 (Relative to rotating object or absolute coordinates)

旋转角可能是相对于世界坐标系或其他坐标系，也有可能相对于被旋转的物体。
左手还是右手定则 (Left or right hand rotation)
旋转角的单位 (Notation for angles)

一种欧拉角参考
aeroplaneGlobal1

	airplane	telescope	symbol	angular velocity
applied first	heading	azimuth	$\theta$ (theta)	yaw	y
applied second	attitude	elevation	$\phi$ (phi)	pitch	z
applied last	bank	tilt	$\psi$ (psi)	roll	x

Rotation Matrices

三维旋转可以通过 ==行列式为1的正交矩阵== 来表示。

$R_x= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta & \cos\theta \end{bmatrix} R_y= \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} R_z= \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

一个矩阵是旋转矩阵的充要条件是，该矩阵为正交矩阵且行列式为1，即满足$A^TA=A^{-1}A=I$，$det(A)=1$。注意，正交矩阵不一定是旋转阵，如果正交阵的行列式为1，则其为旋转阵，但==如果行列式为-1，则其表示反射矩阵== 4。

Quaternions

四元数可以表达三维镜像 (reflections)、旋转 (rotations)和缩放 (scaling)，但一个四元数不包含平移，所以如果需要围绕一个非原点的点旋转、镜像或者缩放，需要独立处理平移部分。

当四元数是 ==单位长度== 时，其表示三维旋转，多个四元数相乘则表示多个旋转变换的结合。

此时可以将其看作与轴角类似，只是四元数的实数部分是 cos(angle/2)，虚数部分是旋转轴向量乘以sin(angle/2)，即对于轴角来说，其对应的四元数为

$q = \cos(a/2) + i(x*\sin(a/2)) + j(y*\sin(a/2)) + k(z*\sin(a/2))$

其中 $a$ 是旋转角，$[x, y, z]$ 是旋转轴。

四元数 $i$ 表示绕 $x$ 轴旋转180度，$i*i = -1$ 表示绕 $x$ 轴旋转360度，$j, k$ 分别表示绕 $y, z$ 轴旋转180度。如果$(a + b i + c j + d k) $ 表示一个旋转，则 $ (- a - b i - c j - d k) $ 表示相同的旋转，这从轴角表达的角度来看是有道理的，如果将旋转角和旋转轴同时取反，将得到相同的结果。所以1和-1都表示identity旋转（什么都没做）。

虽然只要3个数（如欧拉角）便可以表示三维旋转，但这样的表达是非线性的，不易操作。但如果将三维空间的旋转映射到 ==四维空间的单位球上==，这个问题便变成线性的了。

Rotation Conversion

AxisAngle to Others

AxisAngle to Euler

1
2
3

heading = atan2(y*sin(angle)-x*z*(1-cos(angle)), 1-(y2+z2)*(1-cos(angle)))
attitude = asin(x*y*(1-cos(angle)) + z*sin(angle)) 
bank = atan2(x*sin(angle)-y*z*(1-cos(angle)), 1-(x2+z2)*(1-cos(angle)))

除了在奇异点处：

向上：heading = 2 * atan2(x * sin(angle/2),cos(angle/2)), bank = 0
向下：heading = -2 * atan2(x * sin(angle/2),cos(angle/2)), bank = 0

except at the singularities, straight up:
heading = 2 atan2(x sin(angle/2),cos(angle/2))
bank = 0
straight down:
heading = -2 atan2(x sin(angle/2),cos(angle/2))
bank = 0

注意：

不同的应用场景计算的结果可能不同；
最好使用atan2函数而不是atan函数；
这里假设旋转轴是单位向量满足ax*ax + ay*ay + az*az = 1。

AxisAngle to Matrix

公式

angle = 2 * acos(qw)
x = qx / sqrt(1-qw*qw)
y = qy / sqrt(1-qw*qw)
z = qz / sqrt(1-qw*qw)

奇异点问题

轴角表示法在角度为0和180度时有两个奇异点，在角度为0时，旋转轴是任意的（任何旋转轴都产生同样的结果），在角度为180度时，旋转轴仍然是无关的，所以此时需要计算旋转轴。

按照轴角表达其对应的四元数为

1	q = cos(angle/2) + i (xsin(angle/2)) + j (ysin(angle/2)) + k (z*sin(angle/2))

角度为0时对应的四元数为q = 1 + i 0 + j 0 + k 0，所以利用上面转化公式

angle = 2 * acos(qw) = 0
x = qx / sqrt(1-qw*qw) = divide by zero = infinity
y = qy / sqrt(1-qw*qw) = divide by zero = infinity
z = qz / sqrt(1-qw*qw) = divide by zero = infinity

所以此时必须检查除数为0的情况，但这不是问题，因为旋转轴可以被设为任意一个单位化的向量。

角度为180度时对应的四元数为q = 0 + i x + j y + k z，所以利用转化公式有

angle = 2 * acos(qw) = 2 * 90 degrees = 180 degrees (or -180 degrees)
x = qx / sqrt(1-qw*qw) = qx
y = qy / sqrt(1-qw*qw) = qy
z = qz / sqrt(1-qw*qw) = qw

Which is correct so the formula works properly in this case. Although some axis angle calculations can jump suddenly around 180 degrees, this quaternion to axis-angle translation seems quite smooth at this region.

AxisAngle to Quaternion

公式

qx = ax * sin(angle/2)
qy = ay * sin(angle/2)
qz = az * sin(angle/2)
qw = cos(angle/2)

注意：

旋转轴是标准化后的，即满足ax*ax + ay*ay + az*az = 1
四元数同样是标准化后的，满足cos(angle/2)^2 + ax*ax*sin(angle/2)^2 + ay*ay*sin(angle/2)^2+ az*az*sin(angle/2)^2 = 1

Euler to Others

Euler to AxisAngle

公式

angle = 2 * acos(c1c2c3 - s1s2s3)
x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3

为了归一化旋转轴x, y , z需分别除以：

sqrt(x^2 + y^2 + z^2) = sqrt((s1 s2 c3 +c1 c2 s3)^2+(s1 c2 c3 + c1 s2 s3)^2+(c1 s2 c3 - s1 c2 s3)^2)

这里：

c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)

Euler to Matrix

$R = \begin{bmatrix} ch*ca & -ch*sa*cb + sh*sb & ch*sa*sb + sh*cb \\ sa & ca*cb & -ca*sb \\ -sh*ca & sh*sa*cb + ch*sb & -sh*sa*sb + ch*cb \end{bmatrix}$

这里：

sa = sin(attitude)
ca = cos(attitude)
sb = sin(bank)
cb = cos(bank)
sh = sin(heading)
ch = cos(heading)

==注意==：具体的转化公式由欧拉角的顺序有关。

Euler to Quaternion

第一种方法：

w = c1 c2 c3 - s1 s2 s3
x = s1 s2 c3 + c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3

where:

c1 = cos(heading / 2), c2 = cos(attitude / 2), c3 = cos(bank / 2)
s1 = sin(heading / 2), s2 = sin(attitude / 2), s3 = sin(bank / 2)

An alternative form is:

w = Math.sqrt(1.0 + C1 * C2 + C1*C3 - S1 * S2 * S3 + C2*C3) / 2
x = (C2 * S3 + C1 * S3 + S1 * S2 * C3) / (4.0 * w) 
y = (S1 * C2 + S1 * C3 + C1 * S2 * S3) / (4.0 * w)
z = (-S1 * S3 + C1 * S2 * C3 + S2) /(4.0 * w)

where:

C1 = cos(heading), C2 = cos(attitude), C3 = cos(bank)
S1 = sin(heading), S2 = sin(attitude), S3 = sin(bank)

note: in the second form the angles are not divided by 2. I don’t know which of these forms is most stable? However, as William points out the first is better because it requires the same number of trig operations, no square root, no worry about dividing by zero, uses familiar formulae, and is fairly clearly normalised.

Matrix to Others

Matrix to AxisAngle

一个3x3的矩阵有9个元素，包含了一些重复的信息，因此有多种方式来推导出旋转，下面是其中的一种：

angle = acos(( m00 + m11 + m22 - 1)/2)
x = (m21 - m12) / sqrt((m21 - m12)^2+(m02 - m20)^2+(m10 - m01)^2)
y = (m02 - m20) / sqrt((m21 - m12)^2+(m02 - m20)^2+(m10 - m01)^2)
z = (m10 - m01) / sqrt((m21 - m12)^2+(m02 - m20)^2+(m10 - m01)^2)

在角度为0或者180度时，上面的公式可能会出问题，所以必须单独测试这两种情况。在角度为0时，旋转轴可以是任意的，在角度为180时，角度仍然是相关的需要去计算它。

Matrix to Euler

1
2
3

heading = atan2(-m20,m00)
attitude = asin(m10) 
bank = atan2(-m12,m11)

except when m10=1 (north pole), which gives: heading = atan2(m02, m22), bank = 0 and when m10=-1 (south pole), which gives: heading = atan2(m02, m22), bank = 0.

Matrix to Quaternion

对于一个旋转矩阵，同时满足正交阵并且==行列式为1==det(matrix)= +1，则该矩阵能够通过下面的公式转化为四元数：

qw= sqrt(1 + m00 + m11 + m22) / 2
qx = (m21-m12) / (4*qw)
qy = (m02-m20) / (4*qw)
qz = (m10-m01) / (4*qw)

但注意该公式必须满足矩阵为正交阵且行列式为1的条件。例如下面的矩阵表示绕着y轴旋转180度

$R_y = \begin{bmatrix} -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix}$

是正交阵但行列式不为1，此时1 + m00 + m11 + m22 = 0即qw=0，无法适用上面的公式。即使qw的值很小不为0，在做除法时也可能产生很大的误差。

另外一种方法为

qw = sqrt( max( 0, 1 + m00 + m11 + m22 )  )  /  2;
qx = sqrt( max( 0, 1 + m00 - m11 - m22 ) ) / 2;
qy = sqrt( max( 0, 1 - m00 + m11 - m22 ) ) / 2;
qz = sqrt( max( 0, 1 - m00 - m11 + m22 ) ) / 2;
qx = copysign( qx, m21 - m12 )
qy = copysign( qy, m02 - m20 )
qz = copysign( qz, m10 - m01 )

copysign函数将第二个参数的符号赋值给第一个参数的符号。

Quaternion to Others

Quaternion to AxisAngle

angle = 2 * acos(qw)
x = qx / sqrt(1-qw*qw)
y = qy / sqrt(1-qw*qw)
z = qz / sqrt(1-qw*qw)

由于轴角表示在0和180度时有两个奇异点，所以需要特别对待。前面说过基于轴角表示的四元数可表示为

1	q = cos(angle/2) + i ( x * sin(angle/2)) + j (y * sin(angle/2)) + k ( z * sin(angle/2))

在角度为0时，q = 1 + i 0 + j 0 + k 0，即qw = 1此时旋转角为0，旋转轴可以设为任意的单位向量
在角度为180时，q = 0 + i x + j y + k z，即qw = 0，此时angle = 2 * acos(qw) = 2 * 90 degrees = 180 degrees (or -180 degrees)，x = qx, y = qy, z = qz，所以上述公式适用，虽然轴角在180度时会突变，但四元素到轴角的转换在该角度附件是连续的。

Quaternion to Euler

1
2
3

heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
attitude = asin(2*qx*qy + 2*qz*qw) 
bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)

except when qx*qy + qz*qw = 0.5 (north pole)
which gives: heading = 2 * atan2(x,w), bank = 0
and when qx*qy + qz*qw = -0.5 (south pole)
which gives: heading = -2 * atan2(x,w), bank = 0

Quaternion to Matrix

如果一个四元素是qw + i qx + j qy + k qz，则对应的表示同样旋转的矩阵为：

$\begin{bmatrix} 1 - 2*qy^2 - 2*qz^2 & 2*qx*qy - 2*qz*qw & 2*qx*qz + 2*qy*qw \\ 2*qx*qy + 2*qz*qw & 1 - 2*qx^2 - 2*qz^2 & 2*qy*qz - 2*qx*qw \\ 2*qx*qz - 2*qy*qw & 2*qy*qz + 2*qx*qw & 1 - 2*qx^2 - 2*qy^2 \end{bmatrix}$

Rotation in Eigen

Eigen 库的 Geometry 模块提供了一个几何变换。

Eigen::AngleAxis

AngleAxis<float> aa(angle_in_radian, Vector3f(ax,ay,az)); // axis vector must be normalized

AngleAxisf aa; aa = Matrix3f(..);  // assumes a pure rotation matrix, use m.isUnitary()
Matrix3f   m;  m  = AngleAxisf(..)

AngleAxisf aa;  aa = Quaternionf(..);
Quaternionf q;  q = AngleAxisf(..);

Eigen::Quaternion

Quaternion<float> q(w, x, y, z);

Quaternionf q;  q = AngleAxisf(..);
AngleAxisf aa;  aa = Quaternionf(..);

Quaternionf q;  q  = Matrix3f(..);
Matrix3f m;     m  = Quaternionf(..);

Eigen::MatrixBase

Vector3f ea = mat.eulerAngles(1, 0, 2); // The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
mat == AngleAxisf(ea[0], Vector3f::UnitY())
     * AngleAxisf(ea[1], Vector3f::UnitX())
     * AngleAxisf(ea[2], Vector3f::UnitZ());

Rotation for Deep Learning

6D

SVD

Refs

Euclidean Space
rotationconverter
Eigen Geometry
SVD分解（奇异值分解）求旋转矩阵
On the Continuity of Rotation Representations in Neural Networks
An Analysis of SVD for Deep Rotation Estimation