SLAM 之四元数转欧拉角再理解

   我们知道在SLAM中，经常会用到旋转，平移等坐标变换，平移的话，相对来说比较好理解，但是旋转就会比较复杂，通常旋转可以有很多种表示方法，像是旋转矩阵，四元数，欧拉角，欧拉轴角，李代数(SO3)等，但是万变不离其宗，都是为了表达两个坐标系之间的相对角度关系。
   最近重新回顾欧拉角这些基础知识的时候有了一些新的体会，这篇博文主要是为了记录一下最近的总结。
   首先，参考了网上很多博客和资源，以前在脑子里记得的欧拉角都是Z-Y-X这种常用的欧拉角转换，但是都知道欧拉角可以有12种组合，就是不知道到底是怎么个意思，所以这里记录一下。
   以下内容仅自己总结，如需转载请著名出处，如有错误，欢迎指出，谢谢！若文中缺少了对相关文章或链接的引用，请指出，我会添加上去的，尊重原创！

欧拉角定义

   总结之前还是要回顾一下，欧拉角的定义，欧拉角定义分为两种，一种是静态欧拉角，也称外旋/固定轴，另一种是动态欧拉角，也称内旋/动态轴，
  参考weiki百科的内容，静态和动态可以从下面两幅图来理解

静态欧拉角，每次都绕着固定参考系的某一个轴转动

动态欧拉角，每次都绕着自身参考系的某一个轴转动

总之一句话，静态欧拉角是绕着固定坐标系旋转的角度，动态欧拉角是绕着刚体自身坐标系的旋转，且内旋是右乘，外旋是左乘变换
在SLAM中其实经常用的是内旋的形式，所以下面的内容也是按照内旋来计算说明的。

四元数到旋转矩阵

   都知道四元数是可以转换为旋转矩阵的，但实际上怎么转换的呢，网上也有很多直接给公式的，但是只要花点时间去推，很快就能推出来的：

假设有点p1和p2，满足对应的旋转关系， p 1 = R ∗ p 2 p1 = R*p2 p1=R∗p2，其中 R R R表示旋转矩阵
R = [ r 00 r 01 r 02 r 10 r 11 r 12 r 20 r 21 r 22 ] R=\left[\begin{matrix} r00 & r01 & r02 \\ r10 & r11 & r12 \\ r20 & r21 & r22\end{matrix} \right] R=⎣⎡​r00r10r20​r01r11r21​r02r12r22​⎦⎤​
而对四元数 q = [ q w , q x , q y , q z ] T = [ q w , q v ] \bold q = [qw,qx,qy,qz]^T = [qw,\bold {qv}] q=[qw,qx,qy,qz]T=[qw,qv]而言，有
[ 0 p 1 x p 1 y p 1 z ] = q ∗ [ 0 p 2 x p 2 y p 2 z ] ∗ q ∗ \left[\begin{matrix}0\\ p1_x \\ p1_y \\ p1_z\end{matrix}\right] = \bold q *\left[\begin{matrix}0\\ p2_x \\ p2_y \\ p2_z\end{matrix}\right]*\bold q^* ⎣⎢⎢⎡​0p1x​p1y​p1z​​⎦⎥⎥⎤​=q∗⎣⎢⎢⎡​0p2x​p2y​p2z​​⎦⎥⎥⎤​∗q∗
其中 q ∗ \bold q^* q∗表示共扼四元数 q ∗ = [ q 0 , − q x , − q y , − q z ] T \bold q^* = [q0,-qx,-qy,-qz]^T q∗=[q0,−qx,−qy,−qz]T。
另外，四元数的乘法也可以写成矩阵的形式，上面的内容可以转换为：
[ 0 p 1 x p 1 y p 1 z ] = [ q ] L ∗ [ q ∗ ] R [ 0 p 2 x p 2 y p 2 z ] 其 中 ， [ q ] L = q w I + [ 0 − q v T q v [ q v ] × ] , 表 示 左 乘 四 元 数 q 然 后 ， [ q ∗ ] R = q w I + [ 0 − q ∗ v T q ∗ v − [ q ∗ v ] × ] , 表 示 乘 右 乘 四 元 数 q ∗ [ q v ] × = [ 0 − q z q y q z 0 − q x − q y q x 0 ] 表 示 反 对 称 矩 阵 \left[\begin{matrix}0\\ p1_x \\ p1_y \\ p1_z\end{matrix}\right] = \bold [\bold q]_L *[\bold q^*]_R\left[\begin{matrix}0\\ p2_x \\ p2_y \\ p2_z\end{matrix}\right] \\ 其中，[\bold q]_L = qw\bold I + \left[\begin{matrix}0 & -\bold {qv}^T \\ \bold {qv} & [\bold {qv}]_\times\end{matrix}\right],表示左乘四元数\bold q \\ 然后，[\bold q^*]_R = qw\bold I + \left[\begin{matrix}0 & -\bold {q^*v}^T \\ \bold {q^*v} & -[\bold {q^*v}]_\times\end{matrix}\right],表示乘右乘四元数\bold q^* \\ [\bold {qv}]_\times=\left[\begin{matrix} 0 & -qz & qy \\ qz & 0 & -qx \\ -qy & qx & 0 \end{matrix}\right]表示反对称矩阵 ⎣⎢⎢⎡​0p1x​p1y​p1z​​⎦⎥⎥⎤​=[q]L​∗[q∗]R​⎣⎢⎢⎡​0p2x​p2y​p2z​​⎦⎥⎥⎤​其中，[q]L​=qwI+[0qv​−qvT[qv]×​​],表示左乘四元数q然后，[q∗]R​=qwI+[0q∗v​−q∗vT−[q∗v]×​​],表示乘右乘四元数q∗[qv]×​=⎣⎡​0qz−qy​−qz0qx​qy−qx0​⎦⎤​表示反对称矩阵
所以，这就可以计算出对应的转换关系：
[ q ] L ∗ [ q ∗ ] R = q w 2 ∗ I + [ 0 0 0 2 ∗ q w ∗ [ q v ] × ] + [ q v T ∗ q v − q v T ∗ [ q v ] × − [ q v ] × ∗ q v q v ∗ q v T + [ q v ] × ∗ [ q v ] × ] 又 因 为 ， 对 向 量 而 言 ， [ a ] × ∗ b = a × b ， 所 以 [ q v ] × ∗ q v = q v × q v = 0 T 则 [ q ] L ∗ [ q ∗ ] R = [ ∣ ∣ q ∣ ∣ 2 0 T 0 q v ∗ q v T + q w 2 I + 2 ∗ q w ∗ [ q v ] × + [ q v ] × ∗ [ q v ] × ] \bold [\bold q]_L *[\bold q^*]_R= qw^2*\bold I + \left[\begin{matrix} 0 & 0 \\ 0 & 2*qw* [\bold {qv}]_\times \end{matrix}\right] + \left[\begin{matrix} \bold {qv}^T*\bold {qv} & -\bold {qv}^T*[\bold {qv}]_\times \\ -[\bold {qv}]_\times*\bold {qv} & \bold {qv}*\bold {qv}^T + [\bold {qv}]_\times*[\bold {qv}]_\times \end{matrix}\right] \\又因为，对向量而言，[\bold {a}]_\times*\bold {b} = \bold a \times\bold b，\\所以[\bold {qv}]_\times*\bold {qv} = \bold {qv} \times\bold {qv} = \bold 0^T \\则 \\\bold [\bold q]_L *[\bold q^*]_R=\left[\begin{matrix} ||\bold {q}||^2 & \bold {0}^T \\ \bold {0} & \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times\end{matrix}\right] [q]L​∗[q∗]R​=qw2∗I+[00​02∗qw∗[qv]×​​]+[qvT∗qv−[qv]×​∗qv​−qvT∗[qv]×​qv∗qvT+[qv]×​∗[qv]×​​]又因为，对向量而言，[a]×​∗b=a×b，所以[qv]×​∗qv=qv×qv=0T则[q]L​∗[q∗]R​=[∣∣q∣∣20​0Tqv∗qvT+qw2I+2∗qw∗[qv]×​+[qv]×​∗[qv]×​​]
所以，综上所述，可以得到:
[ 0 p 1 ] = [ ∣ ∣ q ∣ ∣ 2 0 T 0 q v ∗ q v T + q w 2 I + 2 ∗ q w ∗ [ q v ] × + [ q v ] × ∗ [ q v ] × ] ∗ [ 0 p 2 ] 即 : p 1 = ( q v ∗ q v T + q w 2 I + 2 ∗ q w ∗ [ q v ] × + [ q v ] × ∗ [ q v ] × ) ∗ p 2 = [ q x 2 + q w 2 − q y 2 − q z 2 2 ∗ q x ∗ q y − 2 q w ∗ q z 2 ∗ q x ∗ q z + 2 ∗ q w ∗ q y 2 ∗ q x ∗ q y + 2 ∗ q w ∗ q z q w 2 + q y 2 − q x 2 − q z 2 2 ∗ q y ∗ q z − 2 ∗ q w ∗ q x 2 ∗ q x ∗ q z − 2 ∗ q w ∗ q y 2 ∗ q y ∗ q z + 2 ∗ q w ∗ q x q w 2 + q z 2 − q x 2 − q y 2 ] \left[\begin{matrix}0\\ \bold p1 \end{matrix}\right] = \left[\begin{matrix} ||\bold {q}||^2 & \bold {0}^T \\ \bold {0} & \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times\end{matrix}\right]*\left[\begin{matrix}0\\ \bold p2 \end{matrix}\right] \\ 即: \bold p1 = ( \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times)*\bold p2 \\ =\left[\begin{matrix} qx^2 + qw^2-qy^2 - qz^2 & 2*qx*qy-2qw*qz & 2*qx*qz + 2*qw*qy \\ 2*qx*qy+2*qw*qz & qw^2+qy^2-qx^2-qz^2 & 2*qy*qz - 2*qw*qx \\ 2*qx*qz-2*qw*qy & 2*qy*qz+2*qw*qx & qw^2+qz^2-qx^2-qy^2\end{matrix}\right] [0p1​]=[∣∣q∣∣20​0Tqv∗qvT+qw2I+2∗qw∗[qv]×​+[qv]×​∗[qv]×​​]∗[0p2​]即:p1=(qv∗qvT+qw2I+2∗qw∗[qv]×​+[qv]×​∗[qv]×​)∗p2=⎣⎡​qx2+qw2−qy2−qz22∗qx∗qy+2∗qw∗qz2∗qx∗qz−2∗qw∗qy​2∗qx∗qy−2qw∗qzqw2+qy2−qx2−qz22∗qy∗qz+2∗qw∗qx​2∗qx∗qz+2∗qw∗qy2∗qy∗qz−2∗qw∗qxqw2+qz2−qx2−qy2​⎦⎤​
完成了上面的推导之后，就可以根据四元数转换到旋转矩阵了。

旋转矩阵到欧拉角

讲解完了上面的内容，接下来就是怎么根据旋转矩阵转换出欧拉角了，正如前面所言，SLAM中常用的其实是内旋的形式，正如我们对旋转矩阵求偏导的时候会采用右扰动的方式一样，也就是说，假如相机/Lidar坐标系经过三次旋转之后，其方位(不考虑平移)能够转到和世界坐标系重合，这三次旋转分别为 R z ( α ) , R y ( β ) , R x ( γ ) Rz(\alpha),Ry(\beta),Rx(\gamma) Rz(α),Ry(β),Rx(γ)，则旋转矩阵表示的形式为：
R = [ c o s ( α ) − s i n ( α ) 0 s i n ( α ) c o s ( α ) 0 0 0 1 ] ∗ [ c o s ( β ) 0 s i n ( β ) 0 1 0 − s i n ( β ) 0 c o s ( β ) ] ∗ [ 1 0 0 0 c o s ( γ ) − s i n ( γ ) 0 s i n ( γ ) c o s ( γ ) ] = [ c o s ( α ) c o s ( β ) − s i n ( α ) c o s ( γ ) + c o s ( α ) s i n ( β ) s i n ( γ ) s i n ( α ) s i n ( γ ) + c o s ( α ) s i n ( β ) c o s ( γ ) s i n ( α ) c o s ( β ) c o s ( α ) c o s ( γ ) + s i n ( α ) s i n ( β ) s i n ( γ ) − c o s ( α ) s i n ( γ ) + s i n ( α ) s i n ( β ) c o s ( γ ) − s i n ( β ) c o s ( β ) s i n ( γ ) c o s ( β ) c o s ( γ ) ] = R = [ r 00 r 01 r 02 r 10 r 11 r 12 r 20 r 21 r 22 ] R = \left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right]* \left[\begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \end{matrix}\right]* \left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos(\gamma) & -sin(\gamma)\\ 0 & sin(\gamma) & cos(\gamma) \end{matrix}\right] \\ = \left[\begin{matrix} cos(\alpha)cos(\beta) & -sin(\alpha)cos(\gamma)+cos(\alpha)sin(\beta)sin(\gamma) & sin(\alpha)sin(\gamma)+cos(\alpha)sin(\beta)cos(\gamma) \\ sin(\alpha)cos(\beta) & cos(\alpha)cos(\gamma)+sin(\alpha)sin(\beta)sin(\gamma) & -cos(\alpha)sin(\gamma) + sin(\alpha)sin(\beta)cos(\gamma)\\ -sin(\beta) & cos(\beta)sin(\gamma) & cos(\beta)cos(\gamma)\end{matrix}\right] \\=R=\left[\begin{matrix} r00 & r01 & r02 \\ r10 & r11 & r12 \\ r20 & r21 & r22\end{matrix} \right] R=⎣⎡​cos(α)sin(α)0​−sin(α)cos(α)0​001​⎦⎤​∗⎣⎡​cos(β)0−sin(β)​010​sin(β)0cos(β)​⎦⎤​∗⎣⎡​100​0cos(γ)sin(γ)​0−sin(γ)cos(γ)​⎦⎤​=⎣⎡​cos(α)cos(β)sin(α)cos(β)−sin(β)​−sin(α)cos(γ)+cos(α)sin(β)sin(γ)cos(α)cos(γ)+sin(α)sin(β)sin(γ)cos(β)sin(γ)​sin(α)sin(γ)+cos(α)sin(β)cos(γ)−cos(α)sin(γ)+sin(α)sin(β)cos(γ)cos(β)cos(γ)​⎦⎤​=R=⎣⎡​r00r10r20​r01r11r21​r02r12r22​⎦⎤​
由此可得到，在内旋，Z-Y-X的旋转顺序下对应欧拉角所构成的旋转矩阵是上面的这个样子，要想反解出对应的角度，只要对矩阵做一定的操作即可：
s y = s q r t ( r 21 ∗ r 21 + r 22 ∗ r 22 ) γ = a t a n 2 ( r 21 , r 22 ) β = a t a n 2 ( − r 20 , s y ) α = a t a n 2 ( r 10 , r 00 ) sy=sqrt(r21*r21+r22*r22) \\ \gamma = atan2(r21,r22) \\ \beta = atan2(-r20,sy) \\ \alpha = atan2(r10,r00) sy=sqrt(r21∗r21+r22∗r22)γ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)
这样，就可以得到三轴的欧拉角为 ( γ , β , α ) (\gamma,\beta,\alpha) (γ,β,α)
但是，上面的计算是不考虑了Gimbol Lock(万向锁)的情况的，具体Gimbol Lock是啥这里不做详细的解释，一句话概括，就是假如第二次旋转的角度为+90度或-90度的时候，会出现绕第三个轴旋转和绕第一个轴旋转的效果是一样的情况，也就是少了一个*度，这个大家可以用自己的右手做一个测试即可，设食指朝前是x轴，中指朝左是y轴，大拇指朝上是z轴，当先绕z轴旋转一定水平角度 α \alpha α之后，再绕此时的中指旋转90度，会惊奇的发现，食指和原来的大拇指方向重合了，这个时候，再去绕食指转动 γ \gamma γ，实际上相当于一开始就绕z轴旋转 α + γ \alpha + \gamma α+γ角度，然后再绕此时的中指旋转90度，所以说是少了一个*度。
从矩阵上看，还是以Z-Y-X的旋转顺序，就是第二角度 β = 90 \beta = 90 β=90，那么 cos ⁡ β = 0 , s i n β = 1 \cos\beta = 0,sin\beta = 1 cosβ=0,sinβ=1，这个时候，再去反解算欧拉角的时候，就会出现0/0的情况，这是明显不对的，所以需要再做个判断，
由于在计算机中，都是浮点数，所以基本上不会完全等于0，一般认为 a < ϵ a<\epsilon a<ϵ的时候就认为是0了，所以假如前面的sy < 1e-6之类的，就可以认为是遇到了万向锁了，这个时候，其实对角度解算就没有太大意义了，因为有两个轴 α , γ \alpha,\gamma α,γ在同一个方向上了，可以假定其中一个为0，然后再带回去算就好了，完整的判断如下：
s y = s q r t ( r 21 ∗ r 21 + r 22 ∗ r 22 ) i f s y < 1 e − 6 : γ = a t a n 2 ( r 01 , r 02 ) β = a t a n 2 ( − r 20 , s y ) α = 0.0 e s l e γ = a t a n 2 ( r 21 , r 22 ) β = a t a n 2 ( − r 20 , s y ) α = a t a n 2 ( r 10 , r 00 ) sy=sqrt(r21*r21+r22*r22) \\ if \quad sy <1e-6: \\ \quad \quad \quad \gamma = atan2(r01,r02) \\ \quad \quad \quad \beta = atan2(-r20,sy) \\ \quad \quad \quad \alpha = 0.0 \\esle \quad \quad \quad\quad \quad \quad \\ \quad \quad \quad \gamma = atan2(r21,r22) \\ \quad \quad \quad \beta = atan2(-r20,sy) \\ \quad \quad \quad \alpha = atan2(r10,r00) sy=sqrt(r21∗r21+r22∗r22)ifsy<1e−6:γ=atan2(r01,r02)β=atan2(−r20,sy)α=0.0esleγ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)
最后得到的结果就是上面所述的情况了，至于其它的旋转顺序，同样可以这么分解，先通过内旋的形式把对应欧拉角的旋转矩阵乘起来，然后再和四元数推出来的旋转矩阵对比，注意一下Gimbol Lock的情况，就可以解出来了，大家可以用在线转换工具测试一下

需要注意的是：以上解法只适用于内旋的反解，目前好像大多的库比如Eigen,Ros tf 都是这么解的，根据旋转矩阵解外旋欧拉角还不清楚有哪些，但是外旋和内旋满足：
( R i z ( α ) ∗ R i y ( β ) ∗ R i x ( γ ) ) − 1 = ( R e x ( γ ) ∗ R e y ( β ) ∗ R e z ( α ) ) 其 中 R i 表 示 内 旋 ， R e 表 示 外 旋 ， 且 : R i z ( α ) = [ c o s ( α ) − s i n ( α ) 0 s i n ( α ) c o s ( α ) 0 0 0 1 ] R e z ( α ) = [ c o s ( α ) s i n ( α ) 0 − s i n ( α ) c o s ( α ) 0 0 0 1 ] (Riz(\alpha)*Riy(\beta)*Rix(\gamma))^{-1} =(Rex(\gamma)*Rey(\beta)*Rez(\alpha)) \\其中Ri表示内旋，Re表示外旋，且: \\ Riz(\alpha)=\left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right] \\Rez(\alpha)=\left[\begin{matrix} cos(\alpha) & sin(\alpha) & 0 \\ -sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right] (Riz(α)∗Riy(β)∗Rix(γ))−1=(Rex(γ)∗Rey(β)∗Rez(α))其中Ri表示内旋，Re表示外旋，且:Riz(α)=⎣⎡​cos(α)sin(α)0​−sin(α)cos(α)0​001​⎦⎤​Rez(α)=⎣⎡​cos(α)−sin(α)0​sin(α)cos(α)0​001​⎦⎤​

参考

特别感谢以下参考，其实还看了很多，记不全了，如有遗漏，也请指出！
<< Quaternion kinematics for the error-state Kalman filter >>
https://en.wikipedia.org/wiki/Euler_angles
https://blog.csdn.net/qq_38288618/article/details/77195271
https://quaternions.online