SLAM基础知识

视觉前端

特征点法

ORB特征

基本原理

ORB算法将FAST特征点的检测方法和BRIEF特征描述子结合起来，并进行改进和优化。

算法步骤

1. 利用FAST检测特征点，然后利用Harris角点的度量方法，从FAST特征点中挑选除Harris角点响应值最大的N个特征点。其中响应函数为
   $$R=det(M)-k(trace(M){)}^2$$
   ORB中用NMS来筛选关键点，只保留分数最大的前n个关键点。由于FAST关键点不具备尺度不变性，因此ORB采用了图像金字塔来实现尺度不变性，通过构建高斯金字塔，然后在每一层金字塔图像上检测角点。

2. 利用灰度质心法实现BRIEF描述子的旋转不变性。
   灰度质心法认为角点的灰度和质心之间存在一个偏移，这个向量可以用于表示方向。对于任意特征点p，p的领域像素的矩定义为：
   $$m_{ij}=\sum _{x=-r}^r\sum _{y=-r}^r{x}^i{y}^{j}I(x,y)$$
   其中$I(x,y)$为点$(x,y)$处的灰度值，$q$为质心,$i,j=0,1$。那么我们可以得到图像的质心为：
   $$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$$
   那么特征点与质心的夹角定义为FAST特征点的方向：
   $$\theta =arctan(m_{01},m_{10})$$
   BRIEF描述子是通过比较中心像素点p的任意邻域内的256对像素间的差值来生成二进制描述子，其结果是一个长度为256的二值码串，由特征点邻域内的256个点对组成，这$2\times 256$个点$(x_i,y_i),i=1,2,.....,2n$组成一个矩阵$S$：
   $$S=\left[\begin{array}{cccc}{x}_1& x_2& ...& x_{2n}\\ y_1& y_2& ...& y_{2n}\end{array}\right]$$
   ORB使用邻域方向$\theta$和对应的旋转矩阵$R_{\theta }$，构建$S$的校正版本$S_{\theta }$:
   $$S_{\theta }=R_{\theta }S$$
   其中，
   $$R_{\theta }=\left[\begin{array}{cc}cos\theta & sin\theta \\ -sin\theta & cos\theta \end{array}\right]$$
   $\theta$就是上面求得的FAST特征点的方向。

   对于任意一对像素点$x_i,y_i$，若$x_i<y_i$，则该位置的描述子为1，否则为0。

3. rBRIEF解决描述子的区分性。

光流法

光流约束方程

光流基于如下两个假设：

1. 在很短的时间内目标的像素值不会变化；
2. 邻近的像素具有相似的运动。

令$I(x,y,t)$表示t时刻的像素点$(x,y)$的灰度值，则根据亮度恒定假设，我们有：
$$I(x,y,t)=I(x+\Delta x,y+\Delta y,t+\Delta t)$$
我们对上式右侧进行一阶Taylor展开，可得：
$$I(x+\Delta x,y+\Delta y,t+\Delta t)\approx I(x,y,t)+\frac{\partial I}{\partial x}\Delta x+\frac{\partial I}{\partial y}\Delta y+\frac{\partial I}{\partial t}\Delta t$$
最后得到光流约束方程：
$$\frac{\partial I}{\partial x}\Delta x+\frac{\partial I}{\partial y}\Delta y+\frac{\partial I}{\partial t}\Delta t=0$$
由于这个方程有两个未知数$(\Delta x,\Delta y)$，所以没有唯一解。为了得到唯一解，就必须新增约束或假设，因此也就有了不同的算法。

Lucas-Kanade算法

Lucas-Kanade算法假设局部光流恒定，他使用一个3x3的patch，patch内的9个像素都具有同样的运动。因此，问题变为了：9个方程，求解2个未知数。算法使用最小二乘法(GN)迭代求解：
$$\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]={\left[\begin{array}{cc}{\sum }_i{f_{x_i}}^2& {\sum }_i{f}_{x_i}{f}_{y_i}\\ {\sum }_i{f}_{x_i}{f}_{y_i}& {\sum }_i{f_{y_i}}^2\end{array}\right]}^{-1}\left[\begin{array}{c}-{\sum }_i{f}_{x_i}{f}_{t_i}\\ -{\sum }_i{f}_{y_i}{f}_{t_i}\end{array}\right]=[J{J}^T{]}^{-1}[-Jr]$$
其中
$$\begin{gathered} f_x=\frac{\partial I}{\partial x}=\frac{I_2(x+\Delta x+1,y+\Delta y)-I_2(x+\Delta x-1,y+\Delta y)}{2} \\ {f}_y=\frac{\partial I}{\partial y}=\frac{I_2(x+\Delta x,y+\Delta y+1)-I_2(x+\Delta x,y+\Delta y-1)}{2} \\ J=[f_x,f_y{]}^T,r=f_t=预测值-真实值=I_2(x+\Delta x,y+\Delta y)-I_1(x,y) \end{gathered}$$
注意，式中的逆矩阵和Harris角点检测中的M很像，这表明角点更适合跟踪。

为了解决较大运动时无法跟踪的问题，算法使用图像金字塔，当图像进行上采样时，大运动变成了小运动，小运动被去除了。

直接法

外点处理（估计基础矩阵）

参考 RANSAC&鲁棒核函数

外点

传感器测量值可能会受多种因素干扰而不可靠，同时特征匹配也会存在误匹配的情况，如果不正确地检测并剔除他们，视觉SLAM中的很多算法（如计算本质矩阵、三角测量、PnP等）将会失败。

我们将非常不可能的测量值（根据测量模型）称为外点（outlier）。
对外点的一种常用判断标准是（一维数据中）将超出平均值三个标准差的测量值作为外点。

处理外点的两种最常用方案为：
1、RANSAC，选择正确的匹配进行估计。
2、M-估计，降低错误匹配的权重。

RANSAC

随机采样一致性（random sample consensus，RANSAC）是一种对带有外点的数据拟合参数模型的迭代方法。

该方法的目的是从所有数据中选择内点数据，用于估计。首先给出如下定义：

点：每一个数据，SLAM里指的是匹配的点对
野值/外点：错误的点
内点：正确的点
内点集：内点的集合
外点集：外点的集合
模型：带估计的参数
$s$：估计模型所需要的最小点数
$S$：所有的点形成的点集

显然，内点集就是我们想要找的正确数据。RANSAC的想法是从点集中随机的选择出$s$个点，估计出一个模型，查看剩余点是否符合这个模型。如果大部分点都符合这个模型，就相当于我们找到了一个合适的模型，自然的符合模型的点就是内点啦。由于是随机选择，一次可能难以找到正确的模型，因此需要多次才可以。下面给出完整的RANSAC算法。

目标

​ 一个模型与含离群值的数据集S的鲁棒拟合。

算法

​ STEP 1. 随机的从$S$中选择$s$个点作为一个样本，估计出模型。
​ STEP 2. 确定在模型距离阈值$t$内的数据点集 $S_i$ ， $S_i$为采样的一致集，并定义为S的内点。
​ STEP 3. 如果 $S_i$的大小（内点数）大于某个阈值$T$，用所有内点$S_i$重新估计模型并结束。
​ STEP 4. 如果$S_i$的大小小于阈值$T$，则选择一个新的自己，重复上述步骤
​ STEP 5. 经过$N$次试验，选择最大一致集$S_i$，并用$S_i$的所有点重新估计模型

RANSAC中有三个参数需要确定，分别是$t$、$T$、$N$。

1 内点阈值t

根据模型，每个点可以计算出一个距离的平方$d^2$，利用阈值$t^2$即可判断该点是否为内点。$d^2$的选择要根据具体模型进行选择，基本矩阵估计中采用点到极线的距离的平方，单应和相机内参估计中使用点到点的距离的平方。m个独立标准正态分布的平方和服从m自由度的卡方分布，选择阈值$t$使得点为内点的概率为$\alpha$。可以基于下式判断是否为内点：
$$\{\begin{array}{rl}Inlier& d^2<t^2\\ Outlier& d^2\ge t^2,where{t}^2=F^{-1}(\alpha ){\sigma }^2\end{array}$$
一般取$\alpha =95\%$。

2 采样次数N

假设一个点为内点的概率为w，则外点概率为$ϵ=1-w$，我们要求：

事件A：在$N$次采样中，至少要有一次选取的子集$s$内没有外点的概率$p=0.99$。

事件A的逆事件为：N次采样中，所有选取的子集$s$内至少存在一个外点。这个概率为$(1-w^s{)}^N=1-p$。那么

$$N=\frac{\log (1-p)}{\log (1-w^s)}$$

由此可得，采样次数与内外点的比例有关，而与数据量无关；并且随着最小点数s的增大而增大。

3 内点数量阈值T

当我们发现内点的数量足够多的时候就可以提前结束采样。内点数量的阈值$T$如何决定呢？我们一般认为，在给定外点的比例$ϵ$时，对于数据个数$n$，有$T=(1-ϵ)n$。这个比例可以估计的稍微保守一些。

4 自适应决定采样次数

从(1)可以看出，采样次数N与内点概率w有关，但实际情况是，一组数据中有多少内点事先是未知的。

开始时可以设$w=0$，此时$N=\infty$ 。经过一次采样后，设获得的内点数为$n_i$，可以更新内点概率为$w=\frac{n_i}{n}$，从而更新一次$N$。如此，迭代往复，每次若发现内点个数增加，就更新$N$。

改进后的RANSAC算法如下：

$N=\infty$，sample_count = 0

if (N > sample_count)

​ 选取s个样本，拟合模型，并根据t计算内点数inliers_count；

​ if (inliers_count > pre_inliers_count)： // 新内点数增加，更新N

​ w = inliers_count / n;

​ N = log(1.0 - p) / log(1.0 - std::pow(w, s));

​ end

​ sample_count++;

end

RANSAC与最小二乘的区别：
最小二乘法尽量去适应包括外点在内的所有点。而RANSAC得出一个仅仅用内点计算出模型，并且概率还足够高。但是，RANSAC不能保证结果一定正确（只有一定的概率得到可信的模型），为了保证算法有足够高的合理概率，必须小心选择算法的参数。对于外点数量较多的数据集，RANSAC的效果远优于直接的最小二乘法。

RANSAC估计基础矩阵

输入：2d-2d匹配关系 vector pts1, pts2；ransac内点距离阈值 double ransac_thresh=3; 置信度 double confidence = 0.9

输出：匹配关系是否为内点 vector status; F矩阵 Eigen::Matrix3d F

步骤：

0、计算采样次数N（maxIters）
$$N=\frac{\log (1-p)}{\log (1-w^k)}$$
其中，p为在N次采样中，至少有一次选取的子集s内没有外点的概率；k为拟合模型需要的样本数，对应基础矩阵num_samples=8；w为一个点为内点的概率。

Example: For w = 50%, p = 99%, k = 8: M = log(0.001) / log(0.99609) = 1176，即需要采样1176次从而保证RANSAC的成功率不低于0.99.

for i = 0 to maxIters：
	随机选取8对匹配点 => Mat pset1, pset2  #齐次坐标(u，v，1)
	
	8点法估计F矩阵
	{
		DLT构建矩阵 A(x'x, x'y, x', y'x, y'y, y', x, y, 1) * f_9x1 = 0
        SVD分解 A = U * S * V.T
        取V的最后一列作为基础矩阵的初值 f = V.col(8) => 由f构建矩阵F
        奇异值约束 F = U * diag(s1, s2, 0) * V.T
	}
	
	统计内点个数 findInliers
	{
		d = ransac_thresh * ransac_thresh;
		inliers = 0;
		for i = 0 to M:
			error = (x'.T * F * x)^2 / ( (F*x)_1^2 + (F*x)_2^2 + (F.T*x')_1^2 + (F.T*x')_2^2 ); #sampson距离
			if (error < d)
				inliers++;
	}
	
	更新最好估计
	if (inliners > best_inliers)
		best_inliers = inliers;
		
根据最好估计的所有内点使用最小二乘重新估计F矩阵
F = calcFundamentalLeastSquare(best_inliers, pts1, pts2);

M-估计

SLAM中的很多状态估计都是以最小化误差平方和作为代价函数，一个偏离较大的外点会对估计结果产生巨大的影响。而M估计的主要思想便是将原先误差的平方项替换为一个增长没那么快的函数，同时保证自己的光滑性质（能求导），获得更加鲁棒的最小二乘，这个函数又称为鲁棒核函数。

常见的鲁棒核函数有Huber核：

对极几何2D-2D

对极几何求解位姿

在特征匹配后，通过对极几何得到2D-2D关系。对极几何是两幅视图之间内在的射影几何，它独立于景物，只依赖于相机的内参和相对位姿。对极几何约束了一幅图上的点在另一幅图上的对应位置应在对极线上。
$$x_2=R{x}_1+t$$
通过等式两边分别先后左乘$[t{]}_{\times }$和$x_2^T$，可以得到：
$$x_2^T[t{]}_{\times }R{x}_1=p_2^T{K}^{-T}[t{]}_{\times }R{K}^{-1}{p}_1=0$$
根据对极几何可以推导出对极约束：
$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$
E为本质矩阵，F为基础矩阵，分别对应归一化平面坐标和像素坐标的对极几何关系。一般使用8点法求解基础矩阵和本质矩阵，若匹配点对多于8对，则可用RANSAC进行求解（见外点处理）。

8点法求基本矩阵：

1. 对于一对像素坐标点$(x,y,1{)}^T$和$(x^{\prime },y^{\prime },1{)}^T$，将方程展开，重构成:
   $$(x^{\prime }x,x^{\prime }y,x^{\prime },y^{\prime }x,y^{\prime }y,y^{\prime },x,y,1){\mathbf{f}}_{9\times 1}=0$$

2. 由于F的自由度为8，因此最少需要8个点即可求解该方程，通过构造8个点的矩阵A，对其进行SVD分解，由A对应的最小奇异值的奇异向量确定F。

根据估计出来的本质矩阵E，可以通过SVD分解恢复出相机的外参R和t。设E的SVD分解为$E=US{V}^T$，其中$S=diag(\sigma ,\sigma ,0)$，则R和t的可能结果为：
$$\begin{gathered} t_1=U(:,2),R_1=U{R}_z(\frac{\pi }{2})V^T \\ {t}_2=-U(:,2),R_2=U{R}_z^T(\frac{\pi }{2})V^T \end{gathered}$$
其中，
$$R_z(\frac{\pi }{2})=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right],R_z^T(\frac{\pi }{2})=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]$$
因此一共存在4组可能的$[R,t]$组合，如下图：

需要使得空间中一点同时位于两个相机的前方，以得到正确的相机位姿，具体方法为：

利用R、t和匹配点$p_1,p_2$进行三角化得到地图点$P$，P需要同时位于两个相机前方：
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=RP+t,z_c>0对两个相机成立$$

单应性矩阵求解位姿

$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]$$

自由度为8的单应矩阵至少需要4对匹配点求解，当大于4对时，可以通过RANSAC方法进行估计。

4点法求单应矩阵：

对于方程$x^{\prime }=\mathbf{H}x$，两边叉乘$x^{\prime }$得：
$${\mathbf{x}}^{\prime }\times \mathbf{H}\mathbf{x}=0$$
把H的第j行记为${\mathbf{h}}^{jT}$，$\mathbf{x}$记为$(x,y,1{)}^T$，则叉积可以写为：
$${\mathbf{x}}^{\prime }\times \mathbf{H}\mathbf{x}=\left[\begin{array}{c}{y}^{\prime }{\mathbf{h}}^{3T}x-{\mathbf{h}}^{2T}x\\ {\mathbf{h}}^{1T}x-x^{\prime }{\mathbf{h}}^{3T}x\\ x^{\prime }{\mathbf{h}}^{2T}x-y^{\prime }{\mathbf{h}}^{1T}x\end{array}\right]$$
由上式可以写出H的三个方程：
$$\left[\begin{array}{ccc}{0}^T& -{\mathbf{x}}^T& y^{\prime }{\mathbf{x}}^T\\ {\mathbf{x}}^T& 0^T& -x^{\prime }{\mathbf{x}}^T\\ -y^{\prime }{{\mathbf{x}}^{\prime }}^T& x^{\prime }{\mathbf{x}}^T& 0^T\end{array}\right]\left[\begin{array}{c}{\mathbf{h}}_1\\ {\mathbf{h}}_2\\ {\mathbf{h}}_3\end{array}\right]=0\to {\mathbf{A}}_{3\times 9}{\mathbf{h}}_{9\times 1}=0$$
上式虽然有3个方程，但由于第3行可由前两行得到，因此每队点实际只能给出H的两个约束，而H的自由度为8。因此，至少需要4组点即可求解方程${\mathbf{A}}_{2n\times 9}\mathbf{h}=0$，由A的最小奇异值对应的奇异向量确定h，从而确定矩阵H。

当相机做纯旋转时，$H=KR{K}^{-1}$，由H可求解得相机的外参R。
在opencv中，使用cv::decomposeHomographyMat(H, K, Rs, Ts, cv::noArray());
求解位姿Rs、Ts。

基础矩阵、本质矩阵和单应矩阵自由度

1、基础矩阵自由度

首先F矩阵是三阶的，并且其满足尺度等价性（因为像素坐标通过齐次坐标表示，因此矩阵同乘一个常数，最后求得的坐标是一致的，即$F_{33}=1$），因此自由度减1，再其次F矩阵为不可逆矩阵，行列式为0，因此自由度再减1，因此F矩阵自由度为7。

PS：相机做纯平移运动时，F矩阵自由度退化为2，在纯平面运动时，自由度退化为6.

2、本质矩阵自由度

由于本质矩阵为$E=[t{]}_{\times }R$，t和R都是3自由度，同时E满足尺度等价性，因此其自由度为5。

3、单应矩阵自由度

单应矩阵满足尺度等价性，因此自由度为8.

总结

• 基础矩阵表示的是两视图的对极约束，独立于景物结构，只依赖于摄像机的内部参数和相对姿态；本质矩阵是基础矩阵在归一化图像坐标下的表示；单应矩阵表示点和点之间的映射，但要求场景点在同一个平面上或者是相机纯旋转。

• 基础矩阵无法得到这个点对应点在另一幅图像上的确切位置（映射的是对极线）；单应矩阵可以找到点在另一幅图像上对应点的确切位置。

• 当相机进行纯旋转时，或者场景中的点在同一平面上或近平面上，由于F矩阵会发生退化，必须使用单应矩阵H而不能使用基础矩阵F；当相机的平移距离相对于场景的深度较小，即低视差场景的时候也可以使用单应矩阵H。因此通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的作为运动估计矩阵（ORB-SLAM的初始化方法）。

PnP(3D-2D)

当知道n个3D地图点及其投影位置时，可以通过PnP估计相机位姿。前面提到的2D-2D的对极几何方法需要至少8个点对，且存在初始化、纯旋转和尺度的问题。PnP问题有多种求解方法，包括直接线性变化（DLT）、EPnP等，还能用非线性优化的motion-BA方法。

DLT

考虑某个空间点P，它的齐次坐标为$\mathbf{P}=(X,Y,Z,1{)}^T$。在图像$I_1$中，投影到特征点$x_1=(u_1,v_1,1{)}^T$（以归一化平面齐次坐标表示）。定义增广矩阵$[\mathbf{R}|\mathbf{t}]$为3x4的矩阵，包括旋转和平移，则存在如下关系：

$$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
用最后一行消去s，得到两个约束：
$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$
将增广矩阵写成行向量形式有：
$$[\mathbf{R}|\mathbf{t}]=\left[\begin{array}{c}{\mathbf{t}}_1\\ {\mathbf{t}}_2\\ {\mathbf{t}}_3\end{array}\right]$$
则两个约束可以写为：
$$\begin{gathered} {\mathbf{t}}_1^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{u}_1=0 \\ {\mathbf{t}}_2^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{v}_1=0 \end{gathered}$$
可见每个特征点提供了两个关于T的线性约束，T一共有12维，因此最少需要6对匹配点即可求解。

在DLT求解中，我们直接将T矩阵看成了12个未知数，忽略了它们之间的联系。因为旋转矩阵$\mathbf{R}\in SO(3)$，需要针对DLT估计的旋转矩阵寻找一个最好的旋转矩阵做近似。可以通过QR分解，也可以把结果从矩阵空间重新投影到SE(3)流形上：
$$\mathbf{R}\leftarrow (\mathbf{R}{\mathbf{R}}^T{)}^{-\frac{1}{2}}\mathbf{R}$$

motion-only BA

考虑n个地图点P和其投影p，我们希望计算相机的位姿R，t，假设某地图点3D坐标为${\mathbf{P}}_i=[X_i,Y_i,Z_i{]}^T$，其投影的像素坐标为${\mathbf{p}}_i=[u_i,v_i{]}^T$，则像素位置和3D坐标的关系如下：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\mathbf{K}(\mathbf{R}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]+\mathbf{t})$$
其中，$s_i$为地图点到相机的距离，$\mathbf{K}$为相机内参矩阵。

由于相机位姿未知，该等式存在一个误差。因此，我们将误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使重投影误差最小：
$${\mathbf{T}}^{\ast }=\arg \underset{\mathbf{T}}{\min }\frac{1}{2}\sum _{i=1}^n||{\mathbf{p}}_i-\frac{1}{s_i}\mathbf{K}(\mathbf{R}{\mathbf{P}}_i+\mathbf{t})|{|}_2^2$$

我们用$exp({\xi }^{\wedge })$表示相机的位姿$[\mathbf{R}|\mathbf{t}]$的李代数，则最小二乘问题变为：
$${\xi }^{\ast }={\mathrm{argmin}}_{\xi }\frac{1}{2}\sum _{i=1}^n{\Vert e_i\Vert }_2^2={\mathrm{argmin}}_{\xi }\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-\frac{1}{s_i}\mathbf{K}exp({\xi }^{\wedge }){\mathbf{P}}_i\Vert }_2^2$$
通过链式法则，可以求得误差相对于位姿的雅可比矩阵：
$$\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^{{}^{\prime }2}}& f_x+\frac{f_x{X}^{{}^{\prime }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]$$
通过g2o或者ceres即可求解该最小二乘问题。

EPnP

EPnP算法理论

输入：N对3D-2D匹配关系$[P_i,p_i^c]$

输出：相机位姿$T_c^w$

算法步骤：

// PCA选择控制点$c_j^w$
cws[4] = choose_control_points()

// 计算每个3D点的hb坐标${\alpha }_{ij}$
computeBarycentricCoordinates()

// 构建方程$M_{2N\times 12}x=0$，求解控制点在相机坐标系下的坐标$x=[c_1^c,c_2^c,c_3^c,c_4^c{]}^T$
M = constructM(K, u, alpha)

// 上述方程解为$x={\sum }_{i=1}^N{\beta }_i{\mathbf{v}}_i$，v为M的右奇异向量，此处计算beta的近似解
betas = findBetaApprox()
// GN法优化beta
betas = GaussianNewton()
// 将3D点在相机系下的坐标$P_i^c$恢复出来: $P_i^c={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^c$
P_c = cameraCoord(V, betas, P)
// 使用ICP计算位姿
R, t = ICP(P_c, P_w)

{

​ 计算质心坐标$p_c^c,p_c^w$

​ 计算点的去质心坐标${\mathbf{P}}^c,{\mathbf{P}}^w$

​ 计算R：$W_{3\times 3}=({\mathbf{P}}^c{)}^T{\mathbf{P}}^w,W=US{V}^T,R=U{V}^T,若|R|<0,则R(2,:)=-R(2,:)$

​ 计算t：$t=p_c^c-R{p}_c^w$

}

ICP（3d-3d）

问题：给定两组点$A=[a_1,...,a_N]\in {\mathbb{R}}^{m\times N}，B=[b_1,...,b_N]\in {\mathbb{R}}^{m\times N}$，计算它们之间的刚体变换：
$$\begin{gathered} f(R,t)=\frac{1}{N}\sum _{i=1}^N\Vert b_i-R{a}_i-t{\Vert }^2=\Vert B-RA-t{\Vert }_F^2 \\ \underset{R,t}{\min }f(R,t),s.t.R{R}^T=I \end{gathered}$$
算法：

1. 对矩阵A、B进行归一化（去质心），得到A’、B’；
2. 对$B^{\prime }{A}^{\prime T}$进行SVD分解，得到$B^{\prime }{A}^{\prime T}=U\Sigma V^T$;
3. 最优解为$R^{\ast }=U{V}^T,若|R|<0,则R(2,:)=-R(2,:);t^{\ast }=\frac{1}{N}(B-R^{\ast }A)1$

双目相机标定

ORB-SLAM2中的D，K，R，P分别代表畸变参数向量（5x1）、相机内参（3x3）、相机旋转矩阵（3x3）、相机投影矩阵（3x4）。

对于一个3D点$\mathbf{X}=(X_i,Y_i,Z_i,1{)}^T$，通过投影矩阵P将其投影到图像坐标x上：
$$\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]=\mathbf{P}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$
投影矩阵P维度为3x4，自由度为11，可以分解为：
$$\mathbf{P}=\mathbf{K}[\mathbf{R}|\mathbf{t}]$$
相机标定算法就是为了求解矩阵P和相机的内参K和外参R、t。其主要分为两步：

1. 通过已知3D点的3D坐标和像素坐标，最小化重投影误差求解矩阵P。
2. 通过QR分解求解K，R，t。

三角化

对于单目SLAM，仅通过单张图像无法获取像素的深度信息，需要通过三角化的方法来估计地图点深度，从而创建该地图点。

三角化是指通过在两处观察同一个3D点的夹角，从而确定该点的距离。

ORB-SLAM的三角化

目标：根据相机的两帧间的位姿及跟踪到的特征点，通过三角化得到这些特征点对应的地图点。

输入：相机位姿、相机内参、两个2D点

输出：地图点

假设地图点为$P_w=[x,y,z,1{]}^T$，参考帧的相机位姿为$T_{rw}=[R_{rw}|t_{tw}]$，表示从世界坐标系变化到参考帧坐标系，同理，当前帧的相机位姿为$T_{cw}=[R_{cw}|t_{cw}]$。点$P_w$在参考帧的归一化平面坐标为$p_r=[x_r/z_r,y_r/z_r,1{]}^T$，深度为$z_r$，在当前帧归一化平面的坐标为$p_c=[x_c/z_c,y_c/z_c,1{]}^T$，深度为$z_c$。根据位置变换关系有：
$$z_r{p}_r=T_{rw}{P}_w$$

$$z_c{p}_c=T_{cw}{P}_w$$

对上述两式分别叉乘对应的归一化平面坐标，得：

$$p_r\times z_r{p}_r=p_r\times T_{rw}{P}_w=0$$

$$p_c\times z_c{p}_c=p_c\times T_{cw}{P}_w=0$$

整理得：
$$\left[\begin{array}{c}{p}_r\times T_{rw}\\ p_c\times T_{cw}\end{array}\right]P_w=Ax=0$$
由此，通过构建一个齐次方程来求解$P_w$，其中$A\in R^{6\times 4}$，是一个超定方程，因此目标函数为：
$$J(x)=\min \Vert Ax\Vert$$
可以通过SVD分解，得到$A=UD{V}^T$，$V_{4\times 4}$的最后一列的列向量即为所求的$P_w$，由于齐次坐标的最后一位是1，因此求得的$P_w$要统一除以最后一位。

输入：两个相机的位姿$[R_c,t_c]$和$[R_r,t_r]$，特征点的2D像素坐标$p_r,p_c$，相机内参$K$。

输出：特征点的3D坐标$P_w$。

算法步骤：

p_rn, p_cn = pixel2Normalized(p_r, p_c) #将像素坐标转换为归一化坐标

A = construtMatrixA(p_rn, p_cn, T_rw, T_cw) #构建矩阵Ax=0

{

​ $A=\left[\begin{array}{c}{p}_{rn}\times T_{rw}\\ p_{cn}\times T_{cw}\end{array}\right]$

}

P_w = computeBySVD(A) #对A做SVD分解，P_w取最小奇异值的奇异向量，注意需要齐次化

A*算法

A*算法通过代价函数$f(n)=g(n)+h(n)$来计算每个节点n的优先级。其中：

• f(n)为节点n的综合优先级，当旋转下一个要遍历的节点时，总是选取综合优先级最高(f(n)值最小)的节点。
• g(n)为节点n距离起点的代价。
• h(n)为节点n距离终点的预计代价，也即启发函数。

A*算法还使用两个集合来分别表示待遍历的节点和已遍历过的节点，称为open_set和close_set。

算法描述：

* 初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_set中删除，并加入close_set中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_set中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m也不在open_set中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入open_set中

扩展卡尔曼滤波

*prediction
	x' = F * x + u 
	P' = F * P * F^T + Q
*measurement update
	y = z - H * x'
	S = H * P' * H^T + R
	K = P' * H^T * S^(-1)
	x = x' + K * y
	P = (I - K * H) * P'

分为预测和更新两步：

1. 预测：根据状态转移矩阵F预测状态量x’，并进行状态协方差传递，预测不确定度P’。
2. 观测：计算观测残差y，并根据测量矩阵H和协方差计算增益K。
3. 更新：根据预测x’和观测残差y更新当前状态x，和不确定度P。

激光SLAM

占据栅格地图构建

原理：对于地图里的一个栅格s，使用$Odd(s)=\frac{p(s=1)}{p(s=0)}$表示其状态，s=1表示占据，s=0表示空闲。其后验估计为：
$$Odd(s|z)=\frac{p(s=1|z)}{p(s=0|z)}=\frac{p(z|s=1)p(s=1)}{p(z|s=0)p(s=0)}=\frac{p(z|s=1)}{p(z|s=0)}\ast Odd(s)$$
两边取对数为：
$$\log Odd(s|z)=\log \frac{p(z|s=1)}{p(z|s=0)}+\log Odd(s)$$
令激光雷达对栅格的观测结果为：
$$\begin{gathered} lofree=\log \frac{p(z=0|s=1)}{p(z=0|s=0)} \\ looccu=\log \frac{p(z=1|s=1)}{p(z=1|s=0)} \end{gathered}$$
则更新规则根据观测变为：
$$S^{+}=S^{-}+lofree/looccu$$

线性代数

非线性优化

GN法：

1、构建误差函数 $r_{ij}$

2、计算误差的雅可比 $J_{ij}$

3、构建增量方程 $(J^{T}J)\delta x=-J^{T}r$，并求解$\delta x$

4、更新 $x_{k+1}=x_k\oplus \delta x$，并返回1进行迭代

LM法：

输入：最大迭代次数N、顶点$x$、残差边$r$

输出：优化后的变量$x^{\ast }$

构建H矩阵makeHessian(); 计算残差$r_{ij}$和雅可比$J_{ij}$，构建H矩阵

LM初始化computeLambdaInit(); 确定收敛阈值和${\lambda }_0=\tau \ast \max |(J^{T}J{)}_{i,i}|$

while (!stop && iter < N) { //迭代求解

​ while (!oneStepSuccess) { // 不断尝试lambda，直至成功下降一步

​ addLambda2Hessian(); // H矩阵加上lambda

​ solveLinear(); //求解Hx=b

​ removeLambdaHessian(); // H矩阵减去lambda

​ 判断是否退出：delta_x <= 1e-6

​ updataStates(); // 更新 $x_{k+1}=x_k\oplus \delta x$

​ oneStepSucess = isGoodStep(); // 判断当前步是否可行，及lambda的更新

​ if (oneStepSuccess)

​ makeHessian(); //在新线性化点构建Hessian

​ else

​ rollbackStates(); //误差没下降，回滚

​ }

}

其中isGoodStep()的流程如下：

1. 近似下降: $L(0)-L({\delta }_x)=\frac{1}{2}\delta x^T(\lambda {\delta }_x+b)$

2. 实际下降：$F(x)-F(x+\delta x)$

3. 计算$\rho =\frac{实际下降}{近似下降}$，并根据$\rho$更新$\lambda$：

   if rho > 0:	//误差下降，则减小阻尼增大步长
   	lambda := lambda * max(1/3, 1 - pow((2*rho-1), 3));
   	ni = 2;
   	return true;
   else		//误差没下降，则增大阻尼减小步长
   	lambda := lambda * ni;
   	ni := ni * 2;
   	return false;
   

Dogleg法：

begin

​ $k:=0;x:=x_0;\Delta :={\Delta }_0;b:=J(x{)}^{T}f(x)$

​ $found:=(||f(x)|{|}_{\infty }\le {ϵ}_3)\mathbf{o}\mathbf{r}(||b|{|}_{\infty }\le {ϵ}_1)$

​ while (not found) and (k < k_max):

​ $k:=k+1$; 最速下降步长$\alpha =\frac{||b|{|}^2}{||J(x)b|{|}^2}$

​ 最速下降$h_{sd}:=-\alpha b;$

​ GN下降$h_{gn}:=-H^{-1}b;$

​ 计算dogleg更新量:

​ {

​ if (h_gn <= Delta)

​ h_dl = h_gn

​ else if (h_sd >= Delta)

​ h_dl = (Delta / abs(h_sd)) * h_sd

​ else

​ h_dl = alpha * h_sd + beta * (h_gn - alpha * h_sd)

​ }

​ 更新状态$x_{new}:=x+h_{dl}$

​ 计算增益比$\rho =\frac{实际下降}{近似下降}$

​ 更新信赖域半径：

​ {

​ if (rho > 0)

​ x = x_new; g = Jt * f;

​ if (rho > 0.75)

​ Delta = max(Delta, 3 * norm(h_dl));

​ else if (rho < 0.25)

​ Delta = Delta / 2;

​ }

为什么求解Ax=0时取得是SVD分解的最小奇异值的奇异向量？

对于齐次方程Ax=0，可以把问题转化为最小化$||Ax|{|}^2$的非线性优化问题，为避免x=0的特解情况，人为增加约束$||x|{|}^2=1$，因此问题变为：
$$\underset{x}{\min }(||Ax|{|}^2),||x|{|}^2=1$$
对A做SVD分解得$||Ax||=||UD{V}^{T}x||=||D{V}^{T}x||$，令$y=V^{T}x$，则问题变为
$$\underset{y}{\min }(||Dy||),||y||=1$$
由于D为对角阵，且对角元的元素按递减排序，因此最优解为$y=(0,0,...,1{)}^T$，又$x=Vy$，因此最优解就是V的最小奇异值对应的列向量。

算法框架

ORB-SLAM

ORBSLAM初始化特征匹配策略？

由于初始化阶段，只有2D-2D匹配关系，因此需要尽可能保证匹配正确。在ORB-SLAM中，作者使用“旋转直方图”筛选正确匹配，函数接口为ORBmatcher::SearchForInitialization。输入参考帧F1和当前帧F2的特征点，其步骤为：

1. 构建旋转直方图，将360°划分为30个区间。

2. 遍历F1中的所有特征点：

   • 对于特征点i1，在半径为100的窗口内搜索F2中所有候选匹配特征点vIndices2;

   • 遍历vIndices2，计算i1的最优和次优匹配；

   • 对匹配结果进行阈值检查：

     • 最优匹配距离 < 次优匹配距离 * 阈值mfNNratio；
     • 删除重复匹配；

   • 选择次优匹配关系，双向建立；

   • 计算匹配特征点的角度差，并加入到相应的直方图区间内;

     rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;

3. 筛除旋转直方图中“非主流”部分：选出旋转直方图中最大的前3个bin，将不在这3个bin内的匹配对剔除掉。

VINS-Mono

什么是边缘化？First Estimate Jacobian？一致性？可观性？

边缘化其实简单说就是将滑窗中丢弃的图像帧的信息保留下来传递给剩余变量的方式。

First Estimate Jacobian是为了解决新测量信息和旧的先验信息构建新的系统时，对某一优化变量求雅克比的线性化点不同导致信息矩阵的零空间发生变化，不可观的变量变成可观变量的问题。做法就是保证变量的线性化点不变，“具体来说，位姿采用propagated值而非updated值，landmark使用第一次估计值”。主要是作用在滤波和滑窗法中，具体可以参考First Estimate Jacobian。

一致性指可观性的一致性，也即线性化点的一致不变。同一个变量在同一次优化中，在不同约束项里进行不同地方的线性化，从而导致出现不一致性。例如，相机位姿在边缘化后留在了滑窗中，但是其观测的某个点在k时刻被边缘化掉。这样它的信息矩阵就会出现在先验中，而当我们在k’时刻对该相机位姿进行优化使用的应该是k’的状态计算雅克比。
为了解决这个问题，只能改变用于计算雅克比的状态估计。在当前状态和边缘化状态相连时，使用边缘化时刻k的状态来计算后面的雅克比。这样保证了信息矩阵只有一个状态。注意，旧的状态只用来计算雅克比。

可观性的定义和现代控制理论中能观性定义是一致的，即通过测量获得状态变量的信息，即该变量是能观的。

FEJ

【参考】
msckf中的可观性分析和FEJ
滑窗法的可观性分析和FEJ

滑动窗口的不一致性：
考虑k时刻滑窗中边缘化后保留的相机位姿$c_{i\ast }$，假设特征$p_{L_j\ast }$被$c_{i\ast }$观测到且已在k时被边缘化掉。那么在计算k时的先验信息$A_p(k)$时使用的是k时刻的状态估计。在k‘时刻做非线性优化时，相机位姿$c_{i\ast }$依然在滑窗中，然而此时观测的雅可比计算使用的是k’时刻的状态估计。该情况会使得估计器错误地认为方向是可观的（信息矩阵的秩增加），从而导致不一致性。

FEJ：
为了避免不一致的问题，对于和边缘化状态（$p_{L_j\ast }$）通过观测相连的滑窗保留状态（$c_{i\ast }$），使用边缘化时刻k的状态估计去计算k’时刻的雅可比。
PS：我们只在计算雅可比时使用“老”的估计，实际的状态量在非线性优化中依然会迭代更新。

VINS-Mono中的可观性分析：
在VINS-Mono中，根据贺博的博客，VINS-Mono中似乎只在计算先验雅可比时使用了FEJ（即固定线性化点），该雅可比通过对边缘化得到的$A_p$做SVD分解计算得到：
$$\begin{gathered} H_p=US{U}^T=J^{T}J \\ {J}_p=\sqrt{S}{U}^T \\ {r}_p={\sqrt{S}}^{-1}{U}^T{b}_p \end{gathered}$$
代码如下：

linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;

至于imu和视觉观测残差的雅可比，VINS-Mono并没有使用FEJ，其线性化点在迭代优化时是不断更新的。

vins中预积分项如何根据bias进行修正？

$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}={\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{\delta b_{a_k}}^{\delta {\alpha }_{b_{k+1}}^{b_k}}\delta b_{a_k}+J_{\delta b_{w_k}}^{\delta {\alpha }_{b_{k+1}}^{b_k}}\delta b_{w_k} \\ {\beta }_{b_{k+1}}^{b_k}={\hat{\beta }}_{b_{k+1}}^{b_k}+J_{\delta b_{a_k}}^{\delta {\beta }_{b_{k+1}}^{b_k}}\delta b_{a_k}+J_{\delta b_{w_k}}^{\delta {\beta }_{b_{k+1}}^{b_k}}\delta b_{w_k} \\ {\gamma }_{b_{k+1}}^{b_k}={\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{\delta b_{w_k}}^{\delta {\theta }_{b_{k+1}}^{b_k}}\delta b_{w_k}\end{array}\right] \end{gathered}$$

当对加速度计偏置和陀螺仪的偏置发生（微小）改变时，就可以根据等式(2)对预积分项进行线性修正，避免了重复积分。