位姿图的L1-norm优化

研究谭平老师新开源的GSLAM的小伙伴都知道，GSLAM内部有一个1DSFM的方法，这个方法对outliers的容忍度较高，对鲁棒性有一定的帮助。

从解码说起

陶哲轩在2004发表的论文Decoding by Linear Programming给出了一个L1优化(1范数优化)的求解方式，并证明了在扰动量e不大的时候，y = Af + e在y和A已知的条件下，可以通过L1优化求出唯一解f，并给出了L1优化的求解方法：线性规划
问题描述为：y = Af +e，已知y和A的确定值，且e是一个不大的量，求解信号f
该问题等价为求解一个优化问题：
f =\min_{g} || y - Ag||_{\mathcal{l}_1}
等价为：
\min 1^t , -t \le y - Ag \le t
其中t和g都是优化变量，最终结果为f = g^*。

Rotation Averaging

2013年的iccv上Avishek Chatterjee发表了一篇使用L1优化旋转的论文: Efficient and Robust Large-Scale Rotation Averaging，该论文声称：由于R_{ij} = R_jR_i^{-1}(虽然我很讨厌这个符号记法就是了，这里和原文统一也就这么记一下)，因此对应的旋转向量\omega_{ij}, \omega_j, \omega_i有这样一个关系：
\omega_{ij} = \omega_j - \omega_i

一点高阶量都没有的公式

于是把上式写成矩阵形式：
\mathbf{A}\boldsymbol{\omega} = [\cdots -\mathbf{I} \cdots \mathbf{I} \cdots ]\boldsymbol{\omega} = \mathbf{b}
其中\mathbf{b}_{ij} =\omega_{ij}, \boldsymbol{\omega}_i = \omega_i
于是这个问题就可以使用L1优化的方式求解。

Global Structure-from-Motion

之后，谭平对该方法进行了推广，提出了Global SFM的方法，论文参考Global Structure-from-Motion by Similarity Averaging，把仅对旋转的优化，拓展到了对\mathbf{Sim}(3)上，具体做法为：
先对旋转，然后对尺度的对数，最后对平移，公式我就不一一列举了，具体求解使用了clp求解器。

Clp的使用

clp求解器是一个线性规划求解器，它可以求解形如：
\begin{array}{c} \min c^Tx \\ row_{lower_bound} \le Ax \le row_{upper_bound} \\ col_{lower_bound} \le x \le col_{upper_bound} \end{array}
这样的问题。
使用它的时候，需要对上述方程进行处理，就以旋转为例：
\min || \mathbf{A}\boldsymbol{\omega} - \mathbf{b} ||_{\mathcal{l}_1}
根据Decoding by Linear Programming的结论，转化为：
\begin{array}{c} \min \mathbf{1}^T \mathbf{e} \\ -\mathbf{e} \le \mathbf{A}\boldsymbol{\omega} - \mathbf{b} \le \mathbf{e} \end{array}
该问题等价为：
\begin{array}{c} \min \mathbf{1}^T \mathbf{e} + \mathbf{0}^T \boldsymbol{\omega} \\ \mathbf{b} \le \mathbf{A}\boldsymbol{\omega} + \mathbf{e} \\ \mathbf{A}\boldsymbol{\omega} - \mathbf{e} \le \mathbf{b} \end{array}

重写一下矩阵，将两个不等式拼起来，\mathbf{e}和\boldsymbol{\omega}拼成参数\mathbf{x}有：
\begin{array}{c} [\mathbf{1}^T, \mathbf{0}^T]\mathbf{x} \\ \left[\begin{array}{c} \mathbf{b} \\ -\inf \end{array}\right] \le \left[ \begin{array}{cc} \mathbf{I} & \mathbf{A} \\ -\mathbf{I} & \mathbf{A} \end{array} \right]\mathbf{x} \le \left[\begin{array}{c} \inf \\ \mathbf{b} \end{array}\right] \end{array}

即为clp的标准式，之后无脑使用求解器即可，具体代码可以参考GSLAM项目下GlobalRotationEstimation.cpp文件的GlobalRotationEstimation::solve函数