简单的两两帧单目视觉里程计

发表于 | 分类于

这是一个简单的两两帧视觉里程计，两帧图像之间R，t的估计只使用了对极几何，尺度因子由数据集的真实轨迹获得，本项目的实现基于Opencv.

问题描述：

输入：

使用相机采集的图片序列；相机的内参（即焦距$f_x$,$fy$,相机光心$c_x$,$c_y$,畸变参数$k_1$,$k_2$,$k_3$,$p_1$,$p_2$），相机内参可通过Opencv、matlab、kalibr等工具进行标定。

输出：

以第一帧图片为参考系，输出每帧图片的坐标。

算法步骤：

  • 读入相机参数
  • 获取两帧连续图像$I^t$,$I^{t+1}$
  • 对获取的图像进行去畸变处理
  • 对$I^t$提取FAST特征点，使用LK金字塔光流法在$I^{t+1}$中跟踪这些特征点，若跟踪的特征点数量小于预设的阈值，则在$I^{t+1}$中重新提取FAST特征点
  • 使用五点法及RANSAC计算本质矩阵$E$
  • 通过本质矩阵$E$估计两帧之间的$R,t$
  • 使用数据集中的真实轨迹获取两帧之间的尺度因子，计算出$I^{t+1}$在参考系中的$R,t$并输出显示

核心算法代码：

  • 从第二帧开始，通过真实轨迹坐标计算并保存每帧的尺度到vector中
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
void getAbusoluteScale(vector<double> scale,string ground_truth){
	ifstream in;
	in.open(ground_truth);
	scale.push_back(1);
	string line="";
	getline(in,line);
	istringstream item(line);
	double x_pre=0,y_pre=0,z_pre=0;
	double x=0,y=0,z=0;
	item>>x_pre>>y_pre>>z_pre;
	while(getline(in,line)){
		istringstream nextItem(line);
		nextItem>>x>>y>>z;
		scale.push_back(sqrt((x - x_pre)*(x - x_pre) + (y - y_pre)*(y - y_pre) + (z - z_pre)*(z - z_pre)));
		x_pre=x;
		y_pre=y;
		z_pre=z;
	}
}
  • 转灰度图、去畸变
1
2
cvtColor(ref,ref,CV_BGR2GRAY);
undistort(ref,ref,K,DistCoef);
  • 获取FAST特征点，并将其存于类型为vector的kp中，便于后续LK跟踪、本质矩阵计算
1
2
3
4
5
void featureDetection(Mat& frame,vector<Point2f>& kp){
	vector<Point2f> keypoints;
	FAST(frame,keypoints,40,true);
	KeyPoint::convert(keypoints,kp);
}
  • 使用opencv的函数进行光流跟踪，返回两幅图像的匹配点，并将跟踪失败的点去掉
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void LKFeatureTracking(Mat& ref,Mat& curr,vector<Point2f>& ref_kp,vector<Point2f>& cur_kp){
	vector<uchar> status;
	vector<float> err;
	cvCalcOpticalFlowPyrLK(ref,curr,ref_kp,cur_kp,status,err);
	vector<Point2f>::iterator ref_iter=ref_kp.begin();
	vector<Point2f>::iterator cur_iter=cur_kp.begin();
	for(auto state:status){
		if(state==0){
			ref_kp.erase(ref_iter);
			cur_kp.erase(cur_iter);			
		}else{
			ref_iter++;
			cur_iter++;
		}
	}	
}
  • 计算本质矩阵，并从本质矩阵中估计R，t
1
2
E=findEssentialMat(cur_kp,ref_kp,K,RANSAC);
recoverPose(E, cur_kp,ref_kp,K,R, t);

  • 设有三帧图像,索引分别为1,2,3，其中，

    所以第三帧到第一帧的变换矩阵为：

    所以，获取各帧图像的pose代码如下：

    1
    2
    t_f=R_f*(scale[frame_id]*t)+t_f;
    R_f=R_f*R;

  • 如果需要跟踪的特征点少于阈值，则重新提取特征点

    1
    2
    3
    if(cur_kp.size()<MIN_NUM){
    	featureDetection(curr,cur_kp);
    }

数据集KITTI，运行结果：

与真实轨迹比较：

参考资料：

http://avisingh599.github.io/vision/monocular-vo/