三维旋转沿轴分解
将旋转分解为固定参考坐标系的绕轴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)
θminarccos2tr(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∧))