目录

直接法介绍
单层直接法
多层直接法

直接法介绍
  这里所说的直接法不同于上一篇所说的LK光流法，LK光流是首先要选取特征点，如何利用特征点附近的光流进行路标的追踪与位姿的求解的，其本质还是先得到匹配好的点对，然后利用点对进行位姿估计。而直接法不同，其核心思想是直接优化位姿$R,t$。

直接法也需要两个假设：
1.灰度不变假设，即$I_1(u,v)=I_2(u+\Delta x,v+\Delta y)$

2.小范围灰度不变假设，即存在$W×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十四讲》 高翔

目录：

LK光流介绍
单层LK光流
多层LK光流

LK光流
  LK光流是一种描述图像运动的方法，利用LK光流可以实现对图像的追踪，从而求解图像运动的位姿。其基本思想如下：



  img1,img2分别为两张已知的图像，相机在运动过程中，img1中的特征点$P(u,v)$经过变换后得到了img2中的$P^\prime(u+\Delta x,v+\Delta y)$。
  LK光流法求解相机位姿的基本问题可以描述为：已知$P$点，求解最佳的$\Delta x,\Delta y$，从而得到img2中的$P^\prime$坐标。因此便求得一对匹配点$P-P^\prime$。在求得多对匹配点的坐标后，整个位姿估计问题可以转换为$ICP$或$PnP$问题。
  为了求解最佳的$\Delta x,\Delta y$，通常采用最小二乘法。在LK光流中有两条非常重要的假设：
1.灰度不变假设，即$I(u,v)=I(u+\Delta x,v+\Delta y)$

2.小范围灰度不变假设，即存在$W×W$的窗口，使$I(u+w,v+w)=I(u+\Delta x+w,v+\Delta y+w)$

  假设1是基础，假设2则是为了构建最小二乘解。
单层光流法
求解的基本流程如下：

特征点的选取
Gauss-Newton法求解$\Delta x,\Delta y$

1.特征点的选取

  关于特征点的选取，笔者前面写了一篇文章进行了详细介绍，这里不在赘述。详情可点击此处.
vector<KeyPoint> kp1;
	vector<KeyPoint> kp2_single;
	Ptr<GFTTDetector> detector = cv::GFTTDetector::create(500, 0.01, 20);
	detector->detect(img1, kp1);

  这里使用的是GFFT特征。
2.Gauss-Newton求解$\Delta x,\Delta y$
  根据假设1有：$error=I_1(u,v)-I_2(u+\Delta x,v+\Delta y)$,这是基于假设1所得到了，理论上error应该为0，由于噪声的存在，实际上不可能为0，因此只能求解最小二乘解。
  根据假设2有$Cost=\sum_{j=1}^{w}\sum_{i=1}^{w}I_1(u+w_,v+w)-I_2(u+\Delta x+w,v+\Delta y+w)$。
  导数$J$是img2的像素梯度，也可以用img1的像素梯度代替，此时为反向光流法。具体形式可参考《视觉SLAM十四讲》。
代码如下：
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) { //参数是一个cv::range的引用


// parameters                                      range是一个区间，应该看作一个窗口
	//第一步：初始化工作：定义全局变量
	int iteration = 15;  //迭代次数
	int max_patch_size = 4;  //w的大小
	for (int i = range.start; i < range.end; i++)  //初始化变量工作
	{
		auto kp = kp1[i];
		double dx = 0, dy = 0;
		Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
		Eigen::Vector2d b = Eigen::Vector2d::Zero();
		Eigen::Vector2d J;
		double cost = 0;
		double lastcost = 0;
		bool succ = true;
		if (has_initial)   //金字塔的核心
		{
			dx = kp2[i].pt.x - kp.pt.x;
			dy = kp2[i].pt.y - kp.pt.y;  //这个值的加减顺序不能变，
			

		} //初始化或者不初始化都可以

		//第二部：计算e,cost,h,b,j
		for (int iter = 0; iter < iteration; iter++)
		{
			if (inverse == false) {
				H = Eigen::Matrix2d::Zero();
				b = Eigen::Vector2d::Zero();
			}
			else {
				// only reset b
				b = Eigen::Vector2d::Zero();
			}
			cost = 0;

           // 两个for循环为w窗口的迭代
			for (int x = -max_patch_size; x < max_patch_size; x++)
			{
				for (int y = -max_patch_size; y < max_patch_size; y++)
				{
					double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
						GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;

					if (inverse == false)   //inverse是反向的意思
					{

						J = -1.0 * Eigen::Vector2d(  //正向梯度，img2
							0.5*(GetPixelValue(img2, kp.pt.x + x + dx + 1, kp.pt.y + y + dy) -
								GetPixelValue(img2, kp.pt.x + x + dx - 1, kp.pt.y + y + dy)),
							0.5*(GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy + 1) -
								GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
						);

					}
					else if (iter == 0)
					{
						J = -1.0 * Eigen::Vector2d(  //反向梯度，img1
							0.5*(GetPixelValue(img1, kp.pt.x + x + dx + 1, kp.pt.y + y + dy) -
								GetPixelValue(img1, kp.pt.x + x + dx - 1, kp.pt.y + y + dy)),
							0.5*(GetPixelValue(img1, kp.pt.x + x + dx, kp.pt.y + y + dy + 1) -
								GetPixelValue(img1, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
						);

					}

					cost += error * error;
					b += -J * error;

					if (iter == 0 || inverse == false)
					{
						H += J * J.transpose();
					}

				}

			}

			//第三部分：第i次迭代完毕，开始求解/

			Eigen::Vector2d update = H.ldlt().solve(b);
			//求解后不要急于迭代，先看看结果是否正确

			if (std::isnan(update[0])) {
				// sometimes occurred when we have a black or white patch and H is irreversible
				cout << "update is nan" << endl;//判断是不是为无穷大,
				succ = false;
				break;
			}

			if (iter > 0 && cost > lastcost) {  //判断损失函数的增长方向
				break;
			}

			lastcost = cost;
			dx += update(0, 0);
			dy += update(1, 0);
			succ = true;//这一句放在这可以保证每次迭代都不出问题

			if (update.norm() < 1e-2) {
				// converge
				break;
			}



		}
		success[i] = succ;

		//把特征点放到kp2里面  注意：keypoint.pt返回的是Point2f而不是2d；
		kp2[i].pt = kp.pt + Point2f(dx, dy);  //把结果返回

	}
}

多层光流法
  多层光流法的基本思想是在单层的基础上，多次运用单层光流法，同时保存每层的求解结果并作为下一层的初始值。这样可以逐层求解，得到比较实用的光流法。其中，最关键的工具应该是金字塔。



  图像金字塔其实是在原图像的基础上，对图像进行缩放，当然，各参数也要进行相应的缩放。一般顶层图像最粗糙，底层为原图，求解过程由上至下，求解结果逐步精确。多层LK光流法的流程如下：

定义金字塔：一般是定义容器
创建金字塔：一般是利用循环以及resize函数，给容器赋值
求解：逐层调用单层的求解算法，但要注意各层之间的结果是如何传递的。

代码如下：
void OpticalFlowMultiLevel(
	const Mat &img1,
	const Mat &img2,
	const vector<KeyPoint> &kp1,
	vector<KeyPoint> &kp2,
	vector<bool> &success,
	bool inverse) {

	//定义全局参数
	int pyramid_size = 4;
	double pyramid_scal[] = { 1.0,0.5,0.25,0.125 };
	double scal = 0.5;

	//创建金字塔：第0层为底层，第3层为顶层
	vector<Mat> img1_pyramid, img2_pyramid;
	for (int i = 0; i < pyramid_size; i++)
	{
		if (i == 0) //第0层为原图
		{
			img1_pyramid.push_back(img1);
			img2_pyramid.push_back(img2);
		}
		else
		{
			Mat pyramid1, pyramid2; //利用好resize函数
			resize(img1_pyramid[i - 1], pyramid1,
				cv::Size(img1_pyramid[i - 1].cols*scal, img1_pyramid[i - 1].rows*scal));
			img1_pyramid.push_back(pyramid1);

			resize(img2_pyramid[i - 1], pyramid2,
				cv::Size(img2_pyramid[i - 1].cols*scal, img2_pyramid[i - 1].rows*scal));
			img2_pyramid.push_back(pyramid2); //图像金字塔仅仅作为导数的计算用
		}
	}
	//同理，坐标也要创建为金字塔
	vector<KeyPoint> kp1_pyramid, kp2_pyramid;
	for (auto kp_top : kp1)
	{
		kp_top.pt *= pyramid_scal[pyramid_size - 1];
		kp1_pyramid.push_back(kp_top);
		kp2_pyramid.push_back(kp_top);
	}

	//开始迭代了

	for (int i = pyramid_size - 1; i >= 0; i--)
	{
		success.clear();
		OpticalFlowSingleLevel(img1_pyramid[i], img2_pyramid[i], kp1_pyramid, kp2_pyramid, success, inverse, true);
	

		if (i > 0)
		{
			for (auto &kp : kp1_pyramid) {
				kp.pt /= scal;
			}
			for (auto &kp : kp2_pyramid) {
				kp.pt /= scal;
			}
		}
	}
	//迭代完毕后的kp2是最需要的
	for (auto &kp_end : kp2_pyramid) { kp2.push_back(kp_end); };
}


注意：

1.坐标的修正

if (i > 0)

{

for (auto &kp : kp1_pyramid) {

kp.pt /= scal;

}

for (auto &kp : kp2_pyramid) {

kp.pt /= scal;

}
  这里是利用参数的引用间接地修改了关键点的坐标，其实kp1_pyramid是没有变化的，只是对原坐标缩放，关键是kp2_pyramid，kp2_pyramid是求解得到的特征点。
  从顶层到底层，与kp2_pyramidkp1_pyramid的匹配程度越来越高，在每层求解后均需要进行缩放，以便于下一层使用。
2.求解结果的传递
OpticalFlowSingleLevel(img1_pyramid[i], img2_pyramid[i], kp1_pyramid, kp2_pyramid, success, inverse, true);
  认真理解 true  这个参数，我们翻到单层求解的算法中，true对应的参数是has_initial，相应的功能是改变dx,dy的初始化方式，前面说过，LK光流法的关键是求解dx,dy。而多层LK光流是步骤得到好的dx,dy初始值。
当has_initial=true时：

  dx = kp2[i].pt.x - kp.pt.x;

  dy = kp2[i].pt.y - kp.pt.y;
  kp2是上一层求解的结果，经过坐标缩放后，用于初始化dx,dy，这也说明了上层求解得到的kp2是如何在下一层被利用的。
当has_initial=false时：

  dx=0,dy=0。此时为单层求解
  因此，金字塔的核心是关于被优化变量的初始化方式。思想是：每层初始化越接近真实值越好。
参考文献：

[1]《视觉SLAM十四讲》高翔

                    

                    

                    
                    
                    
点击上方“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视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加群或投稿
				

目录

Mat 类
Point2d/Point3d 类
Vector2d/Vector3d 类
Sophus::SE3d/SO3d 类

Mat类
  Mat类是opencv中图像存储矩阵，常常与cv::imread()函数一起使用，注意的是，Mat矩阵不能与Eigen库中的矩阵进行任何数学运算，需要时必须进行转换。Mat类也存放一些固定的参数，如相机内参等等，对Mat类一个最重要操作便是访问矩阵内的元素。
1.利用Mat.ptr访问
 cv::Mat image = cv::Mat(400, 600, CV_8UC1); 
 //创建一个Mat类的image矩阵
double d=image.ptr<unsigned short>(int(y))[int(x)];
//访问（x,y）处的元素

2.利用Mat.at访问
 cv::Mat image = cv::Mat(400, 600, CV_8UC1); 
 //创建一个Mat类的image矩阵
double d=image.at<double>(y, x);
//访问（x,y）处的元素

注意：（x,y）是图像的像素坐标系，与一般的坐标系原点不同，在图像的左上角。对于矩阵便是：第x行，第y列的元素。
3.Mat矩阵的手动输入元素
cv::Mat y=(Mat_<double>(1,3)<<3,2,1);
//定义一个1×3的矩阵，并且输入数据

Point2d/Point3d 类
   cv::Point2d; cv::Point3d是opencv中的一种存储二维/三维点的一种数据结构，常常用与容器配合使用，用于存储图像的特征点等等。
1.Keypoint转换为Point2d类
for(Dmatch m:matches){
vector<Point2d> pts_2=
(keypoints[matches[i].queryIdx].pt)
}

keypoints[i].pt返回的是一个point类
2.Point类访问元素
vector<Point2d> pts_2=
(keypoints[matches[i].queryIdx].pt)
double x= pts_2.x;
double y= pts_2.y;

3.Point类的数学运算
  Point类可之间进行加减运算，乘除运算转换为cv::Vec3d类。
//求解特征点的质心
Point3f p1, p2;     //质心坐标
	int N = pts1.size(); 
	 //pts1为point3f类型
	for (int i = 0; i < N; i++) {
		p1 += pts1[i]; //加法运算
		p2 += pts2[i];
	}
	p1 = Point3f(Vec3f(p1) / N);
	p2 = Point3f(Vec3f(p2) / N);
	//向量的乘法运算要转换为Vec3f类型

Vector2d/Vector3d 类
  Vector2d/Vector3d是Eigen库中的向量，也常常用于图像相关的运算，其灵活性比cv中的point类要高，更加"数学化"。

1.运算性质

  Vector2d/3本质是Eigen库的矩阵，实际运算过程严格按照矩阵运算即可。常常也用于存储图像的特征向量，特征点等，一般与容器配合使用。
2.赋值与遍历（手写输入数据）
Eigen::Vector3d xyz;
xyz <<
 1 ,
 2 ,
 3;
Eigen::Vector3d t(1,2,3);

//创建一个向量，两种方式都可以，第一种不能之间赋值
double x =xyz(0 ,0);
double y =xyz(1 ,0);
double z =xyz(2 ,0);
//访问相应的元素

注意：手写赋值与Mat类不同，遍历是必须严格按照矩阵的行与列进行遍历。
3.Eigen与Point类的互相转换
//Point to eigen
//若pts为定义好的 vector<Point3d> pts_3d l类型并且已经赋值	
	vector<Eigen::Vector3d> pts3_eigen;

	for (int i = 0; i < pts_3d.size(); i++)
	{
		pts3_eigen.push_back(
		Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z)
		);
	}

//eigen to Point
vector<cv::Point3d> point3;
	for (int i = 0; i < pts3_eigen.size(); i++)
	{
		point3.push_back(
		cv::Point3d(pts3_eigen[i](0, 0), pts3_eigen[i](1, 0), pts3_eigen[i](2, 0)));
	}

Sophus::SE3d/SO3d类
  李群李代数最直接的用法就是表达为位姿，如何对特征点进行变换，值得注意的是，此时的特征点必须是Eigen的Vector类型。

1.变换
	Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 1, 0)).toRotationMatrix();
	//沿着y轴旋转90°

	Eigen::Vector3d t(1, 0, 0);
	//沿着x轴平移一个单位

	Sophus::SE3d T(R, t);
	Eigen::Vector3d v(0, 1, 0);
	Eigen::Vector3d v_after = T * v;
	cout << "变换前的向量：\n" << v << endl;
	cout << "变换后的向量：\n" << v_after<< endl;

注：

1.以李群表示的位姿对某个Vector点变换后的数据类型还是Vector。

2.凡是在图像层次的运算都用cv里面的结构，凡是涉及其他的数学运算都用Eigen里面的结构。