AI学习笔记（五）坐标变换与视觉测量

左右手坐标系即转换

左手坐标系、右手坐标系、笛卡尔坐标系

左手坐标系：伸开左手，大拇指指向X轴正方向，食指指向Y正反向，其他三个手指指向Z轴正方向。
右手坐标系：伸开右手，大拇指指向X轴正方向，食指指向Y正反向，其他三个手指指向Z轴正方向。

在高等数学中使用的三维空间是笛卡尔坐标系：
笛卡尔坐标系其实就是右手坐标系，OpenGL中使用右手坐标系，Direct3D使用的是左手坐标系。

左手旋转规则、右手旋转规则

左手坐标系和右手坐标系相互转换

位置（location）与位移（Translation）
位置与位移的转换是相同的，都是顶点位置，转换也比较简单，将X、Y、Z中任意一个取反即可。
旋转
右手坐标系下旋转RR，那么左手坐标系下的旋转应为：
$M=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& -1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]LR=M\cdot RR\cdot M$
注意，一定要和Location，Translation取反的值一致，也就是说Location是y轴取反，那么矩阵M一定要第二行为-1，其他两个周同理，要对应起来。

旋转矩阵

对于两个三维点$P_1\left(x_1,y_1,z_1\right),P_2\left(x_2,y_2,z_2\right)$，由点$P_1$经过旋转矩阵$R$旋转到$P_2$，则有：
$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=R\left[\begin{array}{c}{x}_1\\ y_2\\ z_2\end{array}\right]$
旋转矩阵为正交矩阵，满足$R{R}^T=E$

• 绕x轴旋转：
  $$R_x\left(\theta \right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$
• 绕y轴旋转：
  $$R_x\left(\theta \right)=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$
• 绕z轴旋转：
  $$R_x\left(\theta \right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$
  任何一个旋转可以表示为依次绕着三个旋转轴旋转三个角度的组合。这三个角度称为欧拉角。三个轴可以指固定的世界坐标系轴，也可以指被旋转物体坐标系的轴，三个旋转轴次序不同，会导致结果不同。
  $R_x\left(-{\theta }_1\right)R_y\left(-{\theta }_2\right)R_z\left(\theta \right)R_y\left({\theta }_2\right)R_x\left({\theta }_1\right)$

欧拉角与万向锁

欧拉角

欧拉角有两种：
1、静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态；
2、动态：即绕物体坐标系三个轴的旋转，由于五一旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
使用动态欧拉角会出现万向锁现象，静态欧拉角不存在万向锁问题。
对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。
参考系又称为实验室参考系，是静止不动的。
而坐标系则固定于刚体，随着刚体的旋转而旋转。
设定xyz-轴为参考系的参考周。称xy-平面与XY-平面的相交为交点线，用英文字母(N)代表。
zxz的欧拉角可以静态地这样定义：

• $\alpha$是x-轴与交点线的夹角；
• $\beta$是z-轴与Z-轴的夹角；
• $\gamma$是交点线与X-轴的夹角；
  
  图中三个欧拉角分别为：$(\alpha ,\beta ,\gamma )$
  蓝色的轴为：xyz轴；红色的轴为：XYZ轴；绿色的线为交线：N
  实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述，或用不同的名字表示同样的欧拉角。因此，在使用欧拉角之前，必须先做好明确的定义。
  欧拉角的基本思想是将角位移分解为绕三个互相垂直的三个旋转组成的序列。所以，欧拉旋转的三个角，可以对应于三个旋转矩阵：
  1、Yaw（偏航）：欧拉角向量的y轴
  2、Pitch（俯仰）：欧拉角向量的x轴
  3、Roll（翻滚）：欧拉角向量的z轴
  欧拉角的缺点
  1、欧拉角的表示方式不唯一。给定某个其实朝向和坐标朝向，基石戈丁yaw、pitch、roll的顺序，也可以通过不同的yaw、pitch、roll的角度组合来表示所需的旋转。这其实主要是由于万向锁（Gimbal Lock）引起的；
  2、欧拉角的插值比较难（以实现“平滑”旋转）；
  3、计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin，cos计算量较大。
  由欧拉角求旋转矩阵
  设三个轴x，y，z的欧拉角分别为${\theta }_x,{\theta }_y,{\theta }_z$，正弦值、余弦值分别为$R({\theta }_x,{\theta }_y,{\theta }_z)=R_z({\theta }_z)R_y({\theta }_y)R_x({\theta }_x)=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& c_x{s}_z+c_x{s}_z{s}_y\\ c_y{c}_z& c_x{s}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_z\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$
  由旋转矩阵求欧拉角
  $R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}{c}_y{c}_z& c_z{s}_x{s}_y-c_x{s}_z& c_x{s}_z+c_x{s}_z{s}_y\\ c_y{c}_z& c_x{s}_z+s_x{s}_y{s}_z& c_x{s}_y{s}_z-c_z{s}_z\\ -s_y& c_y{s}_x& c_x{c}_y\end{array}\right]$
  解方程得：
  $\{\begin{array}{l}{\theta }_x=a\tan 2(r_{32},r_{33})\\ {\theta }_y=a\tan 2(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\\ {\theta }_z=a\tan 2(r_{21},r_{11})\end{array}$

万向锁

正常状态：三个独立的旋转轴
万向锁：一旦选择±90°作为pitch角，就会导致第一次旋转和第三次旋转等价，整个旋转表示系统被限制在只能绕竖直轴旋转，表示丢失了一个维度。

• pitch是围绕x轴旋转，也叫做俯仰角；
• yaw是围绕y轴旋转，也叫做偏航角
• roll是围绕z轴旋转，也叫做翻滚角。
  
  万向锁含义：当两个旋转轴重合时，导致只剩下2个旋转，2个旋转不能将所有情况进行描述，其产生的根本原因是绕三个轴的旋转不是同事进行的。
  

四元数

• 除了知名的数集（自然数、整数、实数、分数）之外，复数系统引入了一个新的数集——虚数。虚数的发明是为了解决一些特定无解的方程，例如$x^2+1=0$
• 虚数有这样的形式：$i^2=-1$
• 复数的集合是一个实数和一个虚数的和，形式：$z=a+bi(a,b\in R,i^2=-1)$（可以认为所有实数都是b=0的复数、所有虚数都是a=0的复数
• 拓展—复数的运算
• 四元数是简单的超复数。
• 四元数都是由实数加上三个虚数单位i、j、k组成，而且他们有如下联系：$i^2=j^2=k^2=-1,i^0=j^0=k^0=1$
• 每个四元数都是1、i、j和k的现行组合，及四元数一般表示为：
  $z=a+bi+cj+dk$，其中a、b、c、d是实数。

四元数的定义

$$q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v]$$

• 其中，$q_0,q_1,q_2,q_3$均为实数，$s=q_0，v=[q_1,q_2,q_3]，i^2=j^2=k^2=-1$
• 对于i、j、k本身的几何意义可以理解为一种旋转，其中i代表x轴与y轴相交平面中x轴正方向向y轴正向的旋转，j旋转代表z轴与x轴相交平面中z轴正向向x轴正向的旋转，k旋转代表y轴与z轴相交平面中y轴正向向z轴正向的玄幻，-i、-j、-k分别代表i、j、k的反向旋转。

四元数的几何意义

• 三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示。
• 这种表示方法里，axis可用一个三维向量(x,y,z)来表示，$\theta$可以用一个角度值来表示。
• 直观来讲，一个思维向量$(\theta ,x,y,z)$就可以表示三维空间任意的旋转。
• 注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是一个单位向量来表示axis，而用该三维向量的长度来表示角度值$\theta$.
• 这样一来，可以用一个三维向量$(\theta \ast x,\theta \ast y,\theta \ast z)$就可以表示三维空间任意的旋转，前提是其中(x,y,z)是单位向量。
• 这就是旋转向量(Rotation Vector)的表示方式。
  $$\left(\cos \frac{\theta }{2},x\ast \sin \frac{\theta }{2},y\ast \sin \frac{\theta }{2},z\ast \sin \frac{\theta }{2}\right)$$

四元数求旋转矩阵

• 已知四元数：$ q=q_0+q_1i+q_2j+q_3k=\lbrack s,v\rbrack$
• 利用Rodrigues公式可以由四元数求得旋转矩阵R:
  $$R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$

旋转矩阵求四元数

$$\begin{gathered} R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right] \\ R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=R\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right] \\ \Rightarrow \{\begin{array}{l}{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_2=\frac{r_{21}-r_{12}}{4q_0}\end{array} \end{gathered}$$