vSLAM学习笔记——四元数到其他旋转表示的转换
作者：互联网
文章目录
四元数表示
q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3k q=q0+q1i+q2j+q3k
四元数到旋转矩阵
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R=vv^T+s^2I+2sv^{\wedge}+(v^\wedge)^2 R=vvT+s2I+2sv∧+(v∧)2
四元数到旋转向量
对上式两侧求迹，得：
t
r
(
R
)
=
t
r
(
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
)
=
v
1
2
+
v
2
2
+
v
3
2
+
3
s
2
−
2
(
v
1
2
+
v
2
2
+
v
3
2
)
=
(
1
−
s
2
)
+
3
s
2
−
2
(
1
−
s
2
)
=
4
s
2
−
1
\begin{aligned} tr(R) &= tr(vv^T+s^2I+2sv^{\wedge}+(v^\wedge)^2) \\ &=v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2) \\ &=(1-s^2)+3s^2-2(1-s^2) \\ &=4s^2-1 \\ \end{aligned}
tr(R)=tr(vvT+s2I+2sv∧+(v∧)2)=v12+v22+v32+3s2−2(v12+v22+v32)=(1−s2)+3s2−2(1−s2)=4s2−1
由罗德里格斯公式可以推出：
θ
=
arccos
t
r
(
R
−
1
)
2
=
arccos
(
2
s
2
−
1
)
\begin{aligned} \theta&=\arccos\frac{tr(R-1)}{2}\\ &=\arccos(2s^2-1) \end{aligned}
θ=arccos2tr(R−1)=arccos(2s2−1)
即
cos
θ
=
2
s
2
−
1
=
2
cos
2
θ
2
−
1
,
\cos\theta=2s^2-1=2\cos^2\frac{\theta}{2}-1,
cosθ=2s2−1=2cos22θ−1,
所以：
θ
=
2
arccos
s
\theta=2\arccos s
θ=2arccoss
至于旋转轴，四元数的虚部组成的向量在旋转时是不动的，因此只要将其除掉模长即可。
从而言之四元数到旋转向量的转换公式如下：
{
θ
=
2
arccos
q
0
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
sin
θ
2
\begin{cases} \theta=2\arccos q_0 \\ [n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/\sin \frac{\theta}{2} \\ \end{cases}
{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
标签：数到,tr,cos,四元,arccos,theta,vSLAM 来源： https://blog.csdn.net/weixin_39739042/article/details/122128400