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 <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include <opencv2/opencv.hpp>
#include <iomanip>
#include "math.h"
#include <time.h>
// #include "extra.h" // used in opencv2 
using namespace std;
using namespace cv;


void ransacmatch(vector<KeyPoint>& querykpt, vector<KeyPoint>& trainkpt, vector<DMatch>& match1, vector<DMatch>& matchransac)
{
    //随机一致性检测
    vector<Point2f>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<int> 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<double>(1, 0), R.at<double>(0, 0));
    double y = atan2(-R.at<double>(2, 0), sqrt(pow(R.at<double>(2, 0), 2) + pow(R.at<double>(2, 2),2)));
    double z = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
    Point3f p1(x, y, z);
    p = p1;
}

void find_feature_matches_byORB(
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches);


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);


int main(int argc, char** argv)
{
    string folder_path = "E:/Data/test/*.bmp";
    vector<cv::String> file_names;
    cv::Mat img_pre, img_new;

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    vector<Point2f> points1, points2;
  /*  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;*/

    //-- 估计两张图像间运动
   /* Mat R, t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);*/
    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<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3 
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();

    //Ptr<DescriptorMatcher> 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<Point2f>的形式
    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