视觉SLAM十四讲|【一】三维空间刚体运动

影子鱼Alexios

已于 2023-12-25 17:08:36 修改

阅读量2.3k

点赞数 24

分类专栏： algorithm 文章标签：线性代数矩阵机器人

于 2023-12-25 15:29:59 首次发布

本文链接：https://blog.csdn.net/qq_43443531/article/details/135197406

版权

algorithm 专栏收录该内容

22 篇文章 8 订阅

订阅专栏

视觉SLAM十四讲|【一】三维空间刚体运动

旋转矩阵

$SO(n)=\{ R \in \mathbb{R^{n \times n}} | RR^T=I,det(R)=1 \}$
SO(n)为特殊正交群
设 $a$ 为一点坐标 $(x, y, z)$ 。

旋转与平移的结合

旋转与平移的结合可以描述为如下形式
$a'=R^Ta+t$

性质

$R^T = R^{-1}$
$RR^T=I,det(R)=1$

旋转矩阵与变换矩阵

$\begin{bmatrix} a' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \triangleq T \begin{bmatrix} a' \\ 1 \end{bmatrix}$
关于变换矩阵，其具有特殊的结构：左上角位旋转矩阵，右侧为平移向量，左下角位0向量，右下角位1，该矩阵又称之为特殊欧式群
$\{ T=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R^{4 \times 4 } } | R \in SO(3), t \in \mathbb{R^3} \}}$

性质

求解该矩阵的逆表示一个反向的变换
$T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \end{bmatrix}$

旋转向量与欧拉角

旋转矩阵的缺点

$SO (3)$ 用9个量表示3个自由度上的变换， $SE (3)$ 用16个量表示了6个自由度上的变换，存在冗余。
旋转矩阵自带约束：必须是个正交矩阵且行列式为1，约束会使得求解更加困难。

因此，使用旋转轴+旋转角来刻画旋转，该向量称之为旋转向量。

旋转向量到旋转矩阵

罗德里格斯公式
$cos\theta + (1-cos\theta) n n^T + sin\theta n^{\land}$
$\land$ 为向量到反对称的转换符（笔者也称之为矩阵化，反之解矩阵），其定位如下所示
$a^{\land} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}$
其中 $n$ 为旋转轴。

旋转矩阵到旋转向量

$\theta = arccos(\frac{tr(R)-1}{2})$

旋转轴的性质

1、位于旋转轴上的向量在旋转后不发生改变，即
$R n = n$
2、旋转轴 $n$ 是矩阵 $R$ 特征值 $1$ 对应的特征向量

四元数（Quaternion）

定义

$q = q_0 + q_1 i + q_2 j + q_3 k$

物理含义

假设某个旋转是围绕单位向量 $n=[n_x, n_y, n_z]^T$ 进行了角度为 $\theta$ 的旋转，这个旋转的四元数形式为
$[cos\frac{\theta}{2}, n_x sin\frac{\theta}{2}, n_y sin\frac{\theta}{2}, n_z sin\frac{\theta}{2}]$
反之，亦可以从单位四元数中计算出对应的旋转轴与夹角
$\theta = 2arccos(q_0)$
$[n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T / sin(\theta/2)$

运算

四元数的定义可以写为如下形式
$q = s + x i + y j + z k = [s, v]$
$v = [x i, y j, z k]$

加法

$q_a \pm q_b = [s_a \pm s_b, v_a \pm v_b]$

乘法

$\begin{split} q_a q_b &= s_a s_b - x_a x_b - y_a y_b - z_a z_b \\ & + (s_a x_b + x_a s_b + y_a z_b -z_ay_b)i \\ & + (s_ay_b - x_az_b + y_as_b + z_ax_b)j \\ & + (s_az_b + x_ay_b - y_ax_b + z_as_b)k \end{split}$
或者
$q_a q_b = [s_as_b-v_a^Tv_b, s_av_b + s_bv_a + v_a \times v_b]$
矩阵外积的性质如下所是
在这里插入图片描述

模长

$||q_a||= \sqrt{(s_a^2+x_a^2+y_a^2+z_a^2)}$
$q_a q_b|| = ||q_a|| ||q_b||$

逆

$q^{-1} = q^* / ||q||^2$
$qq^{-1}=q^{-1}q=1$
$q_a q_b)^{-1} = q_b^{-1}q_a^{-1}$

数乘与点乘

$q_a \cdot q_b = s_a s_b + x_ax_b + y_ay_b + z_az_b$
$k q = [k s, k v]$

四元数表示旋转

假设一空间三维点 $\in \mathbb{R^3}$ ，由轴角 $n,\theta$ 表示旋转
三维空间点用一个虚四元数表示为
$p = [0, x, y, z]$
用四元数表示旋转为
$q=[cos\frac{\theta}{2}, nsin\frac{\theta}{2}]$
乘积可以表示为
$p'=qpq^{-1}$

四元数转旋转矩阵

$\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_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix}$