旋转矩阵2四元数

最新推荐文章于 2024-04-16 20:01:24 发布

xiaoooli

最新推荐文章于 2024-04-16 20:01:24 发布

阅读量268

点赞数

分类专栏：个人笔记

本文链接：https://blog.csdn.net/qq_40230900/article/details/112726271

版权

个人笔记专栏收录该内容

20 篇文章 0 订阅

订阅专栏

四元数转旋转矩阵

$C_{b}^{a} = \begin{bmatrix} q_{1}^{2} + q_{2}^{2} - q_{3}^{2} - q_{4}^{2} & 2(q_{2}q_{3} - q_{1}q_{4}) & 2(q_{2}q_{4} + q_{1}q_{3}) \\ 2(q_{2}q_{3} + q_{1}q_{4}) & q_{1}^{2} - q_{2}^{2} + q_{3}^{2} - q_{4}^{2} & 2(q_{3}q_{4} - q_{1}q_{2}) \\ 2(q_{2}q_{4} - q_{1}q_{3}) & 2(q_{3}q_{4} + q_{1}q_{2}) & q_{1}^{2} - q_{2}^{2} - q_{3}^{2} + q_{4}^{2} \\ \end{bmatrix}$

旋转矩阵转四元数

$P_{1}=1+ trace(C_{b}^{a}), P_{2}=1+2c_{11} - trace(C_{b}^{a}) \\ P_{3}=1 + 2c_{22} - trace(C_{b}^{a}), P_{4}=1+2c_{33} - trace(C_{b}^{a})$
其中 $C_{b}^{a}$ 是trace转矩阵，trace为矩阵迹， $c_{ij}$ 是矩阵 $C_{b}^{a}$ 的元素。

case1: $P_{1}= max(P_{1}, P_{2}, P_{3}, P_{4})$ ，则：
$q_{1} = 0.5\sqrt{P1}\\ q_{2} = \frac{c_{32} - c_{23}}{4q_{1}}\\ q_{3} = \frac{c_{13} - c_{31}}{4q_{1}}\\ q_{4} = \frac{c_{21} - c_{12}}{4q_{1}}$
case2: $P_{2}= max(P_{1}, P_{2}, P_{3}, P_{4})$ ，则：
$q_{2} = 0.5\sqrt{P2}\\ q_{3} = \frac{c_{21} + c_{12}}{4q_{2}}\\ q_{4} = \frac{c_{13} + c_{31}}{4q_{2}}\\ q_{1} = \frac{c_{32} - c_{23}}{4q_{2}}$
case3: $P_{3}= max(P_{1}, P_{2}, P_{3}, P_{4})$ ，则：
$q_{3} = 0.5\sqrt{P3}\\ q_{4} = \frac{c_{32} + c_{23}}{4q_{3}}\\ q_{1} = \frac{c_{13} - c_{31}}{4q_{3}}\\ q_{2} = \frac{c_{21} + c_{12}}{4q_{3}}$
case3: $P_{4}= max(P_{1}, P_{2}, P_{3}, P_{4})$ ，则：
$q_{4} = 0.5\sqrt{P4}\\ q_{1} = \frac{c_{21} - c_{12}}{4q_{4}}\\ q_{2} = \frac{c_{13} + c_{31}}{4q_{4}}\\ q_{3} = \frac{c_{32} + c_{23}}{4q_{4}}$

最后，如果 $q_{1} < 0$ ，则q=-q

xiaoooli

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
旋转矩阵2四元数

P1=1+trace(Cba),P2=1+2c11−trace(Cba)P3=1+2c22−trace(Cba),P4=1+2c33−trace(Cba)P_{1}=1+ trace(C_{b}^{a}), P_{2}=1+2c_{11} - trace(C_{b}^{a}) \\P_{3}=1 + 2c_{22} - trace(C_{b}^{a}), P_{4}=1+2c_{33} - trace(C_{b}^{a}) P1=1+trace(Cba),P2=1+2c11−trace(Cba
复制链接

扫一扫