视觉SLAM前端——直接法_直接法slam

目录

1. 直接法介绍
2. 单层直接法
3. 多层直接法

直接法介绍

  这里所说的直接法不同于上一篇所说的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);
	}	
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

接下来是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;
	}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

注意：
  要创建一个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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

注意：
1.金字塔的层数是从第0层开始的，这样比较符合编程的思维。
2.金字塔不仅仅指图像，只要是需要缩放的各类参数都要建立金字塔。
3.层与层直接的参数传递主要是位姿矩阵T，T传入的是引用，不需要初始化，定义时默认初始化为单位矩阵，每层求解后T均被保存下来。

效果如下：

单层法：

多层法：

  实际上，理论与代码的实现有着巨大的鸿沟，代码中还有很多细节部分没有理解清楚。加油！

参考文献：
[1]《视觉SLAM十四讲》 高翔