2020.10.19-2020.10.25工作周报_omnislam

摘要

这周就又要结束了。。。时间都去哪了 -.-

• 《视觉slam十四讲》第三讲、第四讲前半部分。
• OmniSLAM 环视相关的论文阅读
• 吴恩达深度学习课程 第四周

一、OmniSLAM论文阅读

以下的图片均来自于论文：OmniSLAM ，旨在记录学习心得，如有侵权，立即删除。

1. 摘要 这篇论文提出了一个基于超宽视场的鱼眼相机的面向wide-baseline的多视角全方向定位和深度地图系统，对环境立体观察能够360°覆盖。为了更加实用、更加精确地重建，我们首先介绍了提升的、轻量级的全方向深度估计的深度神经网络，比已经存在的网络更快更精确。其次，我们把我们的全方向深度估计与视觉里程计进行了整合，并且针对全局一致性添加了一个闭环模型。使用估计的深度地图，我们将关键点重新投影到了每个视场，这使得出现了一个更好更有效的特征匹配过程。最后，我们将全方向深度地图和估计的装备位姿融合到了TSDF中来获得一个3D地图。我们在用ground-truth合成的数据集以及真实世界中具有挑战性的环境序列上验证了我们的方法。广泛的实验说明我们提出的系统在合成的和真实世界环境中都有着优秀的重建结果。

2. 介绍 这一部分主要说现存的一些方法大部分有垂直分辨率低、由于光照操作或者诸如尺寸、成本和功耗等实际问题而导致的传感器间相互干扰的问题。这篇文献主要的贡献如下：
   1. 我们为宽带基线立体设置的全向深度估计提出了轻量级和改进的网络。 我们网络的准确性，参数数量和运行时间已经变得比以前的版本更好，这使我们的系统更加实用。
   2. 通过将深度图集成到ROVO中并添加闭环模块，我们构建了一个强大的全向可视SLAM系统。 在充满挑战的室内和大型室外环境中，估计轨迹的准确性均比以前的版本有所提高。
   3. 我们提出了一个集成的全向定位和密集制图系统，在合成的或现实世界的室内和室外环境下进行的大量实验表明，我们提出的系统可以为各种场景生成重构良好的3D密集图。

3. 相关研究 目前，环视方面的系统和研究相对较少，有一些多相机系统用来全方向立体深度估计和视觉里程计，然而，它们需要许多个相机，这在一些情况下可能是有问题的。

4. 方法 下图阐述了我们提出的系统地总体流程，由三个部分组成：全方向深度估计、全方向视觉SLAM以及基于TSDF的密集地图。
   

5. 实验结果
   下图展示了在合成数据集上的密集地图的完整性和精确性。
   
   本文提出的$OmniMV{S}^{+}$和$Tin{y}^{+}$重建的3D地图比$OmniMVS$更加精确。尤其是在低阈值下，就像下图 Fig.3所示的那样。
   
   在所有数据集中使用来自ROVO +的位姿也比ROVO表现更好，图6a显示了它们之间的定性比较。 车库的定性结果也显示在图6b中，我们的方法成功地重建了诸如桌子和沙发之类的小物体，以及墙壁和地板等无纹理的大区域。
   
   我们还分别在图1和图7中的真实数据集：IT / BT和Wangsimni上显示了系统的定性结果。
   
   

6. 结论
   在本文中，我们为具有宽FOV鱼眼镜头的宽基线多相机钻机提出了一个集成的全向定位和密集映射系统。 所提出的轻量级深度神经网络在参数数量较少的情况下，可以更快，更准确地估计全方位深度图。 然后，将输出深度图集成到视觉里程表中，并且我们提出的视觉SLAM模块可实现更好的姿态估计性能。 广泛的实验表明，我们的系统可以生成合成环境和真实环境的重建良好的3D地图。

二、吴恩达深度学习课程 第四周

三、《视觉slam十四讲》——第三讲 三维刚体运动

果然，李群李代数还是逃不过去。接着硬啃下去。上周学到了3.2 实践Eigen就没好好学了，这周就从这里接着来把。

3.2 实践Eigen

如果你的ubuntu上还没有安装Eigen。请输入以下命令来安装它：

sudo apt-get install libeigen3-dev
1

Eigen头文件的默认位置在“/usr/include/eigen3/”中，如果不确定，可以输入以下命令来查找它的位置。

sudo updatedb
locate eigen3
1
2

相比于其他库，Eigen特殊之处在于，它是一个纯用头文件搭建起来的库（这非常神奇！）。这意味着你只能找到它的头文件，而没有.so或.a那样的二进制文件。我们在使用时，只需引入Eigen的头文件即可，不需要链接它的库文件（因为它没有库文件）。

下面写一段代码，来实际练习一下Eigen的使用：

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main( int argc, char** argv )
{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
    Eigen::Vector3d v_3d;
	// 这是一样的
    Eigen::Matrix<float,3,1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << matrix_23 << endl;

    // 用()访问矩阵中的元素
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;      // 转置
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 迹
    cout << 10*matrix_33 << endl;               // 数乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.determinant() << endl;    // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大

    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    
	// 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103

要编译它，需要在CMakeLists.txt里制定Eigen的头文件目录。CMakeLists.txt内容如下：

cmake_minimum_required( VERSION 2.8 )
project( useEigen )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )

# 因为只有头文件，我们不需要再用target_link_libraries语句将程序链接到库上。

add_executable( eigenMatrix eigenMatrix.cpp )
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

输出结果如下，代码中有详细注释，就不一一解释每行输出了。

3.3 旋转向量和欧拉角

3.3.1 旋转向量

通过3.1节，我们有了旋转矩阵来描述旋转，有了变换矩阵描述一个六自由度的三维刚体运动，是不是已经足够了呢？但是，矩阵的表示方式至少有以下几个缺点：

1. $SO(3)$的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度的变换。那么，是否有更紧凑的表示呢？
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为$1$。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时。这些约束会使得求解变得更困难。

因此，我么希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用六维向量表达变换。


对于坐标系的旋转，我们知道，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量（或轴角，Axis-Angle）。这种表示法只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。

事实上，旋转向量就是第四讲准备介绍的李代数。 旋转向量和旋转矩阵之间是如何转换的呢？假设有一个旋转轴为$\mathbf{n}$，角度为$\theta$的旋转，显然，它对应的旋转向量为$\theta \mathbf{n}$。由旋转向量到旋转矩阵的过程由**罗德里格斯公式（Rodriguez’s Formula）**表明，由于推到过程比较复杂，只给出转换的结果：
$$R=\cos \theta I+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta \hat{\mathbf{n}}\text{(3.14)}$$.
符号^是向量到反对称的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角$\theta$，有：
$$\begin{array}{rl}tr(R)& =\cos \theta tr(I)+(1-\cos \theta )tr(\mathbf{n}{\mathbf{n}}^T)+\sin \theta tr(\hat{\mathbf{n}})\\ & =3\cos \theta +(1-\cos \theta )\text{(3.15)}\\ & =1+2\cos \theta .\end{array}$$

因此：
$$\theta =\arccos (\frac{tr(R)-1}{2}).\text{(3.16)}$$
关于转轴$\mathbf{n}$,由于旋转轴上的向量在旋转后不发生改变，说明
$$R\mathbf{n}=\mathbf{n}$$
因此，转轴$\mathbf{n}$是矩阵$R$特征值$1$对应的特征向量。求解此方程，再归一化，就得到了旋转轴。也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。仍然剧透几句，这里的两个转换公式在下一讲仍将出现，你会发现它们正是$SO(3)$上李群与李代数的对应关系。

3.3.2 欧拉角

下面我们来说说欧拉角。
虽然旋转矩阵、旋转向量能描述旋转，但对人类非常不直观。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。当然，由于分解方式有许多种，所以欧拉角也存在着不同的定义方法。你或许在航空、航模中听说过“俯仰角”、“偏航角”这些词。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。大概如图3-3所示。


欧拉角一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）：在俯仰角为$\pm 90^{°}$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适合于插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。不过，若你想验证自己算法是否有错时，转换成欧拉角能够快速辨认结果的正确与否。

3.4 四元数

3.4.1 四元数的定义

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；
欧拉角和旋转向量是紧凑的，但具有奇异性；

事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于，当我们想用两个坐标表示地球表面时（如经度、纬度），必定存在奇异性（纬度为$\pm 90^{°}$时经度无意义）。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。

回忆复数，类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数(Quaternion)。四元数时Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。如果说缺点的话，四元数不够直观，其运算稍微复杂一些。

一个四元数$\mathbf{q}$拥有一个实部和三个虚部。
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k,\text{(3.17)}$$
其中$i,j,k$为四元数的三个虚部。这三个虚部满足关系式：
$$\{\begin{array}{ll}& i^2=j^2=k^2=-1\\ & ij=k,ji=-k\text{(3.18)}\\ & jk=i,kj=-i\\ & ki=j,ik=-j\end{array}$$
由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：
$$\mathbf{q}=\left[\begin{array}{c}s,\mathbf{v}\end{array}\right],s=q_0\in \mathbb{R},\mathbf{v}={\left[\begin{array}{c}{q}_1,q_2,q_3\end{array}\right]}^T\in {\mathbb{R}}^3,$$
这里，$s$称为四元数的实部，而$\mathbf{v}$称为它的虚部。如果一个四元数虚部为$0$，称之为实四元数。反之，若它的实部为$0$，称之为虚四元数。

我们能用单位四元数表示三维空间中任意一个旋转。这种表达方式和旋转矩阵、旋转向量有什么关系呢？我们不妨先来看旋转向量。假设某个旋转是绕单位向量$\mathbf{n}={\left[\begin{array}{c}{n}_x,n_y,n_z\end{array}\right]}^T$进行了角度为$\theta$的旋转，那么这个旋转的四元数形式为：
$$\mathbf{q}={\left[\begin{array}{c}\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\end{array}\right]}^T.\text{(3.19)}$$
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
$$\{\begin{array}{l}\theta =2\arccos q_0\text{(3.20)}\\ {\left[\begin{array}{c}{n}_x,n_y,n_z\end{array}\right]}^T={\left[\begin{array}{c}{q}_1,q_2,q_3\end{array}\right]}^T/\sin \frac{\theta }{2}\end{array}$$
对式$(3.19)$的$\theta$加上$2\pi$，我们得到一个相同的旋转，但此时对应的四元数变成了$-\mathbf{q}$。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

3.4.2 四元数的运算

四元数和通常复数一样，可以进行一系列运算。常见的有四则运算、数乘、求逆、共轭等等。我们分别介绍它们。
现有两个四元数${\mathbf{q}}_{\mathbf{a}},{\mathbf{q}}_{\mathbf{b}}$，它们的向量表示为$\left[\begin{array}{c}{s}_a,{\upsilon }_{\mathbf{a}}\end{array}\right]$，$\left[\begin{array}{c}{s}_b,{\upsilon }_{\mathbf{b}}\end{array}\right]$，或者原始四元数表示为：
$${\mathbf{q}}_{\mathbf{a}}=s_a+x_{a}i+y_{a}j+z_{a}k,{\mathbf{q}}_{\mathbf{b}}=s_b+x_{b}i+y_{b}j+z_{b}k.$$
那么，它们的运算可表示如下。

1. 加法和减法
   

2. 乘法
   
   

3. 共轭

4. 模长
   

5. 逆
   

6. 数乘与点乘
   

3.4.3 用四元数表示旋转

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

任意单元四元数描述了一个旋转，该旋转亦可用旋转矩阵或旋转向量描述。从旋转向量到四元数的转换方式已在式（3.20）中给出。因此，现在看来，把四元数转换为矩阵的最直观方法，是先把四元数$\mathbf{q}$转换为轴角$\theta$和$\mathbf{n}$，然后再根据罗德里格斯公式转换为矩阵。不过那样要计算一个$\arccos$函数，代价较大。实际上这个计算可以通过一定的技巧绕过的。我们省略过程中的推到，直接给出四元数到旋转矩阵的转换方式。

值得一提的是，由于$\mathbf{q}$和$-\mathbf{q}$表示同一个旋转，事实上一个$R$对应的四元数表示并不是唯一的。同时，除了上面给出的转换方式外，还存在其他几种计算方法，而本书（《视觉slam十四讲》）都省略了。实际编程中，当$q_0$接近$0$时，其余三个分量会非常大，导致解不稳定，此时我们再考虑使用其他的方式进行转换。

最后，无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择对我们最为方便的形式，而不必拘泥于某种特定的样子。

3.5 相似、仿射、射影变换

3D空间中的变换，除了欧式变换之外，还存在其余几种，其中欧式变换是最简单的。它们一部分和测量几何有关，因为在之后的章节中可能会提到，所以我们先罗列出来。欧式变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。而其他几种变换则会改变它的外形。他们都拥有类似地矩阵表示。

1. 相似变换
   相似变换比欧式变换多了一个自由度，它允许物体进行均匀的缩放，其矩阵表示为：
   $$T_S=\left[\begin{array}{cc}sR& \mathbf{t}\\ 0^T& 1\end{array}\right].(3.37)$$
   注意到旋转部分多了一个缩放因子$s$，表示我们在对向量旋转之后，可以在$x,y,z$三个坐标上进行均匀的缩放。由于含有缩放，相似变换不再保持图形的面积不变。你可以想象一个边长为1的立方体通过相似变换后，变成边长为10的样子（但仍然是立方体）。
2. 仿射变换
   放射变换的矩阵形式如下：
   $$T_A=\left[\begin{array}{cc}A& \mathbf{t}\\ 0^T& 1\end{array}\right].(3.38)$$
   与欧式变换不同的是，仿射变换只要求$A$是一个可逆矩阵，而不必是正交矩阵。放射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。
3. 射影变换
   射影变换是最一般的变换，它的矩阵形式为：
   $$T_P=\left[\begin{array}{cc}A& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right].(3.39)$$
   它左上角为可逆矩阵$A$，右上为平移$\mathbf{t}$，左下缩放${\mathbf{a}}^T$。由于采用齐坐标，当$v\ne 0$时，我们可以对整个矩阵除以$v$得到一个右下角为$1$的矩阵；否则，则得到右下角为$0$的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。
   
   
   射影变换是现在讲过的变换中，形式最为一般的。从真实世界到相机照片的变换可以看成一个射影变换。可以想象一个原本方形的地板砖，在照片当中是什么样子：首先，它不再是方形的。由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。

表3-1总结了目前讲到的几种变换的性质。注意在“不变性质”中，从下到上是有包含关系的。例如，欧式变换除了保体积之外，也具有保平行、相交等性质。

我们之后会说到，从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换则为仿射变换。不过，在详细讲述相机模型之前，我们只要对它们有个大致的印象即可。

3.6 实践：Eigen几何模块

现在，我们来实际演练一下前面讲到的各种旋转表达方式。我们将在Eigen中使用四元数、欧拉角和旋转矩阵，演示他们之间的变换方式。我们还会给出一个可视化程序，帮助读者理解这几个变换的关系。

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

CMakeLists.txt:

cmake_minimum_required( VERSION 2.8 )
project( geometry )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

add_executable( eigenGeometry eigenGeometry.cpp )
1
2
3
4
5
6
7

编译运行

mkdir build
cd build
cmake ..
make 
./eigenGeometry
1
2
3
4
5

输出结果：

二、《视觉slam十四讲》——第四讲 李群与李代数

本节目标

1. 理解李群与李代数的概念，掌握SO(3), SE(3)与对应李代数的表示方式。
2. 理解BCH近似的意义
3. 学会在李代数上的扰动模型
4. 使用Sophus对李代数进行运算

上一讲重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的$R,\mathbf{t}$，使得误差最小化。

如前所言，旋转矩阵自身是带有约束的（正交且行列式为$1$）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群-李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。由于我没有李群李代数的基本知识，从最基础的开始看起。

4.1 李群李代数基础

上一讲，我们介绍了旋转矩阵和变换矩阵的定义。当时，我们说三维旋转矩阵构成了特殊正交群$SO(3)$，而变换矩阵构成了特殊欧氏群SE(3)：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } (4.1) SO(n)=\{R\in\mathbb{R}^{n\times n}|RR^T=I,det(R)=1\} \quad \text{(4.1)} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(4.1)
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . (4.2) SE(3) = \{T= $$\begin{bmatrix}R&t\\ \mathbf{0}^T & 1 \end{bmatrix}$$ \in \mathbb{R}^{4\times 4} | R \in SO(3), \mathbf{t} \in \mathbb{R}^3 \}. \quad \text{(4.2)} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}.(4.2)

不过，当时我们并没有详细解释群的含义。我们会发现他们只有一种较好的运算：乘法。$SO(3)$和$SE(3)$关于乘法是封闭的。对于这种只有一个运算的集合。我们把它叫做群。

4.1.1 群

群（Group）是一种集合加上一种运算的代数结构。我们把集合记作$A$，运算记作·，那么群可以记作$G=(A,\cdot )$。群要求这个运算满足一下几个条件：

1. 封闭性：$\forall a_1,a_2\in A,a_1\cdot a_2\in A$
2. 结合律：$\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3).$
3. 幺元：$\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆：$\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

李群是指具有连续（光滑）性质的群。像整数群$\mathbb{Z}$那样离散的群没有连续性质，所以不是李群。而$SO(n)$和$SE(n)$，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。由于$SO(3)$和$SE(3)$对于相机姿态估计尤为重要，我们主要讨论这两个李群。

下面，我们先从较简单的$SO(3)$开始讨论，我们将会发现每个李群都有对应的李代数。我们首先引出$SO(3)$上面的李代数$\mathfrak{s}\mathfrak{o}(3)$。

4.1.2 李代数的引出

考虑任意旋转矩阵$R$，我们知道它满足：
$$R{R}^T=I(4.5)$$

现在，我们说，$R$是某个相机的旋转，它会随时间连续地变化，即为时间的函数：$R(t)$。由于它仍是旋转矩阵，有
$$R(t)R(t{)}^T=I$$

在等式两边对时间求导，得到：
$$\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0$$
整理得：
$$\dot{R}(T)R(t{)}^T=-(\dot{R}(t)R(t{)}^T{)}^T.(4.6)$$

（额……由于不会打那个向上和向下尖的符号，直接把书上的搬过来了。。）

我们看到，旋转矩阵$R$与另一个反对称矩阵${\varphi }_0$通过指数关系发生了联系。也就是说，当我们知道某个时刻的$R$时，存在一个向量$\varphi$，我们满足这个矩阵指数关系。但是矩阵的指数是什么呢？这里有两个问题需要澄清：

1. 如果上式成立，那么给定某时刻的$R$，我们就能求得一个$\varphi$，它描述了$R$在局部的导数关系。与$R$对应的$\varphi$有什么含义呢？后面会看到，$\varphi$正是对应到$SO(3)$上的李代数$\mathfrak{s}\mathfrak{o}(3)$；
2. 其次，矩阵指数$exp(\hat{\varphi })$如何计算？——事实上，这正是李群与李代数间的指数/对数映射。

下面我们一一加以介绍。

4.1.3 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数定义如下：
李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算$[,]$组成。如果它们满足以下几条性质，称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$\mathfrak{g}$。

其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

作为例子，三维向量${\mathbb{R}}^3$上定义的叉积$\times$是一种李括号，因此$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数。我们可以尝试将叉积的性质代数到上面四条性质中。

$\mathbf{a}=(a_x,a_y,a_z)\mathbf{b}=(b_x,b_y,b_z)$
$\mathbf{a}\in {\mathbb{R}}^3,\mathbf{b}\in {\mathbb{R}}^3$
$\mathbf{a}\times \mathbf{b}=\left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|=(a_y{b}_z-a_z{b}_y,a_z{b}_x-a_x{b}_z,a_x{b}_y-a_y{b}_x)$
显然，$\mathbf{a}\times \mathbf{b}\in {\mathbb{R}}^3$ ，满足性质1：封闭性


额… 其他的 就不验证了，我信。。

4.1.4 李代数$\mathfrak{s}\mathfrak{o}(3)$

下面我们说，之前提到的$\varphi$，事实上是一种李代数。 $SO(3)$对应的李代数是定义在${\mathbb{R}}^3$上的向量，我们记作$\varphi$。根据前面的推导，每个$\varphi$都可以生成一个反对称矩阵：

在此定义下，两个向量${\varphi }_1,{\varphi }_2$的李括号为：

可以验证，该定义下的李括号满足上面的几条性质。由于$\varphi$与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$\mathfrak{s}\mathfrak{o}(3)$的元素是3维向量或者3维反对称矩阵，不加区别：

至此，我们清楚了$\mathfrak{s}\mathfrak{o}(3)$的内容。它们是一个三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与$SO(3)$的关系由指数映射给定：

指数映射会在稍微介绍。由于已经介绍了$\mathfrak{s}\mathfrak{o}(3)$，我们顺带先来看看$SE(3)$上对应的李代数。

4.1.5 李代数$\mathfrak{s}\mathfrak{e}(3)$

与$\mathfrak{s}\mathfrak{o}(3)$相似，$\mathfrak{s}\mathfrak{e}(3)$位于${\mathbb{R}}^6$空间中：

我们把每个$\mathfrak{s}\mathfrak{e}(3)$元素记作$\xi$，它是一个六维向量。前三维为平移，记作$\rho$；后三维为旋转，记作$\varphi$，实质上是$\mathfrak{s}\mathfrak{o}(3)$元素。同时，我们扩展了^{符号的含义。在$\mathfrak{se}(3)$中，同样使用}符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称：

---

不行了，好抽象，看不下去了。。。改天再来~