SLAM后端PGO优化中代价函数的距离度量

总结几种在PGO优化的时候采用的几种距离度量方式，先介绍定义，具体后续实际表现还需要再多看多读。

对变换矩阵SE3来说

Geodesic Distance

PGO优化问题采用极大似然估计的方式表示，可以将位姿估计问题转化为最小二乘估计问题，如下所示：
$${\mathcal{X}}^{\ast }=\underset{\mathcal{X}\in SE(3)}{\mathrm{argmax}}\prod _{(i,j)\in \mathcal{E}}p(Z_{ij}|\mathcal{X})$$
$$\Downarrow$$
$${\mathcal{X}}^{\ast }=\underset{\mathcal{X}\in SE(3{)}^N}{\mathrm{argmin}}\underset{(i,j)\in \mathcal{E}}{\Sigma }{r}_{ij}(\mathcal{X})$$
$$\Downarrow$$核心就是代价函数如何构建。
$${\mathbf{r}}_{i,j}(\mathcal{X})=\frac{1}{2}\Vert \log (Z_{ij}^{-1}{\tilde{Z}}_{ij}(\mathcal{X})){\Vert }_{{\Sigma }_{ij}}^2$$
这里的$r_{ij}$就可以被称为是Geodesic 距离或者Geodesic cost

• log是李群李代数相关转换需要的对数映射，存在奇异性

Chordal distance

表示成chord距离就是
将原来的z（因为是PGO，因此Z也是SE3的元素）：
$$Z=\left[\begin{array}{cc}R& t\\ 0_{1\times 3}& 1\end{array}\right]\in SE(3)$$
$$\Downarrow$$转换成
$$Z=\left[\begin{array}{cc}\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]& t\\ 0_{1\times 3}& 1\end{array}\right]\in SE(3)$$
将z展开（flat操作）变成：
$$\mathbf{z}=\mathrm{flat}(\mathbf{Z})=\left(\begin{array}{c}{\mathbf{r}}_1\\ {\mathbf{r}}_2\\ {\mathbf{r}}_3\\ \mathbf{t}\end{array}\right)\in {\mathbb{R}}^{12}$$
其中
$${\mathbf{\Sigma }}^{[c]}=\mathbf{J}\mathbf{\Sigma }{\mathbf{J}}^T$$
这样极大似然估计中每个概率密度函数就可以表示成：
$$p\left({\mathbf{Z}}_{ij}\mid \mathcal{X}\right)\propto \exp \left(-\frac{1}{2}{\Vert flat\left({\tilde{\mathbf{Z}}}_{ij}(\mathcal{X})\right)-\mathrm{flat}\left({\mathbf{Z}}_{ij}\right)\Vert }_{{\Sigma }_{ij}^{[c]}}^2\right)$$
那么对应的代价函数就可以写成如下所示：
$${\mathbf{r}}_{ij}(\mathcal{X})=\frac{1}{2}\Vert \mathrm{flat}({\tilde{\mathbf{Z}}}_{ij}(\mathcal{X}))-\mathrm{flat}\left({\mathbf{Z}}_{ij}\right){\Vert }_{{\Sigma }_{ij}^{[c]}}^2$$

• 不同的度量函数目的在于使得非线性优化的代价函数的非凸性程度降低，加速迭代求解时的收敛速度，最终的结果可能是差不多的。
• 可能非凸性程度降低之后更容易收敛到全局最优。

对旋转矩阵SO3来说

Angular Distance

The rotation angle of $R_2^T{R}_1$:
$$d_{ang}(R_1,R_2)=si{n}^{-1}\Vert \Vert =|\theta |$$
$\theta$ is the rotation vector corresponding to the rotation matrix $R_2^T{R}_1$.

Chordal Distance

The Frobenius norm of the matrix $R_1-R_2$:
$$d_{chord}=(R_1,R_2)=\Vert R_1-R_2{\Vert }_F$$
就是对应列元素直接相减

• 就是geodesic distance中旋转矩阵对应的那部分。
• chord distance也是欧几里得距离中的一种。

补充

• Chord在图论中指的是任意两个不邻接的节点之间的连线。

TODO

• 实际SLAM中的表现

Reference

• Nasiri S M, Hosseini R, Moradi H. Novel parameterization for gauss–newton methods in 3-D pose graph optimization[J]. IEEE Transactions on Robotics, 2020, 37(3): 780-797.
• Aloise I, Grisetti G. Chordal based error function for 3-D pose-graph optimization[J]. IEEE Robotics and Automation Letters, 2019, 5(1): 274-281.
• Guadagnino T, Di Giammarino L, Grisetti G. HiPE: Hierarchical Initialization for Pose Graphs[J]. IEEE Robotics and Automation Letters, 2021, 7(1): 287-294.
• Carlone L, Tron R, Daniilidis K, et al. Initialization techniques for 3D SLAM: a survey on rotation estimation and its use in pose graph optimization[C]//2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015: 4597-4604.