视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示

专栏系列文章如下：

goldqiu：视觉 SLAM 十四讲学习笔记 - 第一讲

goldqiu：视觉 SLAM 十四讲学习笔记 - 第二讲 - 初识 SLAM

goldqiu：视觉 SLAM 十四讲学习笔记 - 第二讲 - 开发环境搭建

goldqiu：视觉 SLAM 十四讲学习笔记 - 第三讲 - 旋转矩阵和 Eigen 库

goldqiu：视觉 SLAM 十四讲学习笔记 - 第三讲 - 旋转向量、欧拉角、四元数

相似、仿射、射影变换

除了欧氏变换之外，3D 空间还存在其他几种变换方式。它们一部分和测量几何有关。欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。其他几种变换则会改变它的外形。它们都拥有类似的矩阵表示。

相似变换

相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为：

旋转部分多了一个缩放因子 s，表示我们在对向量旋转之后，可以在 x,y,z 三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。可以想象一个边长为 1 的立方体通过相似变换后，变成边长为 10 的样子（但仍然是立方体）。三维相似变换的集合也叫做相似变换群，记作 Sim(3)。

仿射变换

仿射变换的矩阵形式如下：

与欧氏变换不同的是，仿射变换只要求 A 是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

射影变换

射影变换是最一般的变换，它的矩阵形式为：

左上角为可逆矩阵 A，右上角为平移 t，左下角为缩放 a。由于采用了齐次坐标，当 v≠0 时，我们可以对整个矩阵除以 v 得到一个右下角为 1 的矩阵；否则得到右下角为 0 的矩阵。因此，2D 的射影变换一共有 8 个自由度，3D 则共有 15 个自由度。射影变换是变换中，形式最为一般的。从真实世界到相机照片的变换可以看成一个射影变换。可以想象一个原本方形的地板砖，在照片当中是什么样子：首先，它不再是方形的。由于近大远小的关系，它甚至不是平行四边形，而是一个不规则的四边形。

**总结：**在 “不变性质” 中，从上到下是有包含关系的。例如，欧氏变换除了保体积之外，也具有保平行、相交等性质。

从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换为仿射变换。

实践：Eigen 几何模块

Eigen 几何模块的数据演示

slambook2/ch3/useGeometry/useGeometry.cpp

代码中有注释。

Eigen 中对各种形式的表达方式总结如下。每种类型都有单精度和双精度两种数据类型，不能由编译器自动转换。以双精度为例，把最后的 d 改成 f，即得到单精度的数据结构。

• 旋转矩阵（3 × 3）：Eigen::Matrix3d。

• 旋转向量（3 × 1）：Eigen::AngleAxisd。

• 欧拉角（3 × 1）：Eigen::Vector3d。

• 四元数（4 × 1）：Eigen::Quaterniond。

• 欧氏变换矩阵（4 × 4）：Eigen::Isometry3d。

• 仿射变换（4 × 4）：Eigen::Affine3d。

• 射影变换（4 × 4）：Eigen::Projective3d。

程序中，演示了如何使用 Eigen 中的旋转矩阵、旋转向量（AngleAxis）、欧拉角和四元数。

进一步了解 Eigen 的几何模块可以参考

http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html

注意，程序代码和数学表示有一些细微的差别。例如，通过运算符重载，四元数和三维向量可以直接计算乘法，但在数学上则需要先把向量转成虚四元数，再利用四元数乘法进行计算，同样也适用于变换矩阵乘三维向量的情况。总体而言，程序中的用法会比数学公式更灵活一些。

举例演示坐标变换：

设有机器人一号和机器人二号位于世界坐标系中。记世界坐标系为 W，机器人的坐标系为 R1 和 R2。机器人一号的位姿为 q1 = [0.35, 0.2, 0.3, 0.1]T, t1 = [0.3, 0.1, 0.1]T。机器人二号的位姿为 q2 = [−0.5, 0.4, −0.1, 0.2]T, t2 = [−0.1, 0.5, 0.3]T。这里的 q 和 t 表达的是世界坐标系到相机坐标系的变换关系。机器人一号看到某个点在自身的坐标系下坐标为 pR1 = [0.5, 0, 0.2]T，求该向量在机器人二号坐标系下的坐标。

程序为：slambook2/ch3/examples/coordinateTransform.cpp

注意：四元数使用之前需要归一化。

可视化演示

显示运动轨迹

轨迹文件记录了一个机器人的运动轨迹，存储于 trajectory.txt，每一行用下面的格式存储：

time,tx,ty,tz,qx,qy,qz,qw, 把它画到一个窗口中。

其中 time 指该位姿的记录时间，t 为平移，q 为旋转四元数，均是以世界坐标系到机器人坐标系记录。

在画轨迹的时候，可以把 “轨迹” 画成一系列点组成的序列。这其实是机器人（相机）坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点，即 OR，那么，此时的 OW 就是这个原点在世界坐标系下的坐标：

OW 正是 TWR 的平移部分。因此，可以从 TWR 中直接看到相机在何处，这是 TWR 更为直观的原因。在可视化程序里，轨迹文件存储了 TWR 而不是 TRW。

在 linux 中，基于 OpenGL 的 Pangolin 库，在支持 OpenGL 的绘图操作基础之上还提供了一些 GUI 的功能。

使用 git 的 submodule 功能来管理依赖的第三方库。可以进入 3rdparty 文件夹中直接安装所需的库，git 保证了使用的版本是一致的。

程序演示了如何在 Panglin 中画出 3D 的位姿。用红、绿、蓝三种颜色画出每个位姿的三个坐标轴（计算了各坐标轴的世界坐标），然后用黑色线将轨迹连起来。

程序是：slambook2/ch3/examples/plotTrajectory.cpp

运行步骤：

https://github.com/stevenlovegrove/Pangolin

一开始从 github 下载最新版本的 Pangolin，发现需要 3.10 的 cmake，

然后先安装 3.10 版本的 cmake，

Index of /files/v3.18

在官网下载源文件后，解压进去文件夹中

./bootstrap --prefix=/opt/cmake-install

make

make install

sudo gedit ~/.bashrc

#把下面两个命令加入~/.bashrc 中

export PATH=/opt/cmake-install/bin:$PATH

export CMAKE_PREFIX_PATH=/opt/cmake-install:$CMAKE_PREFIX_PATH

#保存一下并运行

source ~/.bashrc

后面发现直接从 https://github.com/gaoxiang12/slambook2.git

下载的 pangolin 不需要安装高版本的 cmake。

需要安装一些依赖：

sudo apt-get install libglew-dev

sudo apt-get install cmake

sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev

sudo apt-get install libxkbcommon-x11-dev

sudo apt-get install wayland-protocols

然后：

mkdir build && cd build

cmake …

运行程序遇到 Pangolin X11: Unable to retrieve framebuffer options 的问题。

解决方法是注释掉 Pangolin 的源代码 src/display/device/display_x11.cpp 中的 L123-L124，即：

//GLX_SAMPLE_BUFFERS , glx_sample_buffers,

//GLX_SAMPLES , glx_sample_buffers > 0 ? glx_samples : 0,

程序运行结果：

显示相机的位姿

slambook2/ch3/visualizeGeometry 中，以可视化的形式演示了相机位姿的各种表达方式。用鼠标操作相机时，左侧的方框里会实时显示相机位姿对应的旋转矩阵、平移、欧拉角和四元数。效果如下：

习题

1. 验证旋转矩阵是正交矩阵。

2. 寻找罗德里格斯公式的推导过程并加以理解。

3. 验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维空间点。

4. 总结旋转矩阵、轴角、欧拉角、四元数的转换关系。

5. 假设有一个大的 Eigen 矩阵，把它的左上角 3 × 3 的块取出来，然后赋值为单位阵。编程实现。

6. 一般线性方程 Ax = b 有哪几种做法？在 Eigen 中实现。

在三维空间中，常常需要变换当前机器人的位姿计算定义的绝对坐标系和当前机器人所处相对坐标系之间的关系。而主要的变换则是平移和旋转，有时候可能需要尺度变换，那么就可以描述为：

Transform<float,3,Affine> T = Translation3f(p) * AngleAxisf(a,axis) * Scaling(s);

而 Affine3d T 是一个 4*4 齐次矩阵变换。

旋转

Eigen::Quaternionf quater;
  可以采用沿着某一个轴进行计算，  
Eigen::AngleAxisd(M_PI/2.0，Eigen::Vector3d::UnitZ());
  也可以直接给定参数  
quater.x() = 0;
quater.y() = 0;
quater.z() = sin(M_PI/2.0 / 2.0);
quater.w() = cos(M_PI/2.0 / 2.0);

平移

Eigen::Translation3f translation(x,y,z);

尺度缩放

Scaling(sx, sy)
Scaling(sx, sy, sz)
Scaling(s)
Scaling(vecN)

仿射变换

//仿射变换矩阵
Eigen::Affine3f affine3f = translation*quater.toRotationMatrix();

//求逆矩阵
affine3f = affine3f.inverse();

Eigen::Affine3f 和 Eigen::Matrix4f 的转换

Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;