从旋转矩阵中提取欧拉角

最新推荐文章于 2025-03-20 17:17:21 发布

shchojj

最新推荐文章于 2025-03-20 17:17:21 发布

阅读量1.5k

点赞数

分类专栏： point cloud rigid registration

原文链接：https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf

版权

rigid registration 同时被 2 个专栏收录

22 篇文章

订阅专栏

point cloud

2 篇文章

订阅专栏

https://www.gregslabaugh.net/publications/euler.pdf

$\theta_1$ 、 $\theta_2$ 、 $\theta_3$ 分别是x、y，z轴。每一个旋转被应用于一个世界轴，而不是一个身体轴（这说的是个啥）， $R_x\left ( \theta_1 \right )R_y\left ( \theta_2 \right )R_z\left ( \theta_3 \right )=\begin{pmatrix} 1 & 0 & 0\\ 0 & c_1& s_1\\ 0 & -s_1& c_1 \end{pmatrix}\begin{pmatrix} c_2 & 0 & -s_2\\ 0 & 1& 0\\ s_2 & 0& c_2 \end{pmatrix}\begin{pmatrix} c_3 & s_3 & 0\\ -s_3 & c_3& 0\\ 0 & 0& 1 \end{pmatrix}=\begin{pmatrix} c_2c_3 & c_2s_3 & -s_2\\ s_1s_2c_3-c_1s_3 & s_1s_2s_3+c_1c_3& s_1c_2\\ c_1s_2c_3+s_1s_3 & c_1s_2s_3-s_1c_3& c_1c_2 \end{pmatrix}$

这里面 $c_1 = cos \theta_1$ 、 $s_1 = sin \theta_1$

假设我们有一个矩阵 $M=\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10}& m_{11} & m_{12}\\ m_{20}& m_{21} & m_{22} \end{pmatrix}$ 我们要从旋转序列中提取旋转角度 $\theta_1$ 、 $\theta_2$ 、 $\theta_3$ 。

Shoemake的代码直接求解

$\theta_1 = atan2\left ( m_{12},m_{22} \right ) =atan2\left ( s_1c_2,c_1c_2 \right )$

$c_2 = \sqrt{m_{00}^2 + m_{01}^2} =\sqrt{c_2^2c_3^2+c_2^2s_3^2}$

$\theta_2 = atan2\left ( -m_{02},c_{2} \right )$

$\theta_3 = atan2\left ( m_{01},m_{00} \right )=atan2\left ( c_2s_3,c_2c_3 \right )$

如果 $m_{00},m_{01}$ 非常小甚至是0，那问题就来了，这样 $m_{12},m_{22}$ 也会变得很小，也许也是0（ $m_{02}$ 不就趋于 $\pm 1$ ），那 $\theta_1$ 不就不好提取了么。

Shoemake根据一个很小的阈值来测试计算的 c_2 值——如果它低于这个阈值，那么矩阵元素大约减少到以下形式:

$\begin{pmatrix} 0 & 0& -\left ( \pm 1 \right ) \\ \pm s_1c_3 - c_1s_3 &\pms_1s_3+c_1c_3 &0 \\ \pm c_1c_3+s_1s_3 & \pm c_1c_3-s_1s_3 & 0 \end{pmatrix}$

在这种情况下，角度 $\theta_1$ 、 $\theta_3$ 被一个不同的编码路径提取。这个矩阵提供了一个gimbal lock现象的例子，在这个例子中，第1和第3轴通过第2轴旋转对齐，有效地失去了一个自由度，因为现在 $\theta_1$ 、 $\theta_3$ 的组合作为一个单一参数。Shoemake通过将其强制 $\theta_3$ 为0来处理这种情况-这是可以的，因为gimbal lock允许我们任意地设置一个 $\theta_1$ 和一个 $\theta_3$ 然后推出另一个。这进一步将矩阵简化为以下形式:

$\begin{pmatrix} 0 & 0& -\left ( \pm 1 \right ) \\ \pm s_1 &c_1 &0 \\ \pm c_1 & \pm -s_1 & 0 \end{pmatrix}$

$\theta_1 = atan2\left ( -m_{21},m_{11} \right )$

${M}' =\left ( \begin{pmatrix} 1 & 0& 0\\ 0 & c_1& s_1\\ 0 & -s_1& c_1 \end{pmatrix}\begin{pmatrix} c_2 & 0& -s_2\\ 0 & 1& 0\\ s_1 & 0& c_2 \end{pmatrix}\right )^T\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10} & m_{11} & m_{12}\\ m_{20} & m_{21} & m_{22} \end{pmatrix}=\begin{pmatrix} c_2 & s_1s_2 & c_1s_2\\ 0 & c_1 & -s_1\\ -s_2 & s_1c_2 & c_1c_2 \end{pmatrix}\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10} & m_{11} & m_{12}\\ m_{20} & m_{21} & m_{22} \end{pmatrix}$