SLAM基础问题总结（2）

1、什么是对极约束？基础矩阵F的推导过程。

简单来讲：就是图像$I_1$中特征点P1，其对应的特征匹配点在图像$I_2$的极线L2上。
但是，由于我们通过特征点匹配，确定了p2的像素位置，所以能够推断P的空间位置，以及相机的运动。这都是多亏了正确的特征匹配。如果没有特征匹配，我们就没法确定 p2到底在极线的哪个位置了，那时，就必须在极线上搜索以获得正确的匹配，非常耗时。

基本矩阵的推导：
在第一帧的坐标系下，设 P 的空间位置为：$\mathbf{P}=[X,Y,Z{]}^T$
根据针孔相机模型，我们知道两个像素点 p1 ,p2 的像素位置为：$s_1{\mathbf{p}}_1=\mathbf{K}\mathbf{P},s_2{\mathbf{p}}_2=\mathbf{K}(\mathbf{R}\mathbf{P}+\mathbf{t})$

取：$${\mathbf{x}}_1={\mathbf{K}}^{-1}{\mathbf{p}}_1,{\mathbf{x}}_2={\mathbf{K}}^{-1}{\mathbf{p}}_2$$这里的 x1 ,x2 是两个像素点的归一化平面上的坐标。归一化尺度代入上式，得：$${\mathbf{x}}_2=\mathbf{R}{\mathbf{x}}_1+\mathbf{t}$$两边同时左乘${\mathbf{t}}^{\wedge }$。然后，两侧同时左乘${\mathbf{x}}_2^T$
$${\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }{\mathbf{x}}_2={\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1$$${\mathbf{t}}^{\wedge }{\mathbf{x}}_2$是一个与 t 和 x2 都垂直的向量。把它再和 x2 做内积时，将得到 0。
$${\mathbf{x}}_2^T{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$$重新代入 p1 ,p2 ，有：$${\mathbf{p}}_2^T{\mathbf{K}}^{-T}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1=0$$这两个式子都称为对极约束。中间部分记作两个矩阵：基础矩阵（Fun-damental Matrix）F 和本质矩阵（Essential Matrix）E，可以进一步简化对极约束：$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R},\mathbf{F}={\mathbf{K}}^{-T}\mathbf{E}{\mathbf{K}}^{-1},{\mathbf{x}}_2^T\mathbf{E}{\mathbf{x}}_1={\mathbf{p}}_2^T\mathbf{F}{\mathbf{p}}_1=0$$对极约束简洁地给出了两个匹配点的空间位置关系。
2、RANSAC的过程及在基础矩阵F求解上的应用，代码实现一个例子
RANSAC：拟合直线，减少误匹配。
RANSAC算法的输入是一组观测数据（往往含有较大的噪声或无效点），一个用于解释观测数据的参数化模型以及一些可信的参数。RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

• 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
• 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
• 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
• 然后，用所有假设的局内点去重新估计模型（譬如使用最小二乘法），因为它仅仅被初始的假设局内点估计过。
• 最后，通过估计局内点与模型的错误率来评估模型。

上述过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。
整个过程可参考下图：
在模型确定以及最大迭代次数允许的情况下，RANSAC总是能找到最优解。
RANSAC很好的例子是在ORB-SLAM的单目初始化中：

......
    // 匹配上的特征点的个数
    const int N = mvMatches12.size();

    // Indices for minimum set selection
    // 新建一个容器vAllIndices，生成0到N-1的数作为特征点的索引
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

    // Generate sets of 8 points for each RANSAC iteration
    // 步骤2：在所有匹配特征点对中随机选择8对匹配特征点为一组，共选择mMaxIterations组
    // 用于FindHomography和FindFundamental求解
    // mMaxIterations:200

    ///< 二维容器，外层容器的大小为迭代次数，内层容器大小为每次迭代算H或F矩阵需要的点
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    DUtils::Random::SeedRandOnce(0);

    for(int it=0; it<mMaxIterations; it++)
    {
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            // 产生0到N-1的随机数
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            // idx表示哪一个索引对应的特征点被选中
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;

            // randi对应的索引已经被选过了，从容器中删除
            // randi对应的索引用最后一个元素替换，并删掉最后一个元素
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

    // Launch threads to compute in parallel a fundamental matrix and a homography
    // 步骤3：调用多线程分别用于计算fundamental matrix和homography
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;   //经过重投影误差筛选之后的匹配点
    float SH, SF; // score for H and F
    cv::Mat H, F; // H and F

    // ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
    // 计算homograpy并打分
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    // 计算fundamental matrix并打分
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

    // Compute ratio of scores
    // 步骤4：计算得分比例，选取某个模型
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 步骤5：从H矩阵或F矩阵中恢复R,t
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;

3、求解BA问题用的最多的G2o，说明G2o的顶点和边代表的含义以及类型

图优化，是把优化问题表现成图（Graph）的一种方式。这里的图是图论意义上的图。一个图由若干个顶点（Vertex），以及连接着这些节点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项，g2o 为 SLAM 提供了图优化所需的内容，节点为优化变量，边为误差项。**
4、GN和LM的迭代过程及优劣
Gauss-Newton迭代方程：
$$\mathbf{J}(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x})\Delta \mathbf{x}=-\mathbf{J}(\mathbf{x}{)}^{T}f(\mathbf{x})$$注意，我们要求解的变量是 ∆x，因此这是一个线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)。我们把左边的系数定义为 H，右边定义为 g，那么上式变为：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$$Gauss-Newton 的算法步骤可以写成：

1. 给定初始值${\mathbf{x}}_0$。
2. 对于第 k 次迭代，求出当前的雅可比矩阵$\mathbf{J}\left({\mathbf{x}}_k\right)$和误差。
3. 求解增量方程：$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$.
4. 若 $\Delta x_k$足够小，则停止。否则，令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回 2.

Gauss-Newton缺点：在使用 Gauss Newton 方法时，可能出现 ${\mathbf{J}}^T\mathbf{J}$为奇异矩阵或者病态 (ill-condition) 的情况，此时增量的稳定性较差，导致算法不收敛。

Levenberg-Marquadt：
由于 Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给 ∆x 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法 (Trust Region Method)。
$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x})\Delta \mathbf{x}}$$ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

Levenberg-Marquadt迭代过程：

1. 给定初始值${\mathbf{x}}_0$，以及初始优化半径 µ。
2. 对于第 k 次迭代，求解：
   $$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2\le \mu$$ 这里 µ 是信赖区域的半径。
3. 计算 ρ。
4. 若 ρ >3/4 则 µ = 2µ；
5. 若 ρ <1/4 则 µ = 0.5µ；
6. 如果 ρ 大于某阈值，认为近似可行。令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$ 。
7. 判断算法是否收敛。如不收敛则返回 2，否则结束。

在 Levenberg 提出的优化方法中，把 D 取成单位阵 I，相当于 直接把 ∆x 约束在一个球中。随后，Marqaurdt 提出将 D 取成非负数对角阵——实际中 通常用 ${\mathbf{J}}^T\mathbf{J}$的对角元素平方根，使得在梯度小的维度上约束范围更大一些。 用 Lagrange 乘子将LM优化转化为一个无约束优化问题：
$$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\Vert \mathbf{D}\Delta \mathbf{x}{\Vert }^2$$这里 λ 为 Lagrange 乘子。把它展开后，我们发现该问题的核心仍是计算增量的线性方程：
$$\left(\mathbf{H}+\lambda {\mathbf{D}}^T\mathbf{D}\right)\Delta \mathbf{x}=\mathbf{g}$$如果考虑它的简化形式，即 D = I，那么相当于求解：
$$(\mathbf{H}+\lambda \mathbf{I})\Delta \mathbf{x}=\mathbf{g}$$Levenberg-Marquadt优点：
当参数 λ 比较小时，H 占主要地位，这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于 G-N 法。另一方面，当 λ 比较大时，λI 占据主要地位，L-M更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 ∆x。
5、解释图优化中图的稀疏性，边缘化过程及可能存在的问题，解释鲁棒核函数。
稀疏性：
在以图优化框架的视觉 SLAM 算法里，BA 起到了核心作用。它类似于求解只有观测方程的 SLAM 问题。
在整体 BA 目标函数上，把自变量定义成所有待优化的变量：
$$\mathbf{x}={\left[{\mathbf{\xi }}_1,\dots ,{\mathbf{\xi }}_m,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_n\right]}^T$$相应的，增量方程中的 ∆x 则是对整体自变量的增量。在这个意义下，当我们给自变量一个增量时，目标函数变为：
$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{p}}_j\Vert }^2$$其中 表示整个儿代价函数在当前状态下对相机姿态的偏导数，而 表示该函数对路标点位置的偏导。
把相机位姿变量放在一起：
$${\mathbf{x}}_c={\left[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2,\dots ,{\mathbf{\xi }}_m\right]}^T\in {\mathbb{R}}^{6m}$$并把空间点的变量也放在一起：
$${\mathbf{x}}_p={\left[{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_n\right]}^T\in {\mathbb{R}}^{3n}$$
因此有：$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2=\frac{1}{2}{\Vert \mathbf{e}+\mathbf{F}\Delta {\mathbf{x}}_c+\mathbf{E}\Delta {\mathbf{x}}_p\Vert }^2$$无论使用 G-N 还是 L-M 方法，最后都将面对增量线性方程：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$$以 G-N 为例，则 H 矩阵为：
$$\mathbf{H}={\mathbf{J}}^T\mathbf{J}=\left[\begin{array}{cc}{\mathbf{F}}^T\mathbf{F}& {\mathbf{F}}^T\mathbf{E}\\ {\mathbf{E}}^T\mathbf{F}& {\mathbf{E}}^T\mathbf{E}\end{array}\right]$$SLAM 的一个重要进展是认识到了矩阵 H 的稀疏结构，并发现该结构可以自然、显式地用图优化来表示。假设一个场景内有 2 个相机位姿 (C 1 ,C 2 ) 和 6 个路标 (P 1 ,P 2 ,P 3 ,P 4 ,P 5 ,P 6 )。

可以推出该场景下的 BA 目标函数应该是：
$$\frac{1}{2}\left({\Vert e_{11}\Vert }^2+{\Vert e_{12}\Vert }^2+{\Vert e_{13}\Vert }^2+{\Vert e_{14}\Vert }^2+{\Vert e_{23}\Vert }^2+{\Vert e_{24}\Vert }^2+{\Vert e_{25}\Vert }^2+{\Vert e_{26}\Vert }^2\right)$$这里的eij 使用之前定义过的代价函数。以 e11 为例，它描述了在C1 看到了 P1 这件事，与其他的相机位姿和路标无关。不难看出 e11对相机变量 ξ2 和路标点 p2 ,…,p6 的偏导都为 0。
$${\mathbf{J}}_{11}=\frac{\partial {\mathbf{e}}_{11}}{\partial \mathbf{x}}=\left(\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{\xi }}_1},0_{2\times 6},\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{p}}_1},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3}\right)$$为了方便表示稀疏性，我们用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为 0。那么上面的 J11 则可以表示成图 10-5 的图案。同理，其他的雅可比矩阵也会有类似的稀疏图案。

为了得到该目标函数对应的雅可比矩阵，我们可以将这些 J ij 按照一定顺序列为向量，那么整体雅可比矩阵以及相应的 H 矩阵的稀疏情况就是图 10-6 中所展示的那样。

对于 H 矩阵当中处于非对角线的矩阵块来说，如果该矩阵块非零，则其位置所对应的变量之间会在图中存在一条边，我们可以从图 10-7 中清晰地看到这一点。所以，H 矩阵当中的非对角部分的非零矩阵块可以理解为它对应的两个变量之间存在联系，或者可以称之为约束。

有 m 个相机位姿，n 个路标点。由于通常路标数量远远会比相机多，于是有 n ≫ m。由上面推理可知，实际当中的 H 矩阵会像图 10-8 所示的那样。它的左上角块显得非常小，而右下角的对角块占据了大量地方。除此之外，非对角部分则分布着散乱的观测数据。由于它的形状很像箭头，又称为箭头形（Arrow-like）

边缘化：
对于具有这种稀疏结构的 H，利用 H 的稀疏性加速计算：Schur 消元 (Schur trick)。在 SLAM 研究中亦称为 Marginalization（边缘化）。
对应的线性方程组也可以由 H∆x = g 变为如下形式：
$$\left[\begin{array}{ll}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^{\mathbf{T}}& \mathbf{C}\end{array}\right]\left[\begin{array}{l}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{l}\mathbf{v}\\ \mathbf{w}\end{array}\right]$$消去右上角的非对角部分 E，得：
$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right]$$经过消元之后，第一行方程组变成和 ∆xp 无关的项。单独把它拿出来，得到关于位姿部分的增量方程：
$$\left[\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^T\right]\Delta {\mathbf{x}}_c=\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}$$这个线性方程组的维度和 B 矩阵一样。我们的做法是先求解这个方程，然后把解得的 ∆xc 代入到原方程，然后求解 ∆xp 。这个过程称为 Marginalization，或者 Schur消元。我们将此方程的系数记为S：


S 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时候称为共视（Co-visibility）。反之，如果该块为零，则表示这两个相机没有共同观测
核函数：
在前面的 BA 问题中，我们最小化误差项的二范数平方和，作为目标函数。这种做法虽然很直观，但存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误的，把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据。这时，算法会看到一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所以，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。
于是就有了核函数的存在。核函数保证每条边的误差不会大的没边，掩盖掉其他的边。鲁棒核函数有许多种，例如最常用的 Huber 核：
$$H(e)=\{\begin{array}{ll}\frac{1}{2}{e}^2& \text{ if }|e|\le \delta \\ \delta \left(|e|-\frac{1}{2}\delta \right)& \text{ otherwise }\end{array}$$当误差 e 大于某个阈值 δ 后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。同时，Huber 核函数又是光滑的，可以很方便地求导。

除了 Huber 核之外，还有 Cauchy 核，Tukey 核等等。