3.1 旋转矩阵

3.1.1点与坐标系

点乘

叉乘
法向量的方向成右手法则，所以叉乘的顺序是会有影响的。

反对称矩阵
反对成矩阵（skew-symmetric matrix），如a = [a1, a2, a3]


3.1.2 坐标系中的欧式变换

  1. 旋转关系加上平移关系统称为坐标系之间的变换关系。
  2. 我们用两个参考系考察机器人运动，一个是固定不动的世界坐标系，一个是依靠机器人自身建立的移动坐标系，我们研究的问题就是如何把一个向量在这两个坐标系中相互转换。
  3. 转换思路：需要先得到该点针对机器人坐标系坐标值，再根据机器人位姿转换到世界坐标系中




3.1.3 变换矩阵与齐次坐标

矩阵相乘的意义

变换矩阵Transform Matrix
1.SO(n)是特殊正交群（special orthogonal group），集合的定义如下：

特殊正交群要求正交，且行列式为1.
​
齐次坐标：
射影几何里的概念，把一个三维向量的末尾添加 1，变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移卸载一个矩阵里，使得整个关系变成线性关系。


5.SE(3)是特殊欧氏群（special euclidean group），定义如下：

6.变换矩阵的逆,反向的变换.inverse()为：

这里这个反对称矩阵的概念，听起来很简单，但是其实很重要观察反对称矩阵的特点，不难发现符合A^{T}=-A的反对称矩阵，3x3的矩阵，有九个元素，但是呢由于对角线都是0，并且他还上下对称。所以，我们只用三个元素就能表达一个反对称矩阵。比如，A用a来表示

用三个元素就能表达A，也就是说反对称矩阵只有三个自由度。 这一点很重要，正因如此，我们就可以把一个三维向量和一个三维矩阵建立对应关系。

3.2 Eigen库的使用

参考链接：
Eigen官网
民间Eigen教程
eigen 中四元数、欧拉角、旋转矩阵、旋转向量互相转换

3.3 旋转向量和欧拉角

3.3.1 旋转向量

3.3.2 欧拉角



欧拉角的奇异性问题
在特定值时，旋转自由度减1；
Yaw-pitch-roll顺序下，当pitch为90度时，存在奇异性

3.4四元数

3.4.1 四元数的定义


3.4.2 四元数的运算



3.4.3 用四元数表示旋转

3.4.4 四元数到旋转矩阵的转换

3.5 补充小萝卜的例子解释,坐标在不同坐标系转换



计算代码和注释如下：

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  //q1 q2存为两个四元数
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  //使用之前要归一化操作
  q1.normalize();
  q2.normalize();
  //两个平移向量t1 t2 
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  //题目给出的小萝卜一号在自身坐标系下的坐标
  Vector3d p1(0.5, 0, 0.2);
  //声明两个变换矩阵  分别代表w->1  w->2的变换矩阵
  //将四元数代表的旋转 赋值给变换矩阵
  Isometry3d T1w(q1), T2w(q2);
  //把平移向量赋值给变换矩阵
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);
  //此时 我们的变换矩阵已经代表了 旋转+平移
  //P2 = T2w * Tw1 * P1
  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}

参考博客

1.视觉SLAM十四讲学习笔记——第三章 三维空间刚体运动
2.SLAM学习笔记——————-（三）三维空间刚体运动
3.视觉SLAM十四讲 第3讲 三维空间刚体运动（相关知识点汇总）