VSLAM学习记录1（适合小白学习）

VSLAM学习记录1

一. slam问题的数学描述

二.相机成像

内参矩阵

像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。

负号表示成的像是倒立的，这里把成像平面对称到相机前方，发三维空间点放在一起放在摄像机坐标系的同一侧，这样就能把负号去掉了。其实就是先成像，再等比例对称回来。

在这里插入图片描述

齐次坐标

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

举个例子：

欧式变换：旋转，平移，等比缩放。

SLAM中一般都是连续的欧氏变换，所以会有多次连续的旋转和平移，假设我们将向量a进行了两次欧氏变换，分别为R1, t1 和 R2，t2，分别得到：

b = R1a + t1, c = R2b + t2

最终的结果 c = R2*(R1*a + t1) + t2

显然，这样的变换在经过多次后会变的越来越复杂。其根本原因是上述表达方式并不是一个线性的变换关系。

如果使用齐次坐标：

外参

外参会随着相机运动发生变化，同时也是slam中待估计的目标，代表着机器人的轨迹。

相机的位姿由旋转矩阵R和平移向量t来描述

三. 非线性优化

由于噪声的存在，前文提到的运动方程和观测方程的等式不是精确成立的，所以，与其假设数据必须符合方程，不如解决如何在有噪声的数据中进行准确的状态估计。由于在slam问题中，同一点往往会被一部相机在不同的时间内多次观测，同一部相机在每一时刻观测的点也不止一个，最终能够较好从噪声数据中恢复出我们需要的东西。

Ceres 最小二乘求解

g2o 图优化

四. 视觉里程计 VO

特征点提取和匹配

特征点

VO的主要问题是如何根据图像来估计相机运动，而图像本身是一个由亮度和色彩组成的矩阵，如果直接从矩阵层面考虑运动估计会很困难。所以，一般是：

• 首先，从图像中选取比较有代表性的点。这些点在相机视角发生少量变化(光照，视角)后会保持不变，所以会在个各个图像中找到相同的点。
• 然后，在这些点的基础上，讨论相机位姿估计的问题，以及点的定位问题。（考虑图像中点的位置 这里一般有两种方法 ：特征点法和直接法）

经典SLAM模型中以位姿——路标（Landmark）来描述SLAM过程

• 路标是三维空间中固定不变的点，能够在特定位姿下观测到

• 数量充足，以实现良好的定位

• 较好的区分性，以实现数据关联

在视觉SLAM中，可利用图像特征点作为SLAM中的路标。

可以作为图像特征的部分：角点，边缘，区块；而图像中的角点，边缘相比于像素区块更加”特别“（在不同图像间的辨识度更强）。为了更加稳定的局部图像特征，特征点有如下性质：

• 可重复性：相同的特征点在不同的图像找到

• 可区别性：特征点是不同的

• 高效率：提这个点不能占用太多时间，也就是实时性

• 本地性：

  例子：SIFT/SURF/ORB

特征点匹配可能遇到的问题：

1. 特征点提取不出来，比如在墙角，只有一面墙，因为特征点一般是角点信息
2. 如果图片变化不明显，纹理不明显，匹配关系就不好找

特征点的信息：

• （提取）关键点：位置，大小，方向，评分等
• （计算）描述子：特征点周围的图像信息，通常是一个向量，“外观相似的特征应该有相似的描述子”，只要两个特征点的描述子在向量空间的距离上相近，就可以认为他们是同样的特征点。

例子：ORB特征

• 关键点：Oriented FAST
• 描述子：BRIEF

FAST：连续N个点的灰度有明显差异（角点提取方式）如果一个像素与邻域的像素差别较大，那么他很可能是角点。

要想判断一个像素点p是不是FAST关键点，只需要判断其周围的16个像素点中是否有连续N个点的灰度值与p的差超出阈值。16个点的位置如下图所示。N一般取12，称为FAST-12，常用的还有FAST-9，FAST-11。阈值一般为p点灰度值的20%。

检测过程：

1. 在图像中选取像素p，假设它的亮度为Ip;

2. 设置一个阈值T(比如，Ip的20%)

3. 以像素p为中心，选取半径为为3的圆上的16个像素点。

4. 假如选取的圆上有连续的N个点的亮度大于Ip+T或者小于Ip-T，那么像素p可以被认为是特征点（N通常取12，即FAST-12，还有9，11）

5. 循环上述四步，对每个像素执行相同的工作。

   在FAST-12算法中，为了更高效，可以添加一项预测试操作，以快速排除绝大多数不是角点的元素。具体操作为：对每个像素，直接检测领域圆上的第1，5，9，13个像素的亮度。只有当这4个像素中有3个同时大于Ip+T,或Ip-T时，当前像素才有可能是一个角点，否则应该直接排除。

   此外，原始的FAST角点经常出现“扎堆”的现象。所以在第一遍检测过后，还需要用非极大值抑制，在一定区域内保留响应最大值的角点，避免角点集中的问题。（ORB中，对原始的FAST角点分别计算Harris响应值，然后选取前N个具有最大响应值的角点作为最终角点的集合）。

Oriented FAST：在FAST基础上计算旋转（旋转角）。相较于原版的FAST，ORB中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变的特性。

灰度质心法

（实现旋转不变性）

P为几何中心，Q为灰度质心
在一个圆内计算灰度质心（思考：为什么是圆？不是正方形？）
ORB通过灰度质心法来找Oriented FAST（计算特征的方向）。

为什么要画一个圆行区域的像素， 因为圆具有旋转不变性。每个特征点都会找灰度质心，每次要旋转到PQ为x轴上，方形无法旋转。但圆形区域就需要一些算法来实现。

灰度质心是指一小块图像中以每个像素的灰度值作为权重计算加权后的中心点。在上图以p点为中心的小块区域中，根据各个点的灰度值可以计算出一个灰度质心，通常不与p点重合，这样从p点到灰度质心的连线就是这个特征点的方向。

UMAX

(这里介绍一下 代码中有体现) 这一步是在求灰度质心前面完成的

umax在后面算方向很重要。umax是四分之一圆的每一行的u轴坐标边界。如下如的右上四分之一圆，横轴为u，竖轴为v，点在圆上滑动，其中横坐标到原点的距离就是umax，对应着图上三个点的AC、AH、AD距离是三个点的umax，E点的umax是0.

VMAX就是纵坐标(已知)，求UMAX。

这里解释一下为什么要有这个UMAX：

为了保证旋转不变性，所以必须在圆内运算；为了圆内运算，就要知道圆的边界，才能去索引像素点坐标（u，v）；为了知道圆的边界，在已知纵坐标VMAX的情况下，还需要求横坐标UMAX。

利用勾股定理 UMAX=sqrt(R*R-VMAX·VMAX)

求完UMAX之后，我们就可以得到X坐标，然后就能计算前面提到的m10和m01，然后根据公式算角，这也是为什么说UMAX对求方向很重要的原因。

IC_Angle做加速运算

要计算特征点的角度，我们要在一个圆域中算出m10和m01，计算步骤是先算出中间红线的m10，然后在平行于x轴算出m10和m01，一次计算相当于图像中的同个颜色的两个line。先算出中间红线的m10，然后在平行于x轴算出m10和m01，一次计算 相当于图像中的同个颜色的两个line。

图像金字塔

为什么会有图像金字塔呢？

因为存在一个问题“远处看着像是角点的地方，接近之后肯就不是角点了”

尺度不变性就由构建图像金字塔，并在金字塔的每一层上检测角点来实现。

代码

目前ORB提取的代码都在ORBextractor.cc里，里面有很多函数，我们先看ORBextractor的构造函数，其代码如下：

ORBextractor::ORBextractor(int _nfeatures,		//指定要提取的特征点数目
						   float _scaleFactor,	//指定图像金字塔的缩放系数
						   int _nlevels,		//指定图像金字塔的层数
						   int _iniThFAST,		//指定初始的FAST特征点提取参数，可以提取出最明显的角点
						   int _minThFAST):		//如果初始阈值没有检测到角点，降低到这个阈值提取出弱一点的角点
    nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
    iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数
{
	//存储每层图像缩放系数的vector调整为符合图层数目的大小
    mvScaleFactor.resize(nlevels);  
	//存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方
    mvLevelSigma2.resize(nlevels);
	//对于初始图像，这两个参数都是1
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
	//然后逐层计算图像金字塔中图像相当于初始图像的缩放系数 
    for(int i=1; i<nlevels; i++)  
    {
		//其实就是这样累乘计算得出来的
        mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
		//原来这里的sigma^2就是每层图像相对于初始图像缩放因子的平方
        mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];
    }

    //接下来的两个向量保存上面的参数的倒数
    mvInvScaleFactor.resize(nlevels);
    mvInvLevelSigma2.resize(nlevels);
    for(int i=0; i<nlevels; i++)
    {
        mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];
        mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
    }

    //调整图像金字塔vector以使得其符合设定的图像层数
    mvImagePyramid.resize(nlevels);

	//每层需要提取出来的特征点个数，这个向量也要根据图像金字塔设定的层数进行调整
    mnFeaturesPerLevel.resize(nlevels);
	
	//图片降采样缩放系数的倒数
    float factor = 1.0f / scaleFactor;
	//第0层图像应该分配的特征点数量
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));

	//用于在特征点个数分配的，特征点的累计计数清空
    int sumFeatures = 0;
	//开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面）
    for( int level = 0; level < nlevels-1; level++ )
    {
		//分配 cvRound : 返回个参数最接近的整数值
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
		//累计
        sumFeatures += mnFeaturesPerLevel[level];
		//乘系数
        nDesiredFeaturesPerScale *= factor;
    }
    //由于前面的特征点个数取整操作，可能会导致剩余一些特征点个数没有被分配，所以这里就将这个余出来的特征点分配到最高的图层中
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);

	//成员变量pattern的长度，也就是点的个数，这里的512表示512个点（上面的数组中是存储的坐标所以是256*2*2）
    const int npoints = 512;
	//获取用于计算BRIEF描述子的随机采样点点集头指针
	//注意到pattern0数据类型为Points*,bit_pattern_31_是int[]型，所以这里需要进行强制类型转换
    const Point* pattern0 = (const Point*)bit_pattern_31_;	
	//使用std::back_inserter的目的是可以快覆盖掉这个容器pattern之前的数据
	//其实这里的操作就是，将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量中
    std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));

    //This is for orientation
	//下面的内容是和特征点的旋转计算有关的
    // pre-compute the end of a row in a circular patch
	//预先计算圆形patch中行的结束位置
	//+1中的1表示那个圆的中间行
    umax.resize(HALF_PATCH_SIZE + 1);
	
	//cvFloor返回不大于参数的最大整数值，cvCeil返回不小于参数的最小整数值，cvRound则是四舍五入
    int v,		//循环辅助变量
		v0,		//辅助变量
		vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);	//计算圆的最大行号，+1应该是把中间行也给考虑进去了
				//NOTICE 注意这里的最大行号指的是计算的时候的最大行号，此行的和圆的角点在45°圆心角的一边上，之所以这样选择
				//是因为圆周上的对称特性
				
	//这里的二分之根2就是对应那个45°圆心角
    
    int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
	//半径的平方
    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;

	//利用圆的方程计算每行像素的u坐标边界（max）
    for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt(hp2 - v * v));		//结果都是大于0的结果，表示x坐标在这一行的边界

    // Make sure we are symmetric
	//这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称（如果按照常规的想法做，由于cvRound就会很容易出现不对称的情况，
	//同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了）
	for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        umax[v] = v0;
        ++v0;
    }
}

//IC_Angle做加速运算，计算m01,m10
static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
	//图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;

	//获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
	//这条v=0中心线的计算需要特殊对待
    //后面是以中心行为对称轴，成对遍历行数，所以PATCH_SIZE必须是奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
		//注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];

    // Go line by line in the circular patch  
	//这里的step1表示这个图像一行包含的字节总数。
    int step = (int)image.step1();
	//注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
		//本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
		// 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
		//在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
			//得到需要进行加运算和减运算的像素灰度值
			//val_plus：在中心线下方x=u时的的像素灰度值
            //val_minus：在中心线上方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
			//在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
			//u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
    }

    //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

特征点匹配

通过描述子的差异判断哪些特征为同一个点

暴力匹配：比较图1中每个特征和图2特征的距离（描述子距离具体表示两个特征之间的相似程度）

加速：快速最近邻（FLANN）更加适合于匹配点数量极多的情况。

由于这些匹配算法已经成熟，实现上也集成到opencv，具体就不再描述了，直接用吧。

根据匹配点估计相机的运动

特征匹配之后，得到了特征点之间的对应关系

• 如果只有两个单目图像，得到2D-2D间的关系 ——对极几何

• 如果匹配的是帧和地图，得到3D-2D间的关系 ——PnP

• 如果匹配的是RGB-D图，得到3D-3D间的关系 ——ICP

对极几何

O1,O2是相机光心或者说两个相机角度，e1和e2是O1和O2连线，与两图像的交点。

E尺度等价性，可以乘上任意一个常数，表示的还是一个意思。所以即使这里t是三个自由度，R是三个自由度，这里尺度等价性，E实质上有5个自由度。把E当成普通矩阵就是3*3，是9个自由度。

在奇异值分解过程中，是把E当成普通矩阵运算的，八个自由度可能就不满足E的内在性质要求，E实质是5个自由度。

八点法的讨论

1. 用于单目SLAM的初始化：

   因为单目，不像RGB-D和双目有深度信息，可以直接获得3D信息，单目需要通过对极几何先把3D信息建出来，然后再通过3D-2D匹配估计相机运动。

2. 尺度不确定性：归一化 t 或特征点的平均深度

   R是有约束的，正交且行列式为1；所以R不能乘任意值，但是E又可以乘以，这里就有t是自由的，但是我们又不希望t特别大或者特别小，所以一般会归一化为1。这就是单目的尺度不确定性。

3. 纯旋转问题：t=0 时无法求解 没有平移，算出来E就是0，没法求解，也就是初始化失败。单目初始化必须有平移

4. 多于八对点时：最小二乘

5. 有外点时：RANSAC （如果误匹配了，就用随机采样一致性去除外点）

单应矩阵

接下来就是类似八点法恢复R,t…还有几个其他的值，就不细讲了。

总结一下

1. 2D-2D情况下，只知道图像坐标之间的对应关系

• 当特征点在平面上时（例如俯视或仰视），使用H恢复R,t

• 否则，使用E或F恢复R,t

2. t 没有尺度 （归一化）

3. 求得R,t后：

• 利用三角化计算特征点的3D位置（即深度）

4. 实际中用于单目SLAM的初始化部分

望t特别大或者特别小，所以一般会归一化为1。这就是单目的尺度不确定性。

3. 纯旋转问题：t=0 时无法求解 没有平移，算出来E就是0，没法求解，也就是初始化失败。单目初始化必须有平移

4. 多于八对点时：最小二乘

5. 有外点时：RANSAC （如果误匹配了，就用随机采样一致性去除外点）

单应矩阵

[外链图片转存中…(img-4tWRpqpX-1706256052361)]

接下来就是类似八点法恢复R,t…还有几个其他的值，就不细讲了。

总结一下

1. 2D-2D情况下，只知道图像坐标之间的对应关系

• 当特征点在平面上时（例如俯视或仰视），使用H恢复R,t

• 否则，使用E或F恢复R,t

2. t 没有尺度 （归一化）

3. 求得R,t后：

• 利用三角化计算特征点的3D位置（即深度）

4. 实际中用于单目SLAM的初始化部分

有需要的话，后面的几章还会更的。