《视觉SLAM十四讲》第三章阅读笔记（二）

最新推荐文章于 2024-06-16 21:04:49 发布

半熟芝士味

最新推荐文章于 2024-06-16 21:04:49 发布

阅读量260

点赞数

分类专栏： SLAM # 视觉SLAM十四讲阅读笔记文章标签：自动驾驶算法人工智能 slam

本文链接：https://blog.csdn.net/weixin_47552638/article/details/121468771

版权

SLAM 同时被 2 个专栏收录

33 篇文章 22 订阅

订阅专栏

视觉SLAM十四讲阅读笔记

5 篇文章 0 订阅

订阅专栏

《视觉SLAM十四讲》第三章阅读笔记（二）

阅读本章前建议阅读以下两篇文章，作为理论支撑：
《视觉SLAM十四讲》第三章阅读笔记（一）
《视觉SLAM十四讲》第三章阅读笔记（补充阅读，Eigen介绍）

1、欧拉角

将一个旋转分成绕三个轴旋转的三个角称为欧拉角。分别是：
绕车体 $Z$ 轴旋转，得到的角为偏航角 $y a w$ ；
绕旋转之后的 $Y$ 轴旋转得到的角，为俯仰角 $p i t c h$ ；
绕旋转之后的 $X$ 轴旋转得到的角，为滚转角 $r o l l$ ；
简记为：

类型	定义
偏航角	绕Z轴旋转
俯仰角	绕Y轴旋转
滚转角	绕X轴旋转

其中通常所称的 $r p y$ 角的旋转方式为 $Z Y X$ 。

欧拉角 $\bm{\Leftrightarrow}$ 旋转矩阵

绕Z轴旋转时定义：
$\bm R_Z=\begin{bmatrix} cos\bm y &-sin\bm y &0\\\ sin\bm y &cos\bm y &0\\\ 0&0 &1\end{bmatrix}$
绕Y轴旋转时定义
$\bm R_Y=\begin{bmatrix} cos\bm p &0&sin\bm p \\\ 0&1 &0\\\ -sin\bm p&0 &cos\bm p\end{bmatrix}$
绕X轴旋转时定义
$\bm R_X=\begin{bmatrix}1 &0&0\\\ 0&cos\bm r&sin\bm r \\\ 0&cos\bm r&sin\bm r\end{bmatrix}$
以ZYX的旋转方式为例，可以求解得到旋转的向量为：
$\bm R=\bm R_Z\bm R_Y\bm R_X$
$\bm R=\begin{bmatrix}c\bm yc\bm p &c\bm ys\bm ps\bm r-s\bm yc\bm r&c\bm ys\bm pc\bm r+s\bm ys\bm r\\\ s\bm yc\bm p&s\bm ys\bm ps\bm r+c\bm yc\bm r&s\bm ys\bm pc\bm r-c\bm ys\bm r \\\ -s\bm p&c\bm ps\bm r&c\bm pc\bm r\end{bmatrix}$
反解出欧拉角为：
$\left\{ \begin{array}{lr} \bm r=atan(\frac{\bm R(32)}{\bm R(33)}),& \\ \bm p=arcsin(-\bm R(31))\\ \bm y=arctan(\frac{\bm R(21)}{\bm R(11)}),& \end{array} \right.$
注意其他几种形式
$\bm R_{XYZ}=\begin{bmatrix}c\bm pc\bm y &-c\bm pc\bm y&s\bm p\\\ c\bm rs\bm y+c\bm ys\bm rs\bm p&c\bm rc\bm y- s\bm rs\bm ps\bm y&-c\bm ps\bm r\\\ s\bm rs\bm y-c\bm rs\bm pc\bm y&c\bm ys\bm r+c\bm rs\bm ps\bm y&c\bm pc\bm r\end{bmatrix}$
$\bm R_{XZY}=\begin{bmatrix}c\bm pc\bm y &-s\bm p&c\bm ps\bm y\\\ s\bm rs\bm y+c\bm rc\bm ys\bm p&c\bm rc\bm p&c\bm rs\bm prs\bm y-c\bm ys\bm r\\\ c\bm ys\bm rs\bm p-c\bm rs\bm y&c\bm ps\bm r&c\bm rc\bm y+s\bm rs\bm ps\bm y\end{bmatrix}$
$\bm R_{YXZ}=\begin{bmatrix}c\bm rc\bm y +s\bm rs\bm ps\bm r&c\bm ys\bm ps\bm r- c\bm rs\bm y&c\bm ps\bm r\\\ c\bm ps\bm y&c\bm yc\bm p&-s\bm p\\\ c\bm rs\bm ps\bm y-c\bm ys\bm r&c\bm rc\bm ys\bm p+s\bm rs\bm y&c\bm rc\bm p\end{bmatrix}$
$\bm R_{YZX}=\begin{bmatrix}c\bm rc\bm p &s\bm rs\bm y-c\bm rs\bm y&c\bm ps\bm r\\\ c\bm ps\bm y&c\bm yc\bm p&-s\bm p\\\ c\bm rs\bm ps\bm y-c\bm ys\bm r&c\bm rc\bm ys\bm p+s\bm rs\bm y&c\bm rc\bm p\end{bmatrix}$
$\bm R_{ZYX}=\begin{bmatrix}c\bm rc\bm p &c\bm rs\bm ps\bm y-c\bm ys\bm r&s\bm rs\bm y+c\bm rc\bm ys\bm p\\\ c\bm ps\bm r&c\bm rc\bm y+s\bm rs\bm ps\bm y&c\bm ys\bm rs\bm p-c\bm rs\bm y \\\ -s\bm p&c\bm ps\bm y&c\bm pc\bm y\end{bmatrix}$
$\bm R_{ZXY}=\begin{bmatrix}c\bm rc\bm y-s\bm rs\bm ps\bm y &-c\bm ps\bm r&c\bm rs\bm y+c\bm ys\bm rs\bm p\\\ c\bm ys\bm r+c\bm rs\bm ps\bm y&c\bm rc\bm p&s\bm rs\bm y-c\bm rc\bm ys\bm p \\\ -c\bm ps\bm y&s\bm p&c\bm pc\bm y\end{bmatrix}$
2、四元数
定义一个类似于复数的的代数：四元数（Quaternion），它是紧凑的，也没有奇异性。一个四元数 $\bm q$ 拥有一个实部和三个虚部，同样的也可以感受一下其使用四元数的魅力，即：
$\bm q=q_0+q_1\bm i+q_2\bm j+q_3\bm k$
其中， $\bm i 、\bm j、\bm k$ 为三个四元数的虚部，满足以下关系：
$\left\{ \begin{array}{lr} \bm i^2=\bm j^2=\bm k^2=-1& \\ \bm i\bm j=\bm k,\bm j\bm i=-\bm k\\ \bm j\bm k=\bm i,\bm k\bm j=-\bm i\\ \bm k\bm i=\bm j,\bm i\bm k=-\bm j& \end{array} \right.$
用一个标量和一个向量来表达四元数：
$\bm q=[s,\bm v], s=q_0\in{\mathbb R}，\bm v={[q_1,q_2,q_3]}^{T}\in{\mathbb R}^{3}$
这里的定义和复数的定义类似， $s$ 称为实部， $\bm v$ 称为虚部，如果四元数虚部为0，称之为实四元数，反之如果虚部为0，称之为虚四元数。
该性质是，乘以 $\bm i$ ，意味着绕 $\bm i$ 旋转 $180\degree$ ，对于 $\bm k、\bm j$ 也同样适用。

四元数 $\bm{\Leftrightarrow}$ 旋转向量

假设某个旋转是绕单位向量 $\bm n=[n_x,n_y,n_z]^T$ ,进行了角度为 $\theta$ 的旋转，旋转四元数的形式为：
$\bm q=[cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2}]^T$
同样可以反解出：
$\left\{ \begin{array}{lr} \bm \theta=2arccosq_0& \\ [n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{\theta}{2}& \end{array} \right.$

四元数表示旋转

假设一个空间的三维点 $\bm p=[x,y,z]\in{\mathbb R^3}$ ,使用四元数来表达为：
$\bm p=[0,x,y,z]=[0,\bm v]$
同样地，如果使用旋转向量则为 $\bm n$ 和 $\theta$ ，根据上面公式的推导则有：
$\bm q=[cos\frac{\theta}{2},\bm nsin\frac{\theta}{2}]$
$q$ 的共轭为：
$\bm q^*=[cos\frac{\theta}{2},-\bm nsin\frac{\theta}{2}]$
求逆为：

$\bm q^{-1}=\frac{q^*}{||q||}$
旋转后的点 $p^{'}$ 即可表示为这样的乘积，具体的推导过程可见Understanding Quaternions 中文翻译《理解四元数》和Understanding Quaternions：
$\bm p'=\bm q\bm p\bm q^{-1}$

四元数 $\bm{\Leftrightarrow}$ 旋转矩阵

四元数 $\bm{\Rightarrow}$ 旋转矩阵
方法一
已知四元数 $\bm q=q_0+q_1\bm i+q_2\bm j+q _3\bm k=[s,\bm v]$ ,可以利用利用Rodrigues公式可以由四元数求得旋转矩阵 $R$ :
$\bm 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^2&2q_2q_3-2q_0q_1\\\ 2q_1q_3-2q_0q2&s2q_2q_3+2q_0q_1&1-2q_1^2-2q_ 2^2\end{bmatrix}$
方法二：
$e^{\mathbf{\omega} \theta} =\bm I + \mathbf{\omega}_{\times} sin\theta + \mathbf{\omega}_{\times}^2 (1 - cos\theta) \\ =\bm I + \left[ \begin{matrix} 0 & -z & y\\ z & 0 & -x \\ -y & x & 0 \end{matrix} \right ] sin\theta + \left[ \begin{matrix} -y^2-z^2 & xy & xz\\ xy & -x^2-z^2 & yz \\ xz & yz & -x^2-y^2 \end{matrix} \right ] (1-cos\theta)\\ = \left[ \begin{matrix} 1 & -zsin\theta & ysin\theta\\ zsin\theta & 1 & -xsin\theta \\ -ysin\theta & xsin\theta & 1 \end{matrix} \right ] + \left[ \begin{matrix} -y^2-z^2 & xy & xz\\ xy & -x^2-z^2 & yz \\ xz & yz & -x^2-y^2 \end{matrix} \right ] (1-cos\theta)\\ = \left[ \begin{matrix} 1-(y^2+z^2)(1-cos\theta) & -zsin\theta + xy(1-cos\theta) & ysin\theta + xz(1-cos\theta) \\ zsin\theta + xy(1-cos\theta) & 1 - (x^2+z^2)(1-cos\theta) & -xsin\theta + yz(1-cos\theta) \\ -ysin\theta + xz(1-cos\theta) & xsin\theta + yz(1-cos\theta) & 1-(x^2+y^2)(1-cos\theta) \end{matrix} \right ] \\ = \left[ \begin{matrix} cos\theta+x^2(1-cos\theta) & -zsin\theta + xy(1-cos\theta) & ysin\theta + xz(1-cos\theta) \\ zsin\theta + xy(1-cos\theta) & cos\theta+y^2(1-cos\theta) & -xsin\theta + yz(1-cos\theta) \\ -ysin\theta + xz(1-cos\theta) & xsin\theta + yz(1-cos\theta) & cos\theta+ z^2(1-cos\theta) \end{matrix} \right ]$
旋转矩阵 $\bm{\Rightarrow}$ 四元数
case1:满足 $q_0 \neq 0$ , $1+r_{11}+r_{22}+r_{33}>0$ ，即 $1+tr(\bm R)>0$
$q_0 = \frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}$
$q_1 = \frac{r_{32}-r_{23}}{4q_0}$
$q_2 = \frac{r_{13}-r_{31}}{4q_0}$
$q_3 = \frac{r_{21}-r_{12}}{4q_0}$
case2:如果 $q_0$ , $tr(\bm R)$ 趋近-1，则求解四元数的过程为：
1.如果 $max\{r_{11},r_{22},r_{33}\}=r_{11}$
$\sqrt{1+r_11 - r_{22} - r_{33}}$
$q_0 = \frac{r_{32} - r_{23}}{t}$
$q_1 = t/4$
$q_2 = \frac{r_{13} + r_{31}}{t}$
$q_3 = \frac{r_{12} + r_{21}}{t}$
2.如果 $max\{r_{11},r_{22},r_{33}\}=r_{22}$
$\sqrt{1 - r_11 + r_{22} - r_{33}}$
$q_0 = \frac{r_{13} - r_{31}}{t}$
$q_1 = \frac{r_{12} + r_{21}}{t}$
$q_2 = t/4$
$q_3 = \frac{r_{32} + r_{23}}{t}$
3.如果 $max\{r_{11},r_{22},r_{33}\}=r_{33}$
$\sqrt{1 - r_11 - r_{22} + r_{33}}$
$q_0 = \frac{r_{21} - r_{12}}{t}$
$q_1 = \frac{r_{13} + r_{31}}{t}$
$q_2 = \frac{r_{23} - r_{32}}{t}$
$q_3 = t/4$

四元数 $\bm{\Leftrightarrow}$ 欧拉角

欧拉角 $\bm{\Rightarrow}$ 四元数
欧拉角到四元数的转换，可以分为两步来推导，从欧拉角到旋转矩阵，再从旋转矩阵到四元数，直接给出结论：
$\left\{ \begin{array}{lr} q_0=cos\frac{r}{2}cos\frac{p}{2} cos\frac{y}{2}+sin\frac{r}{2}sin\frac{p}{2} sin\frac{y}{2} \\ q_1=sin\frac{r}{2}cos\frac{p}{2} cos\frac{y}{2}-cos\frac{r}{2}sin\frac{p}{2} sin\frac{y}{2} \\ q_2=cos\frac{r}{2}sin\frac{p}{2} cos\frac{y}{2}+sin\frac{r}{2}cos\frac{p}{2} sin\frac{y}{2} \\ q_3=cos\frac{r}{2}cos\frac{p}{2} sin\frac{y}{2}-sin\frac{r}{2}sin\frac{p}{2} cos\frac{y}{2}& \end{array} \right.$
反之：
四元数 $\bm{\Rightarrow}$ 欧拉角
$\left\{ \begin{array}{lr} r=arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)} \\ p=arcsin2(q_0q_2-q_1q_3)\\ y=arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_1^2+q_3^2)} & \end{array} \right.$

参考资料:
旋转矩阵、欧拉角、四元数理论及其转换关系
 坐标变换学习笔记—理论篇

半熟芝士味

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
《视觉SLAM十四讲》第三章阅读笔记（二）

《视觉SLAM十四讲》第三章阅读笔记（二）阅读本章前建议阅读以下两篇文章，作为理论支撑：《视觉SLAM十四讲》第三章阅读笔记（一）《视觉SLAM十四讲》第三章阅读笔记（补充阅读，Eigen介绍）1、欧拉角将一个旋转分成绕三个轴旋转的三个角称为欧拉角。分别是：绕车体ZZZ轴旋转，得到的角为偏航角yawyawyaw；绕旋转之后的YYY轴旋转得到的角，为俯仰角pitchpitchpitch；绕旋转之后的XXX轴旋转得到的角，为滚转角rollrollroll；简记为：类型定义
复制链接

扫一扫