视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数
专栏系列文章如下：
视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客
实践：Sophus
Sophus的基本使用方法
Sophus 库是Strasdat 维护的 。Sophus 库支持SO(3) 和 SE(3)，此外还含有二维运动 SO(2),SE(2) 以及相似变换 Sim(3) 的内容。它是直接在 Eigen 基础上开发的，不需要安装额外的依赖库。可以直接从 GitHub 上获取 Sophus，在代码目录 slambook/3rdparty 下也提供了 Sophus 源代码。
slambook/ch4/useSophus.cpp
该演示程序分为两部分。前半部分介绍 SO(3) 上的操作，后半部分则为 SE(3)。演示了如何构造 SO(3),SE(3) 对象，对它们进行指数、对数映射，以及当知道更新量后，如何对李群元素进行更新。
编译：在 CMakeLists.txt 里添加以下几行：
# 为使用 sophus，需要使用 find_package 命令找到它
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable( useSophus useSophus.cpp )如： slambook/ch4/useSophus/CMakeLists.txt
注：find_package 命令是 cmake 提供的寻找某个库的头文件与库文件的指令。如果 cmake 能够找到它，就会提供头文件和库文件所在的目录的变量。在 Sophus 这个例子中，就是 Sophus_INCLUDE_ DIRS。基于模板的 Sophus 库和 Eigen 一样，是仅含头文件而没有源文件的。
例子：评估轨迹的误差
在实际工程中，经常需要评估一个算法的估计轨迹与真实轨迹的差异，来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得，而估计轨迹则是待评价的算法计算得到。
考虑一条估计轨迹Testi,i 和真实轨迹Tgt,i，其中 i = 1,··· ,N，那么可以定义一些误差指标来描述它们之间的差别。
误差指标有很多种，常见的有绝对轨迹误差（Absolute Trajectory Error, ATE），这是每个位姿李代数的均方根误差（Root-Mean-Squared Error, RMSE）。这种误差可以刻画两条轨迹的旋转和平移误差。仅考虑平移误差 [23]，可以定义绝对平移误差（Average Translational Error）。
因为从整条轨迹上来看，旋转出现误差后，随后的轨迹在平移上也会出现误差，所以两种指标在实际当中都适用。
除此之外，也可以定义相对的误差。例如考虑 i 时刻到 i + ∆t 时刻的运动，有相对位姿误差（Relative Pose Error, RPE）；同样地，亦可只取平移部分。
利用 Sophus 库，很容易实现这部分计算。
如：slambook/ch4/example/trajectoryError.cpp
在这个例子中，程序将读取 groundtruth.txt 和 estimated.txt 两条轨迹，计算误差，然后显示到 3D 窗口中。
程序依赖安装和运行：
安装pangolin，直接从https://github.com/gaoxiang12/slambook2.git
下载pangolin（或者slambook1）
需要安装一些依赖：
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 ..安装sophus库，直接github下载最新的（老版本的不兼容）
https://github.com/strasdat/Sophus.git
需要安装3.3以上的eigen，用ubuntu16.04 ros自带的eigen版本太低了。
可能还需要安装fmt库，去github下载并安装：
https://github.com/fmtlib/fmt.git
运行程序遇到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,
程序运行结果：
相似变换群与李代数
相似变换群 Sim(3)是在单目视觉中使用的，对应的李代数是sim(3)。双目 SLAM 或 RGB-D SLAM 没有用到。
如果在单目SLAM中使用 SE(3) 表示位姿，那么由于尺度不确定性与尺度漂移，整个 SLAM 过程中的尺度会发生变化，这在 SE(3) 中未能体现出来。因此，在单目情况下一般会显式地把尺度因子表达出来。用数学语言来说，对于位于空间的点p，在相机坐标系下要经过一个相似变换，而非欧氏变换：
在相似变换中，把尺度s表达了出来。它同时作用在p的3个坐标之上，对p进行了一次缩放。与 SO(3)、SE(3) 相似，相似变换亦对矩阵乘法构成群，称为相似变换群 Sim(3)：
同样地，Sim(3) 也有对应的李代数、指数映射、对数映射等。李代数 sim(3) 元素是一个7维向量ζ。它的前 6 维与 se(3) 相同，最后多了一项 σ。
关联Sim(3)和sim(3)的仍是指数映射和对数映射。指数映射为：
通过指数映射，能够找到李代数与李群的关系。对于李代数ζ，它与李群的对应关系为：
旋转部分和 SO(3) 是一致的。平移部分，在 se(3) 中需要乘一个雅可比J，而相似变换的雅可比更复杂一些。对于尺度因子，可以看到李群中的 s 即为李代数中 σ 的指数函数。
Sim(3) 的 BCH 近似与 SE(3) 是类似的。可以讨论一个点p经过相似变换Sp后，相对于S 的导数，存在微分模型和扰动模型两种方式，而扰动模型较为简单。设给予Sp左侧一个小扰动 exp(ζ∧)，并求Sp对于扰动的导数。因为Sp 是 4 维的齐次坐标，ζ 是 7 维向量，该导数应该是 4 × 7 的雅可比。
更详细的关于 Sim(3) 的资料，参见文献 [24]：H. Strasdat, Local accuracy and global consistency for efficient visual slam. PhD thesis, Citeseer, 2012.
小结
这讲引入了李群 SO(3) 和 SE(3)，以及它们对应的李代数 so(3) 和 se(3)。介绍了位姿在它们上面的表达和转换，然后通过 BCH 的线性近似，就可以对位姿进行扰动并求导了。这给讲解位姿的优化打下了理论基础，因为需要经常地对某一个位姿的估计值进行调整，使它对应的误差减小。
在实际应用中，可以使用 SO(3) 加上平移的方式来代替 SE(3)，从而回避一些雅可比的计算。
习题
- 验证 SO(3)、SE(3) 和 Sim(3) 关于乘法成群。
- 验证 (R3,R,×) 构成李代数。
- 验证 so(3) 和 se(3) 满足李代数要求的性质。
- 证明：Rp∧RT = (Rp)∧.
- 证明：R exp(p∧) RT = exp((Rp)∧).
该式称为 SO(3) 上的伴随性质。同样地，在 SE(3) 上亦有伴随性质：
T exp(ξ∧)T−1 = exp((Ad(T)ξ)∧)
6. 仿照左扰动的推导，推导 SO(3) 和 SE(3) 在右扰动下的导数。
7. 搜索 cmake 的 find_package 指令是如何运作的。它有哪些可选的参数？为了让 cmake 找到某个库，需要哪些先决条件？
习题感兴趣的可以去做一下。