视觉SLAM14讲——VO

Slam14讲学习笔记

(七) 视觉里程计

本讲主要使用了特征提取关键点和描述子，对极约束估计位姿（RＴ），三角计算估计深度等

• 头文件及函数声明

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace cv;

//本程序演示了如何使用2D-2D的特征匹配估计相机运动

void find_feature_matches(
    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(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat& R, Mat& t);
//三角计算
void triangulation(
    const vector<KeyPoint>& keypoint_1,
    const vector<KeyPoint>& keypoint_2,
    const std::vector<DMatch>& matches,
    const Mat& R, const Mat& t,
    vector<Point3d>& points
);
//像素坐标转相机归一化坐标 返回值是Point2f类型
Point2f pixel2cam (const Point2d& p, const Mat& K);

接下来先对这四个函数进行分析

• find_feature_matches 特征匹配

//该函数的功能是输入img_1,img_2,计算得到两组描述子和一个匹配结果
void find_feature_matches(
    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;
    //开始使用opencv里面的匹配算法
    Ptr<FeatureDetector> detector = ORB::create();//特征提取器
    Ptr<DescriptorExtractor> descriptor = ORB::create(); //描述子
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//匹配结果

    //1 检测Oriented Fast 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //2 根据角点计算BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //3 对两幅图像中的BRIEF描述子进行匹配，使用Hamming 距离进行度量
    vector<DMatch> match;//用来存放匹配结果  同1 不同0
    matcher->match(descriptors_1, descriptors_2, match);

    //4 将得到的汉明距离进行筛选 匹配点筛选
    double min_dist = 1000, 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;
    }
    printf("--Max dist: %f\n", max_dist);
    printf("--Min dist: %f\n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时，即认为匹配有误，但有时距离会非常小，设置一个经验值30作为下限
    for ( int i=0; i < descriptors_1.rows; i++)
    {
    
    
        if (match[i].distance <= max( 2*min_dist, 30.0))
        {
    
    
            matches.push_back(match[i]);//将筛选后的描述子放入最终匹配结果
        } 
    }
}   

• 完整代码

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace cv;

//本程序演示了如何使用2D-2D的特征匹配估计相机运动

//函数声明

//特征提取器
void find_feature_matches(
    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(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat& R, Mat& t);
//三角计算
void triangulation(
    const vector<KeyPoint>& keypoint_1,
    const vector<KeyPoint>& keypoint_2,
    const std::vector<DMatch>& matches,
    const Mat& R, const Mat& t,
    vector<Point3d>& points
);
//像素坐标转相机归一化坐标 返回值是Point2d类型
Point2f pixel2cam (const Point2d& p, const Mat& K);


int main(int argc, const char** argv) {
    
    
    if (argc!=3)
    {
    
    
        cout<<"usage:pose_estimation_2d2d img1 img2"<<endl;
        return 1;
    }

    //读取图像
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

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

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

    // 三角化
    vector<Point3d> points;
    triangulation(keypoints_1, keypoints_2, matches, R, t, points);//points 里面是世界坐标系下的点 x y z

    //验证三角化点与特征点的重投影关系
    Mat K = (Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
    for (int i = 0; i < matches.size(); i++)
    {
    
    
        Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt,K);
        Point2d pt1_cam_3d(
            points[i].x/points[i].z,
            points[i].y/points[i].z);
        cout<<"point in the first camera frame:\n"<<pt1_cam<<endl;
        cout<<"point projected from 3D\n"<<pt1_cam_3d<<",d="<<points[i].z<<endl;
 
        //  第二个图
        Point2f pt2_cam = pixel2cam(keypoints_2[matches[i].trainIdx].pt, K);//取到第二个图上的点
        Mat pt2_trans = R*( Mat_<double>(3, 1)<<points[i].x, points[i].y, points[i].z) + t;
        pt2_trans/=pt2_trans.at<double>(2, 0);
        cout<<"point in the second camera fram:"<<pt2_cam<<endl;
        cout<<"point reprojected from second frame:"<<pt2_trans.t()<<endl;

    }

    return 0;
}

//该函数的功能是输入img_1,img_2,计算得到两组描述子和一个匹配结果
void find_feature_matches(
    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;
    //开始使用opencv里面的匹配算法
    Ptr<FeatureDetector> detector = ORB::create();//特征提取器
    Ptr<DescriptorExtractor> descriptor = ORB::create(); //描述子
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//匹配结果

    //1 检测Oriented Fast 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //2 根据角点计算BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //3 对两幅图像中的BRIEF描述子进行匹配，使用Hamming 距离进行度量
    vector<DMatch> match;//用来存放匹配结果  同1 不同0
    matcher->match(descriptors_1, descriptors_2, match);

    //4 将得到的汉明距离进行筛选 匹配点筛选
    double min_dist = 1000, 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;
    }
    printf("--Max dist: %f\n", max_dist);
    printf("--Min dist: %f\n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时，即认为匹配有误，但有时距离会非常小，设置一个经验值30作为下限
    for ( int i=0; i < descriptors_1.rows; i++)
    {
    
    
        if (match[i].distance <= max( 2*min_dist, 30.0))
        {
    
    
            matches.push_back(match[i]);//将筛选后的描述子放入最终匹配结果
        } 
    }
}   

void pose_estimation_2d2d(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat& R, Mat& t)
{
    
    
    //相机内参 TUM Freiburg2
    Mat K = (Mat_<double>(3,3)<<
    520.9, 0, 325.1, 
    0, 521.0, 249.7,
     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, CV_FM_8POINT);
    cout<<"fundamental_matrix is:\n"<<fundamental_matrix<<endl;

    //计算本质矩阵
    Point2d principal_point (325.1, 249.7); // 相机光心 TUM dataset 标定值
    double focal_length = 521;  // 相机焦距
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout<<"essential_matrix is\n"<<essential_matrix<<endl;

    //计算单应矩阵
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    cout<<"homography_matrix = \n"<<homography_matrix<<endl;

    //从本质矩阵中恢复旋转和平移
    recoverPose (essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout<<"R is \n"<<R<<endl;
    cout<<"t is \n"<<t<<endl;

}

void triangulation(
    const vector<KeyPoint>& keypoint_1,
    const vector<KeyPoint>& keypoint_2,
    const std::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));// T2=[R t]
    // 内参
    Mat K = (Mat_<double>(3,3)<< 520.9, 0 ,325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point2f> pts_1, pts_2;
    for( DMatch m:matches)
    {
    
    
        // 将像素坐标转换至相机坐标
        pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));//当前点的索引值
        pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));//匹配点的索引值
    }
    Mat pts_4d;
    cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

    //转换成非齐次坐标
    for (int i = 0; i < pts_4d.cols; i++)
    {
    
    
        Mat x = pts_4d.col(i);
        x /= x.at<float>(3, 0);//归一化 (x, y, z)/d  
        Point3d p(
            x.at<float>(0,0),
            x.at<float>(1,0),
            x.at<float>(2,0)
        );
        points.push_back(p);
    }
}

//像素坐标转相机归一化坐标 返回值为Point2d类型
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)
    );
}