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

专栏系列文章如下： 

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

相似、仿射、射影变换

除了欧氏变换之外，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​cmake.org/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 中实现。