旋转矩阵、转换矩阵、欧拉角和四元数的关系及Eigen和g2o中的变换

最新推荐文章于 2024-04-22 20:45:04 发布

原创最新推荐文章于 2024-04-22 20:45:04 发布 · 1.6k 阅读

8 ·

CC 4.0 BY-SA版权

学习笔记专栏收录该内容

20 篇文章

订阅专栏

本文详细阐述了旋转矩阵R、转换矩阵T、欧拉角、四元数之间的数学关系及在g2o和Eigen库中的具体应用。通过解析旋转矩阵与欧拉角、四元数的转换公式，介绍了SE3Quat类型在g2o中的作用，以及Eigen库中旋转矩阵、旋转向量、四元数和平移向量的表示方法。

旋转矩阵R,转换矩阵T,欧拉角：偏航角yaw记为 $\theta _y$ ，俯仰角pitch记为 $\theta_x$ ,偏转角roll记为 $\theta_z$ ,四元数 $q=q_0+q_1i+q_2j+q_3k$

1.旋转矩阵R与转换矩阵T的关系如下：

$T=\begin{bmatrix} R&t \\ 0^T& 1 \end{bmatrix}$ (1)

2.四元数q与旋转矩阵R的关系如下：

$R=\begin{bmatrix} 1-2q_2^2-2q_3^2 &2q_1q_2+2q_0q_3 &2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3&1-2q_1^2-2q_3^3 &2q_2q_3+2q_0q1 \\ 2q_1q_3+2q_0q_2&2q_2q_3-2q_0q_1 &1-2q_1^2-2q_2^2 \end{bmatrix}$ (2)

$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{r_{23}-r_{32}}{4q_0},q_2=\frac{r_{31}-r_{13}}{4q_0},q_3=\frac{r_{12}-r_{21}}{4q_0}.$ (3) 其中 $R=\begin{Bmatrix} {r_{ij} \end{Bmatrix}, i,j\in[1,2,3]$ .

3.旋转矩阵R与欧拉角 $\theta_x,\theta_y,\theta_z$ 的关系如下：

$R(\theta_x,\theta_y,\theta_z)=\begin{bmatrix} c_yc_z&c_zs_xs_y-c_xs_z &s_xs_z+c_xc_zs_y \\ c_ys_z&c_xc_z +s_xs_ys_z&c_xs_ys_z-c_zs_z \\ -s_y&c_ys_x &c_xc_y \end{bmatrix}$ (4). 其中sx,cx,sy,cy,sz,cz分别为 $\theta_x,\theta_y,\theta_z$ 的正余弦值。

$\theta_x=atan2(r_{32},r_{33}),\theta_y=atan2(-r_{31},\sqrt{r_{32}^2+r_{33}^2}),\theta_z=atan2(r_{21},r_{11})$ (5)

g2o中使用SE3Quat类型表示变换T: g2o::SE3Quat(R,t), g2o::SE3Quat &T;

Eigen库中各种形式的表示如下：
旋转矩阵（3x3）:Eigen::Matrix3d
旋转向量（3x1）:Eigen::AngleAxisd
四元数（4x1）:Eigen::Quaterniond
平移向量（3x1）:Eigen::Vector3d
转换矩阵（4x4）:Eigen::Isometry3d