点击上方“3D视觉工坊”，选择“星标”
 
干货第一时间送达
 

 
本文由知乎作者Welson WEN授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/112620266
 
就目前视觉SLAM的引用来区分，分为基于特征法的和直接法的视觉SLAM。上一篇文章（https://zhuanlan.zhihu.com/p/108298657）中分析了特征法和直接法之间的对比关系。以下全篇均在分析基于视觉+IMU的视觉SLAM，即VINS。
 
基于特征法的视觉SLAM（视觉+imu）在目前发展相对更佳成熟，本文重点分析基于特征法的视觉SLAM的前端特征检测与跟踪问题。就目前来看，主流的框架还是基于fast方法进行特征快速检测，基于光流法进行特征跟踪。与ORB-SLAM不同，基于光流法进行特征跟踪可以甚至在高动态的场景下进行有效的特征跟踪。在高动态场景下，一个关键的问题就是图片容易模糊，基于ORB这种采用描述子进行特征匹配的特征跟踪方法，极易在高动态场景下特征跟踪失败。
 
jiacheng: 特征点法的话，以VINS为例，需要提前把路上运动中的车提前检测出来，去掉，这样是为了去掉动的特征点。车行驶环境中有很多重复的纹理和区域，感觉像VINS中用光流跟踪比计算描述子的特征点匹配，误匹配的情况可能要好一些，因为光流法会指定当前帧图像上的特征点的初始位置，也就是会在上一帧对应光流点的坐标值附近搜索和上一帧的匹配点。还有就是用特征点法地图重用要简单，只存储特征点，需要的时候拿出来就能恢复位姿了。
 
jiacheng: 我也理解光流法是对光照更加敏感一些才对，但是测试的时候发现，只有在非常严重的光照变化，比如从黑漆漆的树荫底下一下子暴露到阳光暴晒下，VO才会坚持不住丢掉。
 
下面简要的讲述一下几种描述子的特点以及相关的问题（特征描述子部分的材料参考了Dr. Yue的讲座介绍）：
 
SIFT(Scale Invariant Feature Transform)：该描述子主要具备尺度不变性和旋转不变性等优秀的品质，在视觉特征描述领域获得很好地口碑；描述子由128维的向量描述子组成。根据以下两个公式计算梯度幅值和梯度方向，基于特征点邻域内的像素对应的梯度信息，以获得旋转描述子的旋转不变性，最终获得128维的描述子。
 
 
 

 
Harris 特征：该特征描述方式主要依赖于特征点与邻域内的相关系数值。其中相关系数计算如下：
 
 
 
其中  表示特征点为中心的邻域范围。 计算如下：
 

 
因此可以的到：
 
 
 
其中：
 
 
 
因此可得到简化形式如下：
 
 
 
Harris特征的优势之一是速度快，虽然 Harris 算法相较于其他众多基于灰度的角点提取算法具有明显的优势，但它仍然存在一些不足: 在经典的 Harris 角点检测中，当对角点的兴趣值进行非极大值抑制来确定局部极大值的时候，角点的提取效果几乎完全由设定的阈值大小决定。而阈值的大小也与所提取的角点数量息息相关，一般情况下，阈值越大提取的角点越少，极易造成正确角点的丢失; 阈值越小提取的角点数越多，也会带来很多伪角点。因此，在用 Harris 算法进行角点检测时，阈值这个经验值的选取和设定对角点提取具有很大的影响。这部分参考https://blog.csdn.net/weixin_41695564/article/details/79962401。
 
Fast特征：Fast特征的提出者Rosten等将Fast角点定义为：若某像素与其周围邻域内足够多的像素点相差较大，则该像素可能是角点。
 
1, 一个以像素p为中心，半径为3的圆上，有16个像素点（p1、p2、...、p16）。
 
2、定义一个阈值。计算p1、p9与中心p的像素差，若它们绝对值都小于阈值，则p点不可能是特征点，直接pass掉；否则，当做候选点，有待进一步考察；
 
3、若p是候选点，则计算p1、p9、p5、p13与中心p的像素差，若它们的绝对值有至少3个超过阈值，则当做候选点，再进行下一步考察；否则，直接pass掉；
 
4、若p是候选点，则计算p1到p16这16个点与中心p的像素差，若它们有至少9个超过阈值，则是特征点；否则，直接pass掉。
 
5、对图像进行非极大值抑制：计算特征点出的FAST得分值（即score值，也即s值），判断以特征点p为中心的一个邻域（如3x3或5x5）内，计算若有多个特征点，则判断每个特征点的s值（16个点与中心差值的绝对值总和），若p是邻域所有特征点中响应值最大的，则保留；否则，抑制。若邻域内只有一个特征点（角点），则保留。得分计算公式如下（公式中用V表示得分，t表示阈值）：
 

 
该fast特征检测方法以快著称，清晰明了，在很多VINS中都采用这种检测方式。
 
小结：
 
以无人驾驶，无人机这种高动态的场景来说，以视觉SLAM为背景，fast + 光流的场景将会是一种主流方案。我们的研究（Bai, Xiwei, Weisong Wen, and Li-Ta Hsu. "Robust Visual-Inertial Integrated Navigation System Aided by Online Sensor Model Adaption for Autonomous Ground Vehicles in Urban Areas." (2020).）也发现，基于fast+光流的方式即使在晚上高动态的场景下，也可以获得相对不错的相对定位精度，全程没有fail掉，但是整体精度依旧有待改善。

 
基于ORB这种高效的描述子的方式，在基于视觉地图做重定位的场合，具有很好的优势，比如类似于VPS等场合。
 
从全局上讲，典型的VINS主要包括两部分；即前端和后端。主要的VINS的误差来源来自于一下几部分。
 
特征检测的精度：由于后期的特征跟踪需要检测出前后属于同一个特征的特征点，如果特征检测的精度误差有0.5个pixel，那么反应到3D定位精度上，可能会是一个不小的误差。
 
特征跟踪精度：在使用光流法跟踪过程中，由于特征跟踪错误，导致计算重投影误差出错，最终的优化求解误差增大。
 
VINS状态初始化精度：由于VINS的整个优化问题是一个极度非线性的问题，因此一个好的初始化解是优化求解达到最优的关键。总的来说，一个好的初始化解，是优化达到最优解的关键之一。
 
匀速运动的自身车辆，导致视觉尺度不可观：由于视觉的尺度需要基于IMU的观测量来恢复，当车辆处于匀速运动的时候，由于加速度为0，将会导致视觉尺度不可观。
 
高动态环境下的动态目标：由于视觉重投影的残差依赖于一个假设：即特征是静止的。然而在高动态环境下，动态车辆上检测的特征将会极大的影响精度。我们组近期的文章在解决这些问题（Bai, Xiwei, Weisong Wen, and Li-Ta Hsu. "Robust Visual-Inertial Integrated Navigation System Aided by Online Sensor Model Adaption for Autonomous Ground Vehicles in Urban Areas." (2020).）
 
参考文献：
 
[1] https://www.cnblogs.com/wyuzl/p/7834159.html
 
[2]https://zhuanlan.zhihu.com/p/108298657
 
[3]https://blog.csdn.net/weixin_41695564/article/details/79962401
 
上述内容，如有侵犯版权，请联系作者，会自行删文。

 
推荐阅读：
吐血整理|3D视觉系统化学习路线
那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）
超全的3D视觉数据集汇总
大盘点|6D姿态估计算法汇总(上)
大盘点|6D姿态估计算法汇总（下）
机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划
汇总|3D点云目标检测算法
汇总|3D人脸重建算法
那些年，我们一起刷过的计算机视觉比赛
总结|深度学习实现缺陷检测
深度学习在3-D环境重建中的应用
汇总|医学图像分析领域论文
大盘点|OCR算法汇总
重磅！3DCVer-知识星球和学术交流群已成立
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导，700+的星球成员为创造更好的AI世界共同进步，知识星球入口：
 
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
 圈里有高质量教程资料、可答疑解惑、助你高效解决问题
 
欢迎加入我们公众号读者群一起和同行交流，目前有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
 

 
▲长按加群或投稿
 

目录
 
直接法介绍
单层直接法
多层直接法
 
直接法介绍
 
  这里所说的直接法不同于上一篇所说的LK光流法，LK光流是首先要选取特征点，如何利用特征点附近的光流进行路标的追踪与位姿的求解的，其本质还是先得到匹配好的点对，然后利用点对进行位姿估计。而直接法不同，其核心思想是直接优化位姿$R,t$。
 直接法也需要两个假设：
 
1.灰度不变假设，即$I_1(u,v)=I_2(u+\Delta x,v+\Delta y)$
 2.小范围灰度不变假设，即存在$W\times W$的窗口，使$I_1(u+w,v+w)=I_2(u+\Delta x+w,v+\Delta y+w)$
   同LK光流法一样，假设1是基础，假设2则是为了构建最小二乘法。
 基本理论如下：
 
  同LK相比，直接法舍弃了$\Delta x,\Delta y$作为优化变量，而是直接利用变换矩阵（位姿）$T$作为优化变量，因为相邻图像上的关键点坐标的移动本质是因为相机在运动。这里理论不会太过详细，后期会单独写SLAM的理论。
 
  设$P$点的像素坐标为$(u,v)$，相机坐标为$(X,Y,Z)$。在已知路标的深度，相机内参的情况下，利用简单的缩放与平移可得到相机坐标。
 
   如上图所示，图1中的$p(u,v)$变换到了图2后，坐标为$p^{\prime }=KTP$,其中$K$是相机内参矩阵，是已知的，基于前面的假设，可以得到以下等式：
 
$error=I_1(u,v)-I_2(KTP)$,这个形式不规范，能理解就行。
 
$Cost={\sum }_{j=1}^w{\sum }_{i=1}^w{I}_1(u+w,v+w)-I_2(KTP+W)$
 
   同理，等式1基于假设1，等式2基于假设2。至于求导过程可参考《视觉SLAM十四讲》，这里也不做过多介绍。
 
单层直接法
 
直接上代码，里面有注解与注意事项。
 
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
	int max_patch_size = 1;  //w的大小
	int cnt_good = 0; 

	Matrix6d hession = Matrix6d::Zero(); //H矩阵
	Vector6d bias = Vector6d::Zero();  //b矩阵

	//double lastcost = 0;
	double cost_tmp = 0;
	//前提是进行坐标变换，图1的像素坐标系到归一化坐标系
	
	for (int i = range.start; i < range.end; i++)
	{
		
		Eigen::Vector3d point_ref = depth_ref[i]*  Eigen::Vector3d((pix_ref[i][0] - cx) / fx, (pix_ref[i][1] - cy) / fy, 1);
		//变换到P(X,Y,Z)
		Eigen::Vector3d point_cur = T * point_ref;
		//T*P

		//检查d是否存在
		if (point_cur[2]<0)
			continue;

		double X = point_cur[0]; 
		double Y = point_cur[1];
		double Z = point_cur[2]; 
		//以上得到了位姿变换后相机坐标

		double Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;

		float u = fx * X / Z + cx;
		float v = fy * Y / Z + cy;
		//得到了位姿变换后的像素坐标(u,v)

		//检查变换后的坐标是否越界
		if (u<max_patch_size || u>img_estimate.cols - max_patch_size ||
			v<max_patch_size || v>img_estimate.rows - max_patch_size)
			continue;
		projection[i] = Eigen::Vector2d(u, v);

		cnt_good++;			
	
		for (int x = -max_patch_size; x <= max_patch_size; x++)
		
			for  (int y = -max_patch_size;  y <= max_patch_size;  y++)
			{
				//开始计算error和j矩阵
				float error = GetPixelValue(img, pix_ref[i][0] + x, pix_ref[i][1] + y) - GetPixelValue(img_estimate, u + x, v + y);
				
                 //开始计算导数
				matrix26d J_pixel_xi;
				Eigen::Vector2d J_img_pixel;
				J_pixel_xi(0, 0) = fx * Z_inv;
				J_pixel_xi(0, 1) = 0;
				J_pixel_xi(0, 2) = -fx * X * Z2_inv;
				J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
				J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
				J_pixel_xi(0, 5) = -fx * Y * Z_inv;

				J_pixel_xi(1, 0) = 0;
				J_pixel_xi(1, 1) = fy * Z_inv;
				J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
				J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
				J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
				J_pixel_xi(1, 5) = fy * X * Z_inv;

				J_img_pixel = Eigen::Vector2d(
					0.5*(GetPixelValue(img_estimate, u + x + 1, v + y) - GetPixelValue(img_estimate, u + x - 1, v + y)),
					0.5*(GetPixelValue(img_estimate, u + x, v + y + 1) - GetPixelValue(img_estimate, u + x, v + y - 1))
				);
				Vector6d J = -1.0*(J_img_pixel.transpose()*J_pixel_xi).transpose();


				hession += J * J.transpose();
				bias += -error * J;
				cost_tmp += error * error;

			}	

	}

	if (cnt_good) {
		// set hessian, bias and cost
		unique_lock<mutex> lck(hession_mutex);
		H += hession;
		b += bias;
		cost += (cost_tmp / cnt_good);
	}	
}
 
接下来是Gauss-Newton的主体部分
 
for (int iter = 0; iter < iteration; iter++)
	{
		jacpbian.reset();  //初始化

		//求解方程
		cv::parallel_for_(cv::Range(0, pixels_ref.size()),
			std::bind(&JacobianAccumulator::accumulate_jacobian, &jacpbian, std::placeholders::_1));  //并行求解

		Matrix6d H = jacpbian.hession();
		Vector6d b = jacpbian.bias();
		double lastcost = 0, cost = 0;

		Vector6d update = H.ldlt().solve(b)；
		T = Sophus::SE3d::exp(update)*T;
		cost = jacpbian.cost_function();
		//接下来考虑三个方面：更新量是否为无穷，增量方向，收敛误差
		
		if (std::isnan(update[0]))
		{
			cout << "更新量为无穷" << endl;
			break;
		}

		if (lastcost>cost)
		{
			cout << "增量方向错误" << endl;
			break;
		}

		if (update.norm()<1e-3)
		{
			break;
		}
		lastcost = cost;
		cout << "iteration: " << iter << ", cost: " << cost << endl;
	}
 
注意：
   要创建一个accumulate_jacobian类，这个类主要是用于计算导数，而高斯牛顿法在外部的函数中调用该类完成，体现了面向对象的编程思维。
 
多层直接法
 
  多层直接法的核心是金字塔，分为三部分：
 
定义金字塔
创建金字塔
求解
 
代码如下：
 
void multy_derect_method(
	const Mat & img1,
	const Mat & imag_estimate,
	const Vecvector2d & px_ref,
	const vector<double>& depth_ref,
	Sophus::SE3d & T) {

	//金字塔方法的步骤，1.定义金字塔参数，2.创建金字塔（图像＋参数） 3.调用单层算法

	//1.定义基本参数:层数，尺度，尺度数组
	int pyramid_size = 4;
	double scal = 0.5;
	double pyramid_scals[] = { 1,0.5,0.25,0.125 };


	//2.创建金字塔
	vector<Mat> pyr_img, pyr_imgestimate;

	for (int i = 0; i < pyramid_size; i++)
	{
		if (i == 0)
		{
			pyr_img.push_back(img1);
			pyr_imgestimate.push_back(imag_estimate);
		}
		else
		{
			Mat img_tmp, imgestimate_tmp;

			cv::resize(pyr_img[i - 1], img_tmp,
				cv::Size(pyr_img[i - 1].cols*scal, pyr_img[i - 1].rows*scal));
			cv::resize(pyr_imgestimate[i - 1], imgestimate_tmp,
				cv::Size(pyr_imgestimate[i - 1].cols*scal, pyr_imgestimate[i - 1].rows*scal));
			pyr_img.push_back(img_tmp);
			pyr_imgestimate.push_back(imgestimate_tmp);

		}//以上为图像金字塔
	}

	double fxG = fx, fyG = fy, cxG = cx, cyG = cy;
	for (int level = pyramid_size - 1; level >= 0; level--)
	{
		fx = fxG * pyramid_scals[level];
		fy = fyG * pyramid_scals[level];
		cx = cxG * pyramid_scals[level];
		cy = cyG * pyramid_scals[level];
		Vecvector2d px_pyr_ref;
		for (auto &kp : px_ref)
		{		
			px_pyr_ref.push_back(pyramid_scals[level] * kp);	
		}
		single_derect_method(pyr_img[level], pyr_imgestimate[level], px_pyr_ref, depth_ref, T);
	}
}
 
注意：
 1.金字塔的层数是从第0层开始的，这样比较符合编程的思维。
 2.金字塔不仅仅指图像，只要是需要缩放的各类参数都要建立金字塔。
 3.层与层直接的参数传递主要是位姿矩阵T，T传入的是引用，不需要初始化，定义时默认初始化为单位矩阵，每层求解后T均被保存下来。
 
效果如下：
 
单层法：
 
 多层法：
 
   实际上，理论与代码的实现有着巨大的鸿沟，代码中还有很多细节部分没有理解清楚。加油！
 
参考文献：
 [1]《视觉SLAM十四讲》 高翔

SLAM(Simultaneous localization and mapping)即同步定位与建图，应用场景一般为机器人导航，场景识别等场景；SLAM的主要实现过程：通过跟踪运动中的相机，估算出来其在每个时刻的位置和姿态，并将相机在不同时刻获取的图像融合重建成完整的三维地图。传统的SLAM可分为视觉前端和优化后端两大模块。
 
视觉前端：
 
总体概括来说，视觉前端主要完成的任务为：利用运动中的相机在不同时刻获取到的图像帧，通过特征匹配，求解除相邻域之间的相机位姿变换，并完成图像帧之间的融合，重建除地图。视觉前端也是一个局部优化的过程，由于进行优化约束条件仅限于相邻图像帧，因此在相机运动过程中，容易产生误差漂移。
 
视觉前端主要依赖于机器人搭载的传感器，常用的传感器有相机（单目相机，双目相机，TOF深度相机）IMU（惯性测量单元），激光雷达等，单纯通过视觉传感器（相机）来完成的SLAM又称为视觉SLAM。若使用单目相机作为视觉传感器，优点是成本低，硬件简单，缺点是算法复杂，对相机的运动具有一定的条件要求，因为其通过SFM算法弯沉深度信息的计算，因此若相机在运动时只是发生了旋转而没有发生平移的话，则无法通过三角测量来计算目标的深度信息。由于SFM算法计算出的深度信息并不是真实的深度信息，因为它毕竟不是真正的双目深度重建。
 
若使用双目相机作为视觉传感器，优点是获取点云数据时，受物体表面材质的反光性质的影响比使用辅助光源的深度传感器方案（如TOF，结构光等）要小，同时其测量距离也相较其他方案要更远，深度图的分辨率更高。缺点是计算复杂度大（计算图像特征及完成特征匹配），同时对于特征的设计具有高鲁棒性的要求。这导致双目视觉SLAM在建立稠密地图（dense map）和算法实时性上具有先天的劣势。
 
若使用结构光方案的深度相机作为视觉传感器，与双目相机方案大同小异。原理都是利用了双目视差和三角测量，不同之处在于，结构光方案对图像的特征进行了编码，该编码特征在计算和匹配时相比双目相机方案的手工设计特征（如ORB，SIFT等）鲁棒性更好，计算更快，缺点是结构光对物体表面的材质敏感，如果是具有反射、透射性质的表面，或者是漫反射过强的表面，都会出现结构光编码信息的丢失，从而无法正确地匹配特征，导致深度信息的缺失。
 
使用TOF方案的深度相机作为视觉传感器，优点是帧率高，无需通过特征计算及匹配来获取深度信息，因此计算复杂度低，用于SLAM上具有更优的实时性。缺点是同结构光方案，对物体表面的材质比较敏感。此外，TOF相机在图像分辨率和测量距离上，也同结构光方案，不如双目相机的方案。
 
在视觉前端中，主要的任务是完成对相机位姿的估计和建图，这时得到的还是一个局部优化的粗略结果，后续还需要通过后端优化，利用更多约束条件对其进行微调，以获得精确的位姿和地图。
 
估计相机位姿的常用算法是ICP，简而言之是寻找源点集中的每个点在目标点集中的最近邻点作为其对应点，建立起多组对应点。对于每组对应点，都用同一个仿射变换矩阵来描述这组对应点的坐标之间的关系（即旋转和平移关系），建立起一个超定方程组，求解出该仿射变换矩阵。计算出该仿射变换矩阵后，将其应用于源点集上，使源点集整体旋转平移到目标点集上，再计算变换后的各组对应点的坐标的总误差，如果小于给定阈值则停止迭代，如果大于给定阈值，则重复上述过程，直到两个点集的对齐收敛。
 
在SLAM中的做法是，通过ICP来完成相邻帧之间的图像配准，进而得到相邻帧之间的相机坐标系之间的旋转平移关系。通常以初始位置的相机坐标系作为全局坐标系，每一帧对应的相机坐标系相对于全局坐标系的旋转平移用于描述该帧对应的时刻的相机的位姿。由于所有相邻帧对之间的旋转平移都可以通过对该相邻帧对进行配准来得到，因此每一帧与第一帧之间的旋转平移也就都可以通过这些中间过程的旋转平移来计算得到，从而得到每一帧的相机位姿。
 
除了通过ICP算法直接对相邻帧的点云进行三维配准，图像配准还可以通过重投影算法来完成： 本质上是最小化重投影误差，即先将当前帧的点云投影到前一帧的相机坐标系中，再从前一帧的相机坐标系投影到前一帧的图像坐标系中；同时，前一帧的点云也重投影到其图像坐标系中。通过最小化该图像坐标系上这两个点集里所有对应点对的总误差，来得到当前帧和前一帧的相机坐标系之间的变换矩阵。相邻帧的点云可以使用重投影算法来完成配准，相邻帧的RGB图像也可以使用重投影算法来完成配准，前者称之为几何位姿估计，后者称之为光度位姿估计。
 
相机的位姿由一个旋转变换和一个平移变换来描述。由于相机位姿的求解是一个非线性优化问题，通常的一种求解思路是利用目标函数的梯度下降方向去寻找极值点，即通过不断地调整下降方向和搜索步长，以迭代的方式去逼近极值点。
 
在完成了位姿估计后，就可以通过每一帧的全局位姿，不断地将新的图像帧中的点云变换到全局坐标系中，与已重建的全局地图/模型完成融合，逐帧地更新重建地图。对于稀疏SLAM而言，在视觉前端中的建图部分至此就结束了。而对于稠密SLAM而言，此时得到的地图不仅是不精确的，而且在可视化上式不够直观的，就是一群离散的点。所以对于稠密SLAM而言，还多了一个步骤就是对地图/模型进行渲染。
 
至此，SLAM流程之前端优化完成了，谢谢各位，若有不足，还请多多指导！

基于直接法的方法总结：
 
       SLAM 主要分为视觉前端和优化后端。前端也称为 视觉里程计（VO）。它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。VO 的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直接法前端。尽管特征点法在视觉里程计中占据主流地位，研究者们认识它至少有以下几个缺点：
 
关键点的提取与描述子的计算非常耗时。
使用特征点时，忽略了除特征点以外的所有信息。
相机有时会运动到特征缺失的地方，往往这些地方都没有什么明显的纹理信息。
我们看到使用特征点确实存在一些问题。针对这些缺点，也存在若干种可行的方法：
 
只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算需要一定时间；
只计算关键点，不计算描述子。同时，使用直接法来计算特征点在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加简单。
既不计算关键点、也不计算描述子——根据像素来直接计算相机运动。
       第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时仍使用PnP或ICP算法。而在，后两个方法中，我们会根据图像的像素信息来计算相机运动，它们称为直接法。
 
       使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）来优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们为何要对特征进行匹配或跟踪的理由。而在直接法中，我们最小化的不再是重投影误差，而是测量误差（Phometric error）。
 　　直接法是本讲介绍的重点。它的存在就是为了克服特征点法的上述缺点（虽然它会引入另一些问题）。直接法直接根据像素亮度信息，估计相机的运动，可以完全不用计算关键点和描述子。于是，直接法既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像特征），直接法就能工作
 
1.光流法
 
  光流是一种描述像素随时间在图像之间运动的方法，计算部分像素的称为 稀疏光流，计算所有像素的称为 稠密光流。稀疏光流以 Lucas-Kanade光流 为代表，并可以在SLAM中用于跟踪特征点位置。
 
 
 
 
LK光流推导过程:
 
灰度不变假设
 
 
对左边一阶泰勒展开:
 
 
则
 
 
简写为
 
 
在LK光流中，假设某个窗口（w x w）内的像素具有相同的运动
 
 
简写为
 
 
从而得到图像间的运动速度或者某块像素的位置。
 
代码：
 
OpenCV 中相关函数：calcOpticalFlowPyrLK：C++ calcOpticalFlowPyrLK（）浅析
 
int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";
    
    ifstream fin( associate_file );
    if ( !fin ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
        return 1;
    }
    
    string rgb_file, depth_file, time_rgb, time_depth;
    list< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    cv::Mat color, depth, last_color;
    
    for ( int index=0; index<100; index++ )
    {
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread( path_to_dataset+"/"+rgb_file );
        depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );
        if (index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect( color, kps );
            for ( auto kp:kps )
                keypoints.push_back( kp.pt );
            last_color = color;
            continue;
        }
        if ( color.data==nullptr || depth.data==nullptr )
            continue;
        // 对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_keypoints; 
        vector<cv::Point2f> prev_keypoints;
        for ( auto kp:keypoints )
            prev_keypoints.push_back(kp);
        vector<unsigned char> status;
        vector<float> error; 
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
        cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;
        // 把跟丢的点删掉
        int i=0; 
        for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
        {
            if ( status[i] == 0 )
            {
                iter = keypoints.erase(iter);
                continue;
            }
            *iter = next_keypoints[i];
            iter++;
        }
        cout<<"tracked keypoints: "<<keypoints.size()<<endl;
        if (keypoints.size() == 0)
        {
            cout<<"all keypoints are lost."<<endl;
            break; 
        }
        // 画出 keypoints
        cv::Mat img_show = color.clone();
        for ( auto kp:keypoints )
            cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
        cv::imshow("corners", img_show);
        cv::waitKey(0);
        last_color = color;
    }
    return 0;
}
 
2.直接法
 
 
推导公式：
 
 
其中 Z1 是 P 的深度，Z2 是 P 在第二个相机坐标系下的深度，也就是 RP + t 的第三个坐标值；
 
       在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的；所以要求解一个优化问题不是重投影误差，而是测量误差（Photometric Error），也就是 P的两个像的亮度误差：
 
 
优化该误差的目标函数：
 
 
直接法的优缺点总结
 
优点：
 
 
可以省去计算特征点、描述子的时间。
 
 
只要求有像素梯度即可，无须特征点。因此，直接法可以在特征缺失的场合下使用。
 
 
可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。
 
缺点：
 
 
非凸性。直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。
 
 
单个像素没有区分度。
 
 
灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。
 
参考：
 
1.https://blog.csdn.net/u011178262/article/details/89685134
 
2.https://blog.csdn.net/u011178262/article/details/85016981
 
3.https://blog.csdn.net/zfjBIT/article/details/96008619