SLAM基础问题总结（1）

1、如何对匹配好的点做进一步的处理，更好保证匹配效果
（1）确定匹配最大距离，汉明距离小于最小距离的两倍
（2）使用KNN-matching算法，令K=2。则每个match得到两个最接近的descriptor，然后计算最接近距离和次接近距离之间的比值，当比值大于既定值时，才作为最终match。
（3）RANSAC（使用RANSAC找到最佳单应性矩阵。由于这个函数使用的特征点同时包含正确和错误匹配点，因此计算的单应性矩阵依赖于二次投影的准确性）
以上方法均为传统的特征匹配筛选算法：https://www.cnblogs.com/Jessica-jie/p/8622449.html
（4）在ORB-SLAM中，根据图像的连续性，使用网格化与图像金字塔的方式，实现ORB特征点之间的的特征匹配。虽然匹配效果较好，但是速度较慢，且匹配的特征点数量较少。最后还结合和RANSAC算法，估计H或F矩阵，实现单目的初始化。
（5）GMS（https://github.com/JiawangBian/GMS-Feature-Matcher）是目前速度最快，效果最好的一种特征匹配算法。通过匹配的特征点周围特征匹配的数量，构建概率统计模型，获得正确的特征匹配。

2、单目相机，F和H矩阵有何不同，E和F矩阵有何不同，只旋转不平移能不能求F，只旋转不平移能不能求H
（1）F矩阵描述的是点与线之间的关系，即空间点的极限约束。H矩阵描述的是图像中同一平面的点的对应关系。
（2）$\mathrm{E}={\mathrm{t}}^{\wedge }\mathrm{R}$， $F=K^{-T}E{K}^{-1}$ ，$\mathrm{H}=\mathrm{R}-{\mathrm{t}}^{\star }\mathrm{n}\mathrm{T}/\mathrm{d}$
（3）在相机只有旋转而没有平移的情况，此时t为0，E也将为0，导致无法求解R，这时可以使用单应矩阵H求旋转，但仅有旋转，无法三角化求深度。

3、什么是BA?推导相机投影模型的雅克比矩阵J，以及J的每一项代表的含义（视觉SLAM14讲）
BA的本质是一个优化模型，其目的是最小化重投影/光度误差，用于优化相机位姿和世界点。如图 7-12 所示，我们通过特征匹配，知道了 p1和 p2 是同一个空间点 P 的投影，但是我们不知道相机的位姿。在初始值中， P 的投影 p^2与实际的 p2 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

局部BA用于优化局部的相机位姿，提高跟踪的精确度；全局BA用于全局过程中的相机位姿，使相机经过长时间、长距离的移动之后，相机位姿还比较准确。BA是一个图优化模型，一般选择LM(Levenberg-Marquardt)算法并在此基础上利用BA模型的稀疏性进行计算；可以直接计算，也可以使用g2o或者Ceres等优化库进行计算。
BA优化的推导过程：
考虑 n 个三维空间点 P 和它们的投影 p，我们希望计算相机的位姿$\mathbf{R},\mathbf{t}$，它的李代数表示为 ξ。假设某空间点坐标为${\mathbf{P}}_i={\left[X_i,Y_i,Z_i\right]}^T$，其投影的像素坐标为${\mathbf{u}}_i={\left[u_i,v_i\right]}^T$。像素位置与空间点位置的关系如下：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$写成矩阵形式就是（K为相机内部参数）：
$$s_i{\mathbf{u}}_i=\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{P}}_i$$由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：
$${\xi }^{\ast }=\arg \underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n{\Vert u_i-\frac{1}{s_i}K\exp \left({\xi }^{\wedge }\right)P_i\Vert }_2^2$$以上是一个最小二乘问题，使用李代数，可以构建无约束的优化，很方便地通过 G-N, L-M 等优化算法进行求解。在使用 G-N 和 L-M 之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：
$$e(\mathbf{x}+\Delta \mathbf{x})\approx \mathbf{e}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$当 e 为像素坐标误差（2 维）， x 为相机位姿（6 维）时， J 将是一个 2 × 6 的矩阵。
首先，记变换到相机坐标系下的空间点坐标为 P ′，并且把它前三维取出来：
$${{\mathbf{P}}^{\prime }={\left(\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)}_{1:3}=\left[X^{\prime },Y^{\prime }\right],Z^{\prime }]}^T$$那么，相机投影模型相对于 P ′ 则为：
$$s\mathbf{u}=\mathbf{K}{\mathbf{P}}^{\prime }$$展开有：$$\left[\begin{array}{c}su\\ sv\\ s\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]$$化简：$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y$$求误差时，可以把这里的 u，v 与实际的测量值比较，求差。在定义了中间变量后，对 ξ^ 左乘扰动量 δξ，然后考虑 e 的变化关于扰动量的导数。利用链式法则，可以列写如下：
$$\frac{\partial e}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi \oplus \xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }$$上式第一项是误差关于投影点的导数：
$$\frac{\partial e}{\partial {\mathbf{P}}^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$而第二项为变换后的点关于李代数的导数：$$\frac{\partial (\mathbf{T}\mathbf{P})}{\partial \delta \mathbf{\xi }}=(\mathbf{T}\mathbf{P}{)}^{\odot }=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]$$取出了前三维：
$$\frac{\partial P^{\prime }}{\partial \delta \xi }=\left[I,-P^{\prime \prime }\right]$$将这两项相乘，就得到了 2 × 6 的雅可比矩阵J:
$$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^2}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，因为这是由于误差是由观测值减预测值定义的。另一方面，除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点 P 的导数。所幸这个导数矩阵相对来说容易一些。仍利用链式法则，有：$$\frac{\partial e}{\partial \mathbf{P}}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}$$第一项已在前面推导了，第二项，按照定义：$${\mathbf{P}}^{\prime }=\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}=\mathbf{R}\mathbf{P}+\mathbf{t}$$我们发现 P ′ 对 P 求导后只剩下 R。于是：$$\frac{\partial e}{\partial \mathbf{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\mathbf{R}$$于是，我们推导了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。
4、单目初始化的目的及两种方法
单目初始化的目的：构建单目系统的归一化尺度，获得初始的三维地图点。

在单目视觉中，我们对两张图像的 t 归一化，相当于固定了尺度。虽然我们不知道它的实际长度为多少，但我们以这时的 t 为单位 1，计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化。
在初始化之后，就可以用 3D-2D 来计算相机运动了。初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目 SLAM 有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。

单目初始化的两种方法：使用基本矩阵F（极限约束），或是单应性矩阵H（平面结构）进行初始化。

从 E 分解到 R; t 的过程中，如果相机发生的是纯旋转，导致 t 为零，那么，得到的E 也将为零，这将导致我们无从求解 R。不过，此时我们可以依靠 H 求取旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置（这将在下文提到），于是，另一个结论是， 单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将无法初始化。
5、三角测量的过程及代码实现，有哪些不确定性及如何提高三角测量的精度

按照对极几何中的定义，设 x 1 ,x 2 为两个特征点p1,p2的归一化坐标，那么它们满足：
$$s_1{\mathbf{x}}_1=s_2\mathbf{R}{\mathbf{x}}_2+\mathbf{t}$$如果我要算 s2，那么先对上式两侧左乘一个${\mathbf{x}}_1^{\wedge }$ ，得：$$s_1{\mathbf{x}}_1^{\wedge }{\mathbf{x}}_1=0=s_2{\mathbf{x}}_1^{\wedge }\mathbf{R}{\mathbf{x}}_2+{\mathbf{x}}_1^{\wedge }\mathbf{t}$$该式左侧为零，右侧可看成 s2 的一个方程，可以根据它直接求得 s2 。有了 s2，s1 也非常容易求出。于是，我们就得到了两个帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，我们估得的 R,t，不一定精确使式子为零，所以更常见做法求最小二乘解而不是零解。
在视觉SLAM14讲中有一个关于三角测量的代码实现，使用的是opencv中的cv::triangulatePoints函数。在ORB-SLAM中也有关于三角测量的代码，使用的是SVD奇异值分解，代码如下：

// Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
// x' = P'X  x = PX
// 它们都属于 x = aPX模型
//                         |X|
// |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
// |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
// |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
// 采用DLT的方法：x叉乘PX = 0
// |yp2 -  p1|     |0|
// |p0 -  xp2| X = |0|
// |xp1 - yp0|     |0|
// 两个点:
// |yp2   -  p1  |     |0|
// |p0    -  xp2 | X = |0| ===> AX = 0
// |y'p2' -  p1' |     |0|
// |p0'   - x'p2'|     |0|
// 变成程序中的形式：
// |xp2  - p0 |     |0|
// |yp2  - p1 | X = |0| ===> AX = 0
// |x'p2'- p0'|     |0|
// |y'p2'- p1'|     |0|

/**
 * @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
 *  * @param kp1 特征点, in reference frame
 * @param kp2 特征点, in current frame
 * @param P1  投影矩阵P1
 * @param P2  投影矩阵P２
 * @param x3D 三维点
 * @see       Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312
 */
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    // 在DecomposeE函数和ReconstructH函数中对t有归一化
    // 这里三角化过程中恢复的3D点深度取决于 t 的尺度，
    // 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
    // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变

    cv::Mat A(4,4,CV_32F);

    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);    
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();   //奇异值最小的为恢复的3D点
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);   //齐次坐标
}

三角测量的精度提高：

• 当平移很小时，像素上的不确定性将导致较大的深度不确定性。也就是说，如果特征点运动一个像素 δx，使得视线角变化了一个角度 δθ，那么测量到深度值将有 δd 的变化。从几何关系可以看到，当 t 较大时，δd将明显变小，这说明平移较大时，在同样的相机分辨率下，三角化测量将更精确。但是，在增大平移，会导致匹配失效；而平移太小，则三角化精度不够。

在SVO中，使用深度滤波器对估计的三维点进行深度估计。假设特征点服从高斯分布，并且对它不断地进行观测，在信息正确的情况下，我们就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。
6、描述PnP
PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。目前遇到的场景主要有两个，其一是求解相机相对于某2维图像/3维物体的位姿；其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。

• 在场景1中，我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机坐标系相对于世界坐标系(Twc)的位姿

• 在场景2中，通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换

两种情况本质上是相同的，都是基于已知3D点和对应的图像2D点求解相机运动的过程。
PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P[45]，直接线性变换（DLT），EPnP（Efficient PnP），UPnP等等）。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。
7、相机标定原理及实现过程，棋盘格有哪些好处
相机标定分为单目相机标定和双目相机标定，很多博客对此进行了详细的介绍：
https://www.cnblogs.com/zyly/p/9366080.html
https://www.cnblogs.com/rainbow70626/p/5514598.html
https://www.cnblogs.com/wangguchangqing/p/8335131.html
这些博客写的非常详细了，因此不再进行过多的描述。棋盘格标定板具有制作简单，精度高的好处。

8、一个SLAM学习的基础文章，讲的挺好，内容丰富
地址：https://mp.weixin.qq.com/s/26iApm5cys3wEEgPC0zUag