SE-Sync

一个实验效果比g2o快一个数量级时间的后端优化算法。
核心思想：通过解析后端优化问题的低阶的几何图论结构，将问题等价简化为一个低维度的基于黎曼流型的优化问题（天书）。同时提出一个黎曼流截断的牛顿致信域算法来解这个问题。
资源地址：项目地址，期刊论文，技术报告

---

问题表示

SE3 模型的（论文中把优化问题称为同步？）的最大似然估计。

可以看到此优化模型只优化位姿，优化变量为旋转\(R_i\)和平移\(t_i\)。而相对位姿变换的估计值为\(\tilde{R}_{ij}, \tilde{t}_{ij}\)，在这里是固定的。而在实际工程中，这些估计值就是里程计/前端/跟踪模块给出的各个帧的相对位姿。

(12)公式里还出现两个参数\(\kappa_{ij},\zeta_{ij}\)。前者是(10)公式中，里程计给出的相对位姿中平移分量的误差所服从的高斯分布的方差逆，越大越好，说明方差更小估计更准。后者中，文章把里程计的旋转误差建模为朗之万分布，均值为单位矩阵，聚集参数为\(\kappa_{ij}\)。

The Theory of Concentrated Langevin Distributions* 对朗之万分布有比较详细的描述

在SE3优化模型中，里程计估计的相对旋转\(R_{ij}\)是一个旋转矩阵，任意一个旋转矩阵可以将\(x_{latent}=(1,0,0)^T\)旋转到一特定位置。而这个位置可以是一个在球面上的概率分布，可以由包含均值和方差的分布来描述。而点和旋转矩阵是一一映射的，也就描述了旋转矩阵的分布，即某个旋转位置的概率密度是多大。
\(R \sim Langevin(R_\mu,\kappa)\)中，\(Rx_{latent}\)可以指被旋转的在（1.1）中的随机矢量\(x\)，而\(R_\mu x_{latent}\)就是均值\(\mu\)。而\(\kappa=0\)则分布为均匀分布，\(\kappa\)越大则分布越集中。而(1.1)前面的a(k)是一个保证概率积分为1的常量。注意SESync中的\(I_d\)中的\(d\)和(1.1)中的\(q\)是一回事，在实际运用中都是35。

模型推导与简化

接下来就是将问题一步步推导到黎曼优化问题的表现形式上

1 -> 2

问题2与问题1相比，使用旋转向量化和图的邻接矩阵（拉普拉斯）来表示问题1。

2 -> 3

如果问题2的旋转部分已知，那么可以解析地求出平移部分。而问题3就是只有旋转部分的问题。

3 -> 4

进一步公式矩阵化。

4 -> 5

在自变量矩阵（旋转矩阵）在Od（正交，但模长没有限制）定义域上的一定优于SOd（正交，模长为1）。比如全国找最高的人的身高，一定大于等于陕西省的。

5 -> 6

到5这里，分析了一下拉格朗日对偶问题。

6 -> 7

得到对偶问题的对偶问题。
TBD：拉格朗日乘子法，好久前的知识点了。要考察对偶问题的对偶问题与原问题的关系？。到问题6，就可以找到对于原问题旋转部分的松弛解。

7 -> 8

对于问题7，正常情况下（有误差时），\(Z^*\) 是一个秩为\(r\)的矩阵，这个\(r\)略微大于\(d\)。于是就求一个低纬度的矩阵\(Y\)。

8 -> 9

在8中\(Y_i^TY_i=I_d\)，根据这个性质，\(Y_i \in St(d,r)\)，即\(Y_i\)是施泰福流型中的元素。可以用问题9来表示。

解问题9

总体分三步

• 首先用截断的牛顿黎曼置信域法来解得\(Y\)，具体的方法，又是几篇论文。
• 在从\(Y_TY\)中解得所有\(R\)，为何\(R\)不是\(Y\)呢，注意\(Y\)的秩是\(r\)，而\(R\)的秩是\(d\)。这里返回R。
• 求解平移分量。
• 返回最优解。

初步分析（快在哪）

理论上快在哪

要说SESync快在哪里，可以先分析一下BA问题。在BA中，只优化相机位姿的话，由于一个位姿可能和多个位姿构成约束，于是这部分稠密的。高斯牛顿+图优化也只能硬解这一部分，即要对一个\(nd\times nd\)的矩阵求逆/各种分解。而SE-Sync从形式上看直接解的是\(Y\)，只要在解\(Y\)时没有干这么复杂的事（高大上的截断的牛顿黎曼置信域法），那速度必然是由于直接矩阵分解求方程组的。

和图优化的结合

g2o、ceres中突出一个舒尔补，将特征点提前边缘化。而边缘化可以看做是对稠密部分额外加残差。也许可以直接加在problem1上，再推导一波，形成一个在基于滑窗的前端中都能用的算法。可以想一想！