[转] SLAM基础知识汇总【长期更新】

SLAM基础知识汇总

特征点相关

特征点由关键点和描述子构成：

• 关键点：特征点在图像里的位置
• 描述子：通常是一个向量，描述了该关键点周围的信息，朝向大小等

[ORB-SLAM2] ORB-SLAM中的ORB特征（提取）https://zhuanlan.zhihu.com/p/61738607

ORB-SLAM中用到的四叉树：

划分为四个网格，每个网格内检测到的特征点在一定范围内（>1且<25），就继续划分网格，若网格中的特征点≤1，就不在继续划分网格。节点数量超过一定值就不再继续下分网格（30个），最后每个网格中保留response最大的特征点。
划分网格：

处理前后结果对比：

这样就能保证图片的特征点比较均匀。

计算机视觉相关

针对不同的条件和环境进行一系列划分：

• 已知 x 、 K 、 R 、 t ，求 X ： 三角化（ Triangulation ）
• 已知 x 、 X 、 K ，求 R 、 t ： 姿态估计（ Pose Estimation ）
• 已知 x 、 X ，求 K 、 R 、 t ： 相机标定（ Camera Calibration ）
• 已知 x ，求 K 、 R 、 t 、 X ： 稀疏重建（ Sparse Reconstruction ）

x：像素坐标，X：3D坐标，R相机旋转矩阵，t相机平移向量，K相机内参矩阵，R和t合起来是相机的外参矩阵T

消影点（vanishing points） ：

空间平行线在图像投影线的交点， 对应三维空间中的无穷远点。消影点只与三维直线方向有关， 与其位置无关。

图像金字塔：

图像金字塔能解决FAST特征点缺乏尺度不变性的问题

（不管原图尺度是多少，在包含了所有尺度的尺度空间下都能找到那些稳定的极值点，这样就做到了尺度不变；即不管距离远或者近，提取看上去都是角点的点）

退化问题：

正常情况下基础矩阵模型应该可以应付，但如果特征点共面（初始化场景中主要是一个平面），或者两帧之间的相对位姿未纯旋转时，基础矩阵的自由度会下降，也就是所谓的退化，类似于方程数少于变量数。此时为了保证运动恢复的精度，就不能再用基础矩阵模型。由此提出了单应矩阵，其假设特征点落在同一平面上，从而适用于这种场景下的运动恢复。

齐次坐标：

简单说来，齐次坐标就是在原有的坐标维度上再添加一个维度。

齐次坐标的优势：

• 方便的表达点在直线或平面上

2D平面上，一条直线l可以用方程ax+by+c=0来表示，该直线用向量来表示记作

l=(a,b,c)^T

点p的齐次坐标为(x,y,1)

则点P在直线上就可以用内积（点乘）来表示

l^T*p=0

• 方便表达直线与直线，平面与平面的交点

结论：在齐次坐标下，可以用两个点 p, q 的齐次坐标叉乘结果来表达一条直线 l，也就是

l = p x q

也可以使用两条直线 l, m 的叉乘表示他们的交点 x

x = l x m

• 能够区分一个向量和一个点：

\1. 从普通坐标转换成齐次坐标时

如果(x,y,z)是个点，则变为(x,y,z,1);

如果(x,y,z)是个向量，则变为(x,y,z,0)

2.从齐次坐标转换成普通坐标时

如果是(x,y,z,1)，则知道它是个点，变成(x,y,z);

如果是(x,y,z,0)，则知道它是个向量，仍然变成(x,y,z)

• 能够表达无穷远

如果一个点的齐次坐标中，最后一个元素为0，则表示为无穷远点。

• 更简洁的表达欧氏空间变换

使用齐次坐标，可以方便的将加法转化为乘法，方便的表达平移。

向量点乘定义：

a * b = ||a||* ||b|| *cos(θ)

叉乘定义：

a * b = ||a||* ||b|| *sin(θ)*n

n为一个与向量a,b所构成的平面垂直的单位向量

相机模型：

像素点(u,v)与世界坐标点(x,y)的投影关系

这里的x,y为归一化坐标即x=X/Z,y=Y/Z

反投影则为：

曼哈顿世界假设

如下图所示，环境中存在垂直/正交的信息，如地板、天花板、墙面等。通过判断相机和曼哈顿世界之间的旋转矩阵，进而可以获得相机与相机之间的相对旋转矩阵。

https://zhuanlan.zhihu.com/p/190232333

位姿估计：

2D-2D 对极几何：

左右两个平行四边形分别是相机在不同位置的成像平面

C0, C1分别是两个位置中相机的光心，也就是针孔相机模型中的针孔。

P是空间中的一个三维点，p0, p1分别是P点在不同成像平面上对应的像素点（通过特征匹配得到）

c0-c1连线在成像平面上的交点为极点

极点和匹配点的连线就是极线

本质矩阵：

对极约束：

极线方程：

Ep1看做是直线的方程，p0看做是直线上的点，也就是说Ep1就是以C0为原点坐标系中的极线

根据对极几何定义，设x1,x2为特征点的归一化坐标，则他们满足：

经过对极几何已经求得相机运动R和t，需要求解特征点的深度s1,s2.

两个深度可以分开算，若先算s2,那么对上式两边左乘一个x1^，得：

该式左侧为0，右侧为s2的一个方程，可根据它直接求解s2。

3D-2D PNP算法

目前遇到的场景主要有两个，
其一是求解相机相对于某2维图像/3维物体的位姿；
其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。
在场景1中，我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机坐标系相对于世界坐标系(Twc)的位姿
在场景2中，通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换
算法伪代码：
求解PNP问题的常用算法 DLT（直接线性变换），EPNP（把2D转换为3D点，用ICP求解），P3P（构建三角形），UPNP等

3D-3D ICP算法：

void pose_estimation_3d3d(const vector<Point3f> &pts1,
                          const vector<Point3f> &pts2,
                          Mat &R, Mat &t) {
                          Point3f p1,p2;
    int N=pts1.size();
    for(int i=0;i<N;i++)
    {
        p1+=pts1[i];
    	p2+=pts2[i];
    }//获取质心坐标
    p1=Point3f(Vec3f(p1)/N);
    p2=Point3f(Vec3f(p2)/N);
    vector<Point3f> q1(N),q2(N);//以质心坐标为原点的坐标系
    for(int i=0;i<N;i++){
        q1[i]=pts1[i]-p1;
        q2[i]=pts2[i]-p2;
    }
    //计算q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i=0;i<N;i++)
    {
        W+=Eigen::Vector3d(q1[i].x,q1[i].y,q1[i].z) * Eigen::Vector3d(q2[i].x,q2[i].y,q2[i].z).transpose();
    }
    
    //对W进行SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | EigenLLComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V =svd.matrixV(); 
    
    Eigen::Matrix3d R_=U*(V.transpose());
    if(R_.determinant()<0){//行列式小于零
        R_=-R_;
    }
    Eigen::Vector3d t_=Eigen::Vector3d(p1.x,p1.y,p1.z)-R_*Eigen::Vector3d(p2.x,p2.y,p2.z);
    R=(Mat_<double>(3,3) <<
      R_(0,0),R_(0,1),R_(0,2),
      R_(1,0),R_(1,1),R_(1,2),
      R_(2,0),R_(2,1),R_(2,2)
      );
    t=(Mat_<double>(3,1) << t_(0,0), t_(0,1), t_(0,2));
}

直接法：

直接法具体过程如下：

1. 准备工作。假设相邻帧之间的位姿 Tk,k−1 已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。
2. 重投影。知道 Ik−1 中的某个特征在图像平面的位置 (u,v) ，以及它的深度 d ，能够将该特征投影到三维空间 pk−1 ，该三维空间的坐标系是定义在 Ik−1 摄像机坐标系的。所以，我们要将它投影到当前帧 Ik 中，需要位姿转换 Tk,k−1 ，得到该点在当前帧坐标系中的三维坐标 pk 。最后通过摄像机内参数，投影到 Ik 的图像平面 (u′,v′) ，完成重投影。
3. 迭代优化更新位姿 。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿 Tk,k−1 。
   将上述过程公式化如下：通过不断优化位姿 Tk,k−1 最小化残差损失函数。

半直接法以SVO为例：

三角测量（三角化）：

定义：通过在两处观测同一个点的夹角，从而确定该点的距离。

作用：通过三角测量来估计地图点的深度。

点的重投影误差：

根据多视图几何得到点的三维坐标在当前帧的投影(运动方程)，与根据帧间估计得到上一帧的点在当前帧中该点的坐标(观测方程)。

投影点-匹配点=点的重投影误差

运动方程与观测方程的差为重投影误差

线的重投影误差：

三维直线两个端点投影到匹配到的特征线段所在直线的距离和

计算机视觉中的warp函数：

Warp这个操作本身可以理解为扭曲，变型，变换；其实就是一种数学运算，是一种统称，一般情况下paper里会定义怎么warp，不同建模warp function不同。

对于计算机几何视觉一般有

1）欧氏变换（SO3,SE3），自由度为3或者6，不变的是长度，夹角，体积；

2）相似变换，自由度为7，不变的是体积比；

3）仿射变换（Affine），自由度12，不变的是平行性，体积比；

4）射影变换，自由度15，相交性不变。

SLAM系统相关

位姿估计

2D-2D:

特征点匹配的方法可以LK光流/暴力匹配/FLANN
特征点匹配->计算基础矩阵->计算本质矩阵->本质矩阵恢复位姿
恢复位姿代码如下

//-- 计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points1, points2, FM_RANSAC);
    //cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

    //-- 计算本质矩阵
    Point2d principal_point(300, 300);				//相机主点, TUM dataset标定值
    int focal_length = 600;						//相机焦距, TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    //cout << "essential_matrix is " << endl << essential_matrix << endl;

    //-- 计算单应矩阵
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    //cout << "homography_matrix is " << endl << homography_matrix << endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    //cout << "R is " << endl << R << endl;
    //cout << "t is " << endl << t << endl;

RANSAC在SLAM中主要是用于消除ORB特征点的误匹配

RANSAC算法基本思想：

1. 从数据集中随机选出一组局内点（其数目要保证能够求解出模型的所有参数），计算出一套模型参数。
2. 用得到的模型去测试其他所有的数据点，如果某点的误差在设定的误差阈值之内，就判定其为局内点，否则为局外点，只保留目前为止局内点数目最多的模型，将其记录为最佳模型。
3. 重复执行1,2步足够的次数（即达到预设的迭代次数）后，使用最佳模型对应的局内点来最终求解模型参数。
4. 最后可以通过估计局内点与模型的错误率来评估模型。

RANSAC中的内点和外点：

内点：符合模型的估计的点成为内点

外点：不符合模型估计的点成为外点

SLAM中旋转矩阵、旋转向量（轴角）、欧拉角、四元数的转换关系

旋转表示之间的对比

旋转矩阵

1. SLAM中编程使用频繁，重点掌握
2. 旋转矩阵R具有正交性，R和R的转置RT的乘积是单位阵，行列式为1
3. 旋转矩阵R的逆矩阵表示一个和R相反的旋转
4. 旋转矩阵R通常和平移向量一起组成齐次变换矩阵T，描述欧式坐标变换，引入齐次坐标是为了可以方便的描述连续的欧氏变换
5. 冗余，9个元素表示三个自由度的旋转，比较冗余

四元数

1. SLAM中编程使用频繁程度接近旋转矩阵

2. 四元数由一个实部三个虚部组成，是一种非常紧凑、没有奇异的表达方式（奇异矩阵不是满秩）

3. 编程时候很多坑，必须注意。首先，一定要注意四元素定义中实部虚部和打印系数的顺序不同，很容易出错！

   其次，单位四元素才能描述旋转，所以四元素使用前必须归一化：q.normalize()。

旋转向量

1. 用一个旋转轴n和旋转角θ来描述一个旋转，所以也称轴角。不过很明显，因为旋转角度有一定的周期性（360°一圈），所以这种表达方式具有奇异性。
2. 从旋转向量到旋转矩阵的转换过程称为 罗德里格斯公式。
3. 旋转向量和旋转矩阵的转换关系，其实对应于李代数和李群的映射

欧拉角

1. 把一次旋转分解成3次绕不同坐标轴的旋转，比如航空领域经常使用的“偏航-俯仰-滚转”（yaw，pitch，roll）就是一种欧拉角。该表达方式最大的优势就是直观。
2. 欧拉角在SLAM中用的很少，原因是它的一个致命缺点：万向锁。也就是在俯仰角为±90°时，第一次和第3次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性。事实上，想要表达三维旋转，至少需要4个变量。

常用误差指标：

绝对轨迹误差（Absolute Trajectory Error,ATE),形式如下：

实际上是每个位姿李代数的均方根误差（RMSE）。这种误差可以刻画两条轨迹的旋转和平移误差。

绝对平移误差(Average Translations Error):

其中，Trans表示取括号内部变量的平移部分。

相对位姿误差(Relative Pose Error,RPE)定义如下：

同样可以只取平移部分：

超像素：

由一系列位置相邻且颜色、亮度、纹理等特征相似的像素点组成的小区域。

图像强度：

表示单通道图像像素的强度（值的大小）。

灰度图中，它是图像的灰度。

RGB颜色空间中，可以理解为它是R通道、G通道、B通道的像素灰度值，也就是RGB中的三个图像强度。

世界坐标系、物体坐标系和惯性坐标系之间的关系

世界坐标系->惯性坐标系：平移

惯性坐标系->物体坐标系：旋转

光流法：一种描述像素随着时间，在图像之间运动的方法。

稀疏光流：计算部分光流。

稠密光流：计算所有光流。

稀疏光流以LK光流法为代表的。

LK光流计算过程：

假设相机的图像是随时间变化的，图像看做是时间的函数：I(t)，在t时刻，位于(x,y)的像素，灰度为I(x,y,t)。

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

对于 t 时刻位于(x,y)位置的像素，设t+dt时刻，运动到(x+dx,y+dy)处，由于灰度不变：

对左边进行泰勒展开，保留一阶项，得：

由于灰度不变，下一个时刻的灰度等于之前的灰度，从而

两边除以dt，得

其中，dx/dt为像素在x轴上运动速度，而dy/dt为y轴速度，记为u,v。同时∂I/∂x为图像在该点处x方向的梯度，另一项是y方向的梯度。记为Ix，Iy。

图像灰度对时间的变化量记为It，写成矩阵形式为：

计算像素运动u,v，但是该式是带有两个变量的一次方程，无法解出，因此，必须引入额外的约束来计算 u,v。在 LK 光流中，我们假设某一个窗口内的像素具有相同的运动。 考虑一个大小为 w ×w 大小的窗口，它含有w²数量的像素。由于该窗口内像素具有同样的运动，因此我们共有个w²方程

用最小二乘法求解：

简化为：

最小二乘法求解：