前言
最近也看了许许多多关于slam，vo，特征点检测匹配，光流法相关的资料。想着自己能动手实践一下，先从最简单的2d-2d位姿估计开始吧，也顺便记录下踩的坑。
注：代码太多了python放另外一篇吧链接
一、总体流程
首先需要说的是2d-2d的位姿估计会丢失平移向量t的尺寸，只有后面不再用2d-2d用三角化求解点再用3d-2d才能解决尺度问题。2d-2d的位姿估计算是蛮简单的，可以参考slam14讲的代码，主要有以下几个步骤：
1、两帧间的特征点寻找与匹配
2、通过特征点求解本质矩阵
3、通过本质矩阵恢复R,t
不得不说我们都是站在巨人的肩膀上，OpenCV几乎提供了所有这一切，我们所做的仅仅是组合就完了。
二、实现代码
c++的2d-2d实现也较为简单，其中借鉴了很多slam14讲的代码。
#include#include #include #include #include #include "opencv2/imgcodecs/legacy/constants_c.h" #include #include #include "math.h" #include // #include "extra.h" // used in opencv2 using namespace std; using namespace cv; void ransacmatch(vector & querykpt, vector & trainkpt, vector & match1, vector & matchransac) { //随机一致性检测 vector queryp(match1.size()), trainp(match1.size());//定义匹配点对坐标 //保存从关键点中提取的点对 for (int i = 0; i < match1.size(); ++i) { queryp[i] = querykpt[match1[i].queryIdx].pt;//match1[i].queryIdx 是结果中对应的query的index trainp[i] = trainkpt[match1[i].trainIdx].pt; } //匹配点对进行RANSAC过滤 vector mask(queryp.size()); findHomography(queryp, trainp, RANSAC, 3, mask); for (int j = 0; j < mask.size(); ++j) { if (mask[j]) { matchransac.push_back(match1[j]); } } } void R2Angle(Mat R, Point3f& p) { double x = atan2(R.at (1, 0), R.at (0, 0)); double y = atan2(-R.at (2, 0), sqrt(pow(R.at (2, 0), 2) + pow(R.at (2, 2),2))); double z = atan2(R.at (2, 1), R.at (2, 2)); Point3f p1(x, y, z); p = p1; } void find_feature_matches_byORB( const Mat& img_1, const Mat& img_2, std::vector & keypoints_1, std::vector & keypoints_2, std::vector< DMatch >& matches); void pose_estimation_2d2d( const std::vector & keypoints_1, const std::vector & keypoints_2, const std::vector< DMatch >& matches, Mat& R, Mat& t); int main(int argc, char** argv) { string folder_path = "E:/Data/test //-- 估计两张图像间运动 cout << fixed << setprecision(3); glob(folder_path, file_names); cv::Mat img; double all = 0.0; int num = 0; for (int i = 0; i < file_names.size(); i++) { num++; clock_t s = clock(); matches.clear(); keypoints_1.clear(); keypoints_2.clear(); if (i < 1) { img_pre = imread(file_names[i]); continue; } img_new = imread(file_names[i]); Mat R, t; find_feature_matches_byORB(img_pre, img_new, keypoints_1, keypoints_2, matches); pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t); Point3f p; R2Angle(R, p); cout << "三个角度" << p << endl; img_pre = img_new; clock_t e = clock(); cout << "总耗时" << double(e - s) << "ms" << endl; cout << "********************" << endl; all += double(e - s); } cout <<"总均耗时" << double(all / num) << "ms" << endl; return 0; } void find_feature_matches_byORB(const Mat& img_1, const Mat& img_2, std::vector & keypoints_1, std::vector & keypoints_2, std::vector< DMatch >& matches) { //-- 初始化 Mat descriptors_1, descriptors_2; // used in OpenCV3 Ptr detector = ORB::create(); Ptr descriptor = ORB::create(); //Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming"); //-- 第一步:检测 Oriented FAST 角点位置 detector->detect(img_1, keypoints_1); detector->detect(img_2, keypoints_2); -- 第二步:根据角点位置计算 BRIEF 描述子 descriptor->compute(img_1, keypoints_1, descriptors_1); descriptor->compute(img_2, keypoints_2, descriptors_2); descriptors_1.convertTo(descriptors_1, CV_32F); descriptors_2.convertTo(descriptors_2, CV_32F); -- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离 vector match; FlannBasedMatcher flannmatch; flannmatch.match(descriptors_1, descriptors_2, match); //-- 第四步:匹配点对筛选 double min_dist = 10000, max_dist = 0; //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 for (int i = 0; i < descriptors_1.rows; i++) { double dist = match[i].distance; if (dist < min_dist) min_dist = dist; if (dist > max_dist) max_dist = dist; } //随机一致性检验 vector ransac_matchers; ransacmatch(keypoints_1, keypoints_2, match, ransac_matchers); Mat img_matches1, img_matches2, img_matches3, img_matches4; drawMatches(img_1, keypoints_1, img_2, keypoints_2, match, img_matches1); drawMatches(img_1, keypoints_1, img_2, keypoints_2, ransac_matchers, img_matches4); imshow("now", img_matches3); imshow("ransac_matchers", img_matches4); matches = ransac_matchers; waitKey(10); } void pose_estimation_2d2d( const std::vector & keypoints_1, const std::vector & keypoints_2, const std::vector< DMatch >& matches, Mat& R, Mat& t) { // 相机内参,TUM Freiburg2 Mat K = (Mat_ (3, 3) << 600, 0, 300, 0, 600, 300, 0, 0, 1); //-- 把匹配点转换为vector 的形式 vector points1; vector points2; for (int i = 0; i < (int)matches.size(); i++) { points1.push_back(keypoints_1[matches[i].queryIdx].pt); points2.push_back(keypoints_2[matches[i].trainIdx].pt); } //-- 计算基础矩阵 Mat fundamental_matrix; fundamental_matrix = findFundamentalMat(points1, points2, FM_RANSAC); //cout << "fundamental_matrix is " << endl << fundamental_matrix << endl; //-- 计算本质矩阵 Point2d principal_point(300, 300); //相机主点, TUM dataset标定值 int focal_length = 600; //相机焦距, TUM dataset标定值 Mat essential_matrix; essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point); //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, R, t, focal_length, principal_point); //cout << "R is " << endl << R << endl; //cout << "t is " << endl << t << endl; } **三、总结** 1、代码和slam4讲里面的差不多，文中采用了随机一致性采样来消除匹配失效的点。实践证明改方法特别有效。 2、也在c++下面测试了下时间如下： sift + knn +随机一致性采样 均帧耗时0.34s orb + 随机一致性采样 均帧耗时0.08s orb还是比较快了，但是实测下LK光流法可以更快，均帧耗时应该在40ms也就是0.04左右。
最后是效果图，记得改内参哦 KITIT的内参是F: 718.9，718.9 Cx：607.2，Cy：185.2
