slam-三维空间刚体的旋转-大师兄（简化版）

1.三维空间中刚体的旋转总共有4种表示方法（第3讲比较详细的讲解）

本节精髓：旋转矩阵、平移向量--->欧式变换矩阵--->旋转向量--->欧拉角--->四元数

（往下面看你就懂了）

接下来顺一下思路

我们要知道：欧式变换中，坐标系的变换有：旋转、平移两种情况

一、旋转矩阵，平移向量

 w是世界坐标系，c是相机坐标系

目的：需要把相机转换为世界

旋转矩阵为R，平移向量为t

R是一个行列式为1的正交矩阵

遇到问题：多次变换后根据上式（非线性关系），好复杂（引出下面的一系列解决办法！）

解决办法：齐次坐标——在三维向量的末尾添加一维（为数字1），将其变成四维向量（不难理解）（某个点的具体坐标不是唯一确定的，比如说[1, 1, 1, 1]和[2, 2, 2, 2]表示的是同一个点）

齐次坐标归一化坐标：归一化坐标是唯一的（这就很棒！），所有坐标除以最后一项。

最终结果：现在是线性变换啦！

 

中间的矩阵称为欧式变换矩阵

二、旋转向量

为了解决冗余！

问题：欧式变换可以很好的描述的刚体的运动，并且还是线性的形式。为了还需要旋转向量？

1. 欧式变换矩阵用9个变量描述6个自由度的运动，信息有冗余
2. 欧式变换矩阵本身是行列式为1的正交矩阵，这样的约束会使得后面优化的求解变得苦难。

思想：用一个旋转轴n和旋转角θ来描述一个旋转

效果：旋转向量用3个自由度描述旋转，3个描述平移向量，这样刚好可以用6个自由度来表达一次变换。

牛逼的转换公式：（罗德里格斯公式）

旋转向量 旋转矩阵

若已知旋转矩阵，求旋转角：

同时，旋转轴经过旋转之后不变，则有，解方程即可得到旋转轴。

三、欧拉角

最直观的方式来表示旋转！

致命缺点：万向锁

SLAM中用的很少

在俯仰角为±90°时，第一次和第3次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性。

事实上，想要表达三维旋转，至少需要4个变量。

四、四元数

形式如下：

单位四元数可以用来表示三维空间中任意一个旋转，这种表达方式和欧式变换矩阵、欧拉角是等价的。

 1）四元数和旋转向量之间的转换

假设某个旋转是绕单位向量进行了角度为，那么这个旋转对应的四元数为：

 

 相反，若已知一个单位四元数为，可以从它计算出旋转轴和夹角：

 

2）用四元数表示旋转

假设一个空间三维点，以及一个由轴角指定的旋转，经过旋转之后变为点。

它们之间的关系可以用下列式子来表达：

1. 把三维空间点用一个纯四元数来描述：
2. 把旋转用四元数表示：
3. 旋转之后的点为：

 可以验证，计算结果的实部为0，也就是一个纯四元数。虚部的3个分量表示旋转后的点的坐标。

 （3）四元数到旋转矩阵的转换---见SLAM十四讲P55。

在这里介绍一下视觉SLAM十四讲3.6.2实际的坐标变换例子的代码分析：

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

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);//定义两个小萝卜自身姿态的两个四元数
  q1.normalize();//?对两个四元数进行归一化
  q2.normalize();
  //定义两个小萝卜的位置的两个三维坐标
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);//用来表示小萝卜一号坐标系下该点坐标（题干给出）
  Vector3d p2; //用来表示小萝卜二号坐标系下该点坐标
  //!下面分别用四元数以及变换矩阵得到小萝卜二号的该点观测信息
  //以下用四元数求解p2坐标
  p2 = q2 * q1.inverse() * (p1 - t1) + t2;//求解p2坐标
  //这里的inverse是相反的意思，也就是求q1的逆矩阵
  //公式为：p2 = q2  * q1^-1 * (p1 - t1) + t2
  cout << "四元数求得的p2坐标" << endl;
  cout << p2 << endl;//列向量显示
  cout << p2.transpose() << endl;//行向量显示，transpose是转置矩阵的意思

  //以下用欧拉矩阵求解p2坐标
  Isometry3d T1w(q1), T2w(q2);//欧式变换矩阵Isometry（虽然称为3d，实质上是4＊4的矩阵）
  T1w.pretranslate(t1);//设置平移向量，我的理解是加入这个平移向量
  T2w.pretranslate(t2);

  Vector3d p3;//用来表示小萝卜二号坐标系下该点坐标（变换矩阵方法）
  p3 = T2w * T1w.inverse() * p1;//求解p3坐标
  cout << "变换矩阵/欧拉矩阵求得的p2坐标" << endl;
  cout << p3.transpose() << endl;
  return 0;
}

 四元数解法

先说一下四元数的推导：
假设目标点p在世界坐标系下的坐标为：

Pw

利用四元数表示旋转，两个小萝卜坐标系下p点坐标有如下公式：

p1​=q1​∗Pw​+t1​

p2​=q2​∗Pw​+t2​

​

变换矩阵解法

若使用欧拉矩阵，我们设两个坐标的变换矩阵为T1和T2，有：

p1​=T1​∗Pw​

p2​=T2​∗Pw​

最后：

学完本章节，我们可以做什么？

1.进行坐标转换（给出两相机位姿：四元数、平移）

2.显示运动轨迹

3.显示相机的位姿（pangolin）