三维旋转沿轴分解

将旋转分解为固定参考坐标系的绕轴n\bold{n}n的旋转和正交轴旋转：
R=R⊥R∥ \bold{R}= \bold{R}{\perp} \bold{R}{\parallel} R=R⊥R∥
R∥=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧ \bold{R}_{\parallel}=\cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}+\sin \theta \bold{n}^{\wedge } R∥=cosθI+(1−cosθ)nnT+sinθn∧

使用最小二乘法估计绕轴旋转，目标函数是使正交轴旋转的角度最小
min⁡θarccos⁡(tr(RR∥−1)−12) \min_{\theta}\arccos\left(\dfrac{\rm{tr}\left(\bold{RR}_{\parallel}^{-1}\right)-1}{2}\right) θminarccos 2tr(RR∥−1)−1

上面的式子使用了旋转矩阵转欧拉角 的公式

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

max⁡θtr(RR∥−1)=max⁡θtr(R(cos⁡θI+(1−cos⁡θ)nnT−sin⁡θn∧))=max⁡θ(cos⁡θtr(R−RnnT)−sin⁡θtr(Rn∧)) \begin{aligned} \max_{\theta} \rm{tr}\left(\bold{RR}{\parallel}^{-1}\right) &=\max{\theta}\rm{tr}\left(\bold{R}\left(\cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}-\sin \theta \bold{n}^{\wedge } \right)\right)\\ &=\max_{\theta}\left(\cos \theta \rm{tr}\left(\bold{R} - \bold{R}\bold{n}\bold{n}^{T}\right)-\sin\theta\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)\right) \end{aligned} θmaxtr(RR∥−1)=θmaxtr(R(cosθI+(1−cosθ)nnT−sinθn∧))=θmax(cosθtr(R−RnnT)−sinθtr(Rn∧))

对θ\thetaθ求导得
−sin⁡θtr(R−RnnT)−cos⁡θtr(Rn∧)=0tan⁡θ=tr(Rn∧)tr(nTRn)−tr(R)θ=arctan⁡(tr(Rn∧)tr(nTRn)−tr(R)) \begin{aligned} -\sin\theta\rm{tr}\left(\bold{R} - \bold{R}\bold{n}\bold{n}^{T}\right)-\cos\theta\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)&=0\\ \tan\theta&=\dfrac{\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)}{\rm{tr}\left(\bold{n}^{T}\bold{R}\bold{n}\right)-\rm{tr}\left(\bold{R}\right) }\\ \theta&=\arctan\left(\dfrac{\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)}{\rm{tr}\left(\bold{n}^{T}\bold{R}\bold{n}\right)-\rm{tr}\left(\bold{R}\right) }\right) \end{aligned} −sinθtr(R−RnnT)−cosθtr(Rn∧)tanθθ=0=tr(nTRn)−tr(R)tr(Rn∧)=arctan(tr(nTRn)−tr(R)tr(Rn∧))