视觉里程计 -- 单目

在进行视觉slam的时候分为前端和后端处理,前端就称为视觉里程计。视觉里程计更具相邻图像的信息估计出错略的相机运动,给后端提供了较好的初始值。在视觉设备中,由单目相机和双目相机,还有双目深度相机。我们从最简单的单目开始讲起。

在机器人运动过程中,单目相机会以很高的频率不断的获取当前的图像,视觉里程计要做的就是在t,t+1用这两个时刻的图像来估计出机器人的运动(移动和旋转)。

先给出具体流程:

图像特征提取

获取图像的特征,必须要得到特征点,特征点由关键点和描述子两部分组成。图像处理领域有很多特征提取的算法,在视觉领域ORB用的比较多,原理可以自行google。总的来说,ORB在图像的缩放和旋转上,保证了特征点的不变形,不容易导致特征点的丢失。有图考虑了旋转和缩放,ORB在平移,旋转和缩放的变化下仍然有良好的表现。

特征匹配

获取了相邻两个图像中每个图像的特征点以后,我们就要对这两个图像中的点进行匹配,并且设定一定的阈值来保证比较优秀的匹配点被保留了下来(比较方法:每个特征点描述子之间的向量距离)。由于ORB的描述子是个二进制,所以我们使用Hamming distance作为度量。特征匹配问题解决了SLAM中的数据关联问题,即确定当前看到的路标和之前看到的路标之间的对应关系。

计算相机运动

我们重点来看运动计算,这是单目的重点难点。

单目相机是根据两组2D点估计运动。我们用对极几何解决这个问题。

上图就是对极几何图。对极几何满足约束: [公式] 。我们来推导下(如果对公式推导比较反感,可以参考这篇,解释的很直白

):

左右两个平行四边形分别是相机在不同位置的成像平面,C0, C1分别是两个位置中相机的光心,也就是针孔相机模型中的针孔,P是空间中的一个三维点,p0, p1分别是P点在不同成像平面上对应的像素点。上面左侧的图,如果将点P沿着C0-p0所在的直线移动,你会发现P在左边相机的成像一直不变,都是p0,这时候P在右边相机的成像点p1是一直在变化的。C0-C1-P-p0-p1他们都是在同一个平面上的,你可以想象C0-C1-P组成的平面是一个三角尺,它所在的平面称之为极平面(epipolar plane),它像一把锋利的刀,切割了左右两个成像平面。中和成像平面相交的直线称之为极线(epipolar line),两个光心C0, C1和成像平面的交点叫做极点(epipole)。

设P坐标:

由小孔成像相机模型,我们可以知道两个成像平面中的点 [公式]

K为相机的内参矩阵,之后会单独讲下小孔相机成像的原理。我们在图像处理时都会使用齐坐标来计算,这样更方便。用齐坐标的时候一个向量等于非零常数乘以自身。

[公式]

上述关系为 [公式]

归一化坐标 [公式] 带入上式,

[公式]

两边同乘以t的反对称矩阵, [公式]

左乘 [公式] ==> [公式] , 观察左侧, t代表的相机的移动,t的反对称矩内积 [公式] == t X [公式]。 所以等式坐标 == 0. 最终有:

[公式] , 这个就是堆积约束,通过对极几何得到的。

重新带入P1, P2 ,我们最终可以得到定义:

[公式]

E叫做本质矩阵,F叫做基础矩阵。我们看到E中有R和t, 所以理所当然我们就想能否通过E来计算旋转和移动,答案是肯定的。

通常我们使用8点法来求解这个E,具体的算法和原理可以google,opencv中已经做了很好的封装,直接拿来用就行了。

-----------------------------------------------------------------------

除了基本矩阵和本质矩阵,还存在一种常见的矩阵,单映矩阵。

单应矩阵:描述处于共同平面上的一点在两张图像之间的变换关系。

这个概念我一开始也花了很多时间理解。其实很容易理解, 之前我们说过,对于对极几何求得的E来说,我们用8点法, 也就是使用8个匹配出来的特征点对计算E。这8对点不是在同一平面上的,很可能每个点的深度都不同。

单应矩阵就是一种可以只用4对匹配点算出E的方法,但是这4对点都是来源于同一个平面。

三角测量

三角测量: 通过不同位置对同一个路标点进行观察,从观察到的位置推断路标点的距离。

原理比较简单,以下摘自《14讲》

 x1, x2为两个特征点的归一化坐标,那么它们满足:s1x1 = s2Rx2 + t。现在已经知道了 R, t,想要求解的是两个特征点的深度 s1, s2。可以对上式两侧左乘一个 x1^ ,得:

求解公式:s1 x0^ x0 = 0 = s2 x0^ R x1 + x0^ t

上式右侧看成s2的一个方程,可以求得s2。有了s2,就可以求出s1。由于噪声的存在,更常见的做法是求最小二乘解而不是零解。

用opencv实现以上对于单目相机旋转角度计算:

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <algorithm>
#include <chrono>

using namespace std;
using namespace cv;

void pose_estiamtion_2d2d(const std::vector<KeyPoint>& keyPoints1, const std::vector<KeyPoint>& keyPoints2, const std::vector<DMatch>& matches, Mat& R, Mat& t)
{
	Mat K = (Mat_<double>(3,3) << 520.9,0,325.1,0,521.0,249.7,0,0,1);
	vector<Point2f> points1;
	vector<Point2f> points2;

	for(int i = 0; i < matches.size(); i++)
	{
		points1.push_back(keyPoints1[matches[i].queryIdx].pt);
		points2.push_back(keyPoints2[matches[i].trainIdx].pt);
	}
	
	Mat fundamental_matrix;
	fundamental_matrix = findFundamentalMat(points1,points2,FM_8POINT);
	cout<<"fundamental_marix is "<<endl<<fundamental_matrix<<endl;

	Point2d principal_point(325.1, 249.7);
	double focal_length = 521;
	Mat essential_matrix;
	essential_matrix = findEssentialMat(points1,points2,K);
	cout<<"essential matrix is "<<endl<<essential_matrix<<endl;

	Mat homography_matrix;
	homography_matrix = findHomography(points1, points2, RANSAC, 3);
	cout<<"homography matrix is "<<endl<<homography_matrix<<endl;

	recoverPose(essential_matrix, points1, points2, K, R, t);
	cout<<"R  is "<<endl<<R<<endl;
	cout<<"t  is "<<endl<<t<<endl;
}

Point2f pixel2cam(const Point2d &p, const Mat &K) {
  return Point2f
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void triangulation(const vector<KeyPoint> &keypoints1,
				   const vector<KeyPoint> &keypoints2,
				   const vector<DMatch> &matches,
				   const Mat &R, const Mat &t,
				   vector<Point3d> &points	
				  )
{
	Mat T1 = (Mat_<float>(3,4) << 1,0,0,0,
								 0,1,0,0,
                                 0,0,1,0);
	Mat T2 = (Mat_<float>(3,4) << 
								R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    							R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    							R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));

	vector<Point2f> pts1,pts2;
	Mat K = (Mat_<double>(3,3) << 520.9,0,325.1,0,521.0,249.7,0,0,1);
    for(DMatch m:matches)
	{
		pts1.push_back(pixel2cam(keypoints1[m.queryIdx].pt, K));
		pts2.push_back(pixel2cam(keypoints2[m.trainIdx].pt, K));	
	}

	Mat pts4d;
	triangulatePoints(T1,T2,pts1,pts2,pts4d);
	cout<<"pts 4d row "<<pts4d.rows<<" col "<<pts4d.cols<<endl;	
 	
	for(int i = 0; i < pts4d.cols; i++)
	{
		Mat x = pts4d.col(i);
		x /= x.at<float>(3,0);
		Point3d p(x.at<float>(0,0),
						x.at<float>(1,0),
						x.at<float>(2,0)
					);
		points.push_back(p);
	}
}

int main(int argc, char** argv)
{
	if(argc != 3)
	{
		cout<<"usage: binary img1 img2"<<endl;
		return 0;
	}
	Mat img1, img2;
	img1 = imread(argv[1], IMREAD_COLOR);
	img2 = imread(argv[2], IMREAD_COLOR);
	
	int w = img1.cols;
	int h = img1.rows;
	double ratio = static_cast<double>(w) / h;
	resize(img1, img1, Size(576,576/ratio),0,0);
	resize(img2, img2, Size(576,576/ratio),0,0);
		

	std::vector<KeyPoint> keypoints1, keypoints2;
	Mat descriptors1, descriptors2;
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> descriptor = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	detector->detect(img1, keypoints1);
	detector->detect(img2, keypoints2);
	
	descriptor->compute(img1, keypoints1, descriptors1);
	descriptor->compute(img2, keypoints2, descriptors2);

	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
	cout<<"extrace ORB cost = "<<time_used.count()<<" seconds ."<<endl;
	
	Mat outimg1;
	drawKeypoints(img1,keypoints1,outimg1);
	//imshow("ORB features", outimg1); 

	vector<DMatch> matches;
	t1 = chrono::steady_clock::now();
	matcher->match(descriptors1,descriptors2,matches);
	t2 = chrono::steady_clock::now();
	time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
	cout<<"match cost time  = "<<time_used.count()<<" seconds ."<<endl;
	cout<<"Mathes count: "<<matches.size()<<endl;
	cout<<"descriptors size is: "<<descriptors1.rows<<" "<<descriptors1.cols<<endl;	
	auto min_max = minmax_element(matches.begin(), matches.end(), [](const DMatch &m1, const DMatch &m2){return m1.distance < m2.distance;});
	double min_dist = min_max.first->distance;
	double max_dist = min_max.second->distance;
	printf("-- Max dist : %f \n", max_dist);
	printf("-- Min dist : %f \n", min_dist);


	vector<DMatch> good_matches;
	for(DMatch m:matches)
	{
		if(m.distance <= max(2*min_dist, 30.0))
			good_matches.push_back(m);
	}

	Mat img_match;
	Mat img_goodmatch;
	drawMatches(img1,keypoints1,img2,keypoints2,matches,img_match);
	drawMatches(img1,keypoints1,img2,keypoints2,good_matches,img_goodmatch);
	//imshow("all matches",img_match);
	imshow("good matches",img_goodmatch);
	cout<<"find point: "<<good_matches.size()<<endl;
	Mat R, t;
	pose_estiamtion_2d2d(keypoints1,keypoints2,good_matches,R,t);
	
	vector<Point3d> points;
	triangulation(keypoints1,keypoints2,good_matches,R,t,points);
	cout<<"points size is "<<points.size()<<endl;
	
	waitKey(0);	
	return 0;
}

结果会输出选取的最优特征点和最终的R和t。