三维旋转沿轴分解

将旋转分解为固定参考坐标系的绕轴$\mathbf{n}$的旋转和正交轴旋转：
$$\mathbf{R}={\mathbf{R}}_{\perp }{\mathbf{R}}_{\parallel }$$
$${\mathbf{R}}_{\parallel }=\cos \theta \mathbf{I}+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }$$
使用最小二乘法估计绕轴旋转，目标函数是使正交轴旋转的角度最小
$$\underset{\theta }{\min }\arccos \left(\frac{\mathrm{tr}\left({\mathbf{RR}}_{\parallel }^{-1}\right)-1}{2}\right)$$

由于$\arccos$函数单调递减，原问题变为

$$\begin{array}{rl}\underset{\theta }{\max }\mathrm{tr}\left({\mathbf{RR}}_{\parallel }^{-1}\right)& =\underset{\theta }{\max }\mathrm{tr}\left(\mathbf{R}\left(\cos \theta \mathbf{I}+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^{\mathrm{T}}-\sin \theta {\mathbf{n}}^{\wedge }\right)\right)\\ & =\underset{\theta }{\max }\left(\cos \theta \mathrm{tr}\left(\mathbf{R}-\mathbf{Rn}{\mathbf{n}}^{\mathrm{T}}\right)-\sin \theta \mathrm{tr}\left(\mathbf{R}{\mathbf{n}}^{\wedge }\right)\right)\end{array}$$
对$\theta$求导得
$$\begin{array}{rl}-\sin \theta \mathrm{tr}\left(\mathbf{R}-\mathbf{Rn}{\mathbf{n}}^{\mathrm{T}}\right)-\cos \theta \mathrm{tr}\left(\mathbf{R}{\mathbf{n}}^{\wedge }\right)& =0\\ \tan \theta & =\frac{\mathrm{tr}\left(\mathbf{R}{\mathbf{n}}^{\wedge }\right)}{\mathrm{tr}\left({\mathbf{n}}^{\mathrm{T}}\mathbf{Rn}\right)-\mathrm{tr}\left(\mathbf{R}\right)}\\ \theta & =\arctan \left(\frac{\mathrm{tr}\left(\mathbf{R}{\mathbf{n}}^{\wedge }\right)}{\mathrm{tr}\left({\mathbf{n}}^{\mathrm{T}}\mathbf{Rn}\right)-\mathrm{tr}\left(\mathbf{R}\right)}\right)\end{array}$$