SLAM实践---2d-2d位姿估计（c++）
SLAM实践—2d-2d位姿估计（c++）
前言
最近也看了许许多多关于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 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<DMatch>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<DMatch> 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<KeyPoint>& keypoints_1,
const std::vector<KeyPoint>& keypoints_2,
const std::vector< DMatch >& matches,
Mat& R, Mat& t)
{
// 相机内参,TUM Freiburg2
Mat K = (Mat_<double>(3, 3) << 600, 0, 300, 0, 600, 300, 0, 0, 1);
//-- 把匹配点转换为vector的形式
vector<Point2f> points1;
vector<Point2f> 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