坐标与坐标系

这里所考虑的3维坐标系由原点、3个相互垂直、尺度一致的坐标轴的单位向量确定。
存在唯一不变的世界坐标系$(o{x}_w,o{y}_w,o{z}_w)、(e{x}_w,e{y}_w,e{z}_w)$。点的位置由世界坐标系下的坐标$(x_w,y_w,z_w)$确定，位置为$(x_w\ast e{x}_w,y_w\ast e{y}_w,z_w\ast e{z}_w)$。
其他坐标系由世界坐标系进行平移、旋转得到。
平移容易理解，即$(o{x}_l-o{x}_w,o{y}_l-o{y}_w,o{z}_l-o{z}_w)$。
旋转的表达就比较多了。

点的旋转

$v$绕着一个方向$u$旋转$\theta$角度，变成了$v_{\theta }$。
${\mathbf{v}}_{\theta }=\mathbf{v}\cos \theta +(\mathbf{u}\times \mathbf{v})\sin \theta +\mathbf{u}(\mathbf{u}\cdot \mathbf{v})(1-\cos \theta )$

有没有一个坐标系$oxy{z}_i$，旋转后的点坐标就是$v$？是存在这样的坐标系的。这样就存在点旋转与坐标系旋转（当前坐标系旋转到$oxy{z}_r$）的“对偶”分析。

寻找与点旋转“对偶”的坐标系旋转

$e{x}_w、e{y}_w、e{z}_w$旋转后的向量为$e{x}_r、e{y}_r、e{z}_r$，则在以$e{x}_r、e{y}_r、e{z}_r$为基向量的坐标系中，旋转后的点坐标是$v$。
两组坐标基向量存在如下关系，$R$的旋转矩阵：
$$(e{x}_r,e{y}_r，e{z}_r)=(e{x}_w,e{y}_w，e{z}_w)R$$
根据上式，基向量的特征，易证$R$是正交矩阵，行列式为+1。
记一个点在两个坐标系下的坐标分别是$(x_w,y_w,z_w{)}^T、(x_r,y_r,z_r{)}^T$，则
$$\begin{gathered} (e{x}_w,e{y}_w，e{z}_w)\ast (x_w,y_w,z_w{)}^T=(e{x}_r,e{y}_r，e{z}_r)\ast (x_r,y_r,z_r{)}^T \\ =(e{x}_w,e{y}_w，e{z}_w)\ast R(x_r,y_r,z_r{)}^T \end{gathered}$$
所以，
$$(x_w,y_w,z_w{)}^T=R(x_r,y_r,z_r{)}^T$$
$$v_{\theta }=Rv$$

旋转矩阵的欧拉角表示

用 z-y’-x’'的欧拉角表示，其中三个旋转角度也叫作yaw（航向角），pitch（俯仰角）和roll（横滚角）。

1. 绕z轴旋转，使得y'轴在另一个坐标的zy平面;
2. 绕y'轴旋转，使得z''轴在另一个坐标的zy平面。且x''与另一个坐标系的x轴重合;
3. 绕x''轴旋转，使得y''、z''与另一个坐标系重合
   
   $\mathbf{R}(\psi ,\theta ,\varphi )={\mathbf{R}}_z(\psi ){\mathbf{R}}_y(\theta ){\mathbf{R}}_x(\varphi )$

点旋转与旋转矩阵的转换

点旋转的表达是$(u,\theta )$，旋转矩阵的表达是$R$，表征了两个坐标系的转换。
根据点旋转的描述，来构造$R$：
需要一个中间的坐标系$oxy{z}^{\prime }$来过渡，$z^{\prime }$与旋转轴重合。这样，就是绕z'轴旋转$\theta$，坐标变换关系易得；
$oxyz$和$oxy{z}^{\prime }$的左边变化关系，也易得。

根据$R$来构造$(u,\theta )$：o-x-x'、o-y-y'、o-z-z'的中垂面的交线就是旋转轴。旋转角度易得。

旋转的李代数表示

知识1：点旋转，也叫罗戈里格斯旋转
知识2：矩阵的指数运算
$$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$
知识3：反对称矩阵的性质
$$\begin{gathered} \varphi {\varphi }^T={\varphi }^{\wedge }{\varphi }^{\wedge }+\Vert \varphi {\Vert }^2{\mathbf{I}}_{3\times 3} \\ {\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=-{\varphi }^{\wedge } \\ \varphi \times v={\varphi }^{\wedge }v \end{gathered}$$

点旋转$(u,\theta )$
$$\begin{array}{rl}\exp \left({\varphi }^{\wedge }\right)& =\exp \left(\theta {\mathbf{u}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta {\mathbf{u}}^{\wedge }\right)}^n\\ & =\mathbf{I}+\theta {\mathbf{u}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }+\frac{1}{4!}{\theta }^4{\left({\mathbf{u}}^{\wedge }\right)}^4+...\\ & =\mathbf{u}{\mathbf{u}}^T-{\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }+\theta {\mathbf{u}}^{\wedge }+\frac{1}{2!}\theta {\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }-\frac{1}{3!}{\theta }^3{\mathbf{u}}^{\wedge }+\frac{1}{4!}{\theta }^4{\left({\mathbf{u}}^{\wedge }\right)}^4+...\\ & =\mathbf{u}{\mathbf{u}}^T+\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...\right){\mathbf{u}}^{\wedge }-\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...\right){\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }\\ & ={\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{u}}^{\wedge }-\cos \theta {\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }\\ & =(1-\cos \theta ){\mathbf{u}}^{\wedge }{\mathbf{u}}^{\wedge }+I+\sin \theta {\mathbf{u}}^{\wedge }\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{u}{\mathbf{u}}^T+\sin \theta {\mathbf{u}}^{\wedge }\end{array}$$

$${\mathbf{v}}_{\theta }=\mathbf{v}\cos \theta +(\mathbf{u}\times \mathbf{v})\sin \theta +\mathbf{u}(\mathbf{u}\cdot \mathbf{v})(1-\cos \theta )=\exp (\theta {\mathbf{u}}^{\wedge })\ast v=Rv$$

李代数表示的优势在于求导优化。可以通过指数展开，舍弃高次方项，推导出简洁的线性公式。
核心是用李代数形式的旋转来表示这次迭代优化要得到的微量旋转，$R\ast exp(\theta u^{\wedge })$、$exp(\theta u^{\wedge })\ast R$

旋转描述

旋转矩阵

每个旋转有唯一的旋转矩阵。
默认，x->y、y->z、z->x的旋转为正角度。逆时针旋转为正、右手螺旋

这里，第2个是z->x旋转，但是x坐标在z坐标前面，所以，旋转矩阵要调整下。

欧拉角

每个旋转可以有无数个欧拉角。因为旋转$2\pi$就会回到原来的位置。
把旋转分解成沿三个轴转动的量：滚转角－俯仰角－偏航角(roll-pitch-yaw)。它的好处是十分的直观，且只有三个参数描述。缺点是会碰到著名的万向锁问题：在俯仰为±90∘时，表达某个姿态的形式不唯一。此外，它也不易于插值和迭代。

万向锁，表达某个姿态的形式不唯一。因为第二次旋转90°后，第一次旋转、和第三次旋转是旋转轴一样的。丢失了一个维度的自由度。我觉得迭代优化时，这个问题也可以接受呀。

欧拉角计算复杂，所以不易于插值和迭代。记录这样一个变换，至少需要三个角的sine和cosine值，也就是一共存储6个单位数据。

罗格里格斯旋转

求一个向量绕任意轴旋转 [公式] 后的向量是多少。我们可以使用罗德里格公式解得。

公式–>旋转矩阵

四元数

知乎链接

李代数

李代数
矩阵指数函数
e

上面模型是$R\ast \theta$。还有一种建模是$\theta \ast R$。