SE-Sync

一個實驗效果比g2o快一個數量級時間的后端優化算法。
核心思想：通過解析后端優化問題的低階的幾何圖論結構，將問題等價簡化為一個低維度的基於黎曼流型的優化問題（天書）。同時提出一個黎曼流截斷的牛頓致信域算法來解這個問題。
資源地址：項目地址，期刊論文，技術報告

---

問題表示

SE3 模型的（論文中把優化問題稱為同步？）的最大似然估計。

可以看到此優化模型只優化位姿，優化變量為旋轉\(R_i\)和平移\(t_i\)。而相對位姿變換的估計值為\(\tilde{R}_{ij}, \tilde{t}_{ij}\)，在這里是固定的。而在實際工程中，這些估計值就是里程計/前端/跟蹤模塊給出的各個幀的相對位姿。

(12)公式里還出現兩個參數\(\kappa_{ij},\zeta_{ij}\)。前者是(10)公式中，里程計給出的相對位姿中平移分量的誤差所服從的高斯分布的方差逆，越大越好，說明方差更小估計更准。后者中，文章把里程計的旋轉誤差建模為朗之萬分布，均值為單位矩陣，聚集參數為\(\kappa_{ij}\)。（其實論文的附錄A中就有）。

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\)是一回事，在實際運用中都是3。

模型推導與簡化

接下來就是將問題一步步推導到黎曼優化問題的表現形式上

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。
• 求解平移分量。
• 返回最優解。
  在Od中的解不一定是個旋轉矩陣，所以在解問題9的第二步中，對每個解得的元素，要求最近的旋轉。

初步分析（快在哪）

理論上快在哪

要說SESync快在哪里，可以先分析一下BA問題。在BA中，只優化相機位姿的話，由於一個位姿可能和多個位姿構成約束，於是這部分稠密的。高斯牛頓+圖優化也只能硬解這一部分，即要對一個\(nd\times nd\)的矩陣求逆/各種分解。而SE-Sync從形式上看直接解的是\(Y\)，只要在解\(Y\)時沒有干這么復雜的事（高大上的截斷的牛頓黎曼置信域法），那速度必然是由於直接矩陣分解求方程組的。

和圖優化的結合

g2o、ceres中突出一個舒爾補，將特征點提前邊緣化。而邊緣化可以看做是對稠密部分額外加殘差。也許可以直接加在problem1上，再推導一波，形成可以優化地圖點的算法？可以想一想！