利用三角测量计算帧间特征点的空间位置

需要安装opencv3中的features2d模块

    代码中的1.png

    代码中的2.png

// 来自高翔SLAM十四讲
#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;

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

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d& p, const Mat& K);

// 三角测量
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
);

int main(){
    // 读取图像
    Mat img_1 = imread("1.png",CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("2.png", 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);

    // 验证三角化点与特征点的重影关系
    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 : "<<pt1_cam<<endl;
        cout<<"point projected from 3D "<<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 frame : "<<pt2_cam<<endl;
        cout<<"point reprohected from second frame : "<<pt2_trans.t()<<endl;
        cout<<endl;
    }

    return 0;
}

// 找到匹配的特征点
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;
    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);

    // 第三步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
    vector<DMatch> match;
    matcher->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;
        min_dist = min_dist<dist?min_dist:dist;
        max_dist = max_dist>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]);
        }
    }
}

Point2d pixel2cam(const Point2d& p, const Mat& K){
    return Point2d(
        (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 pose_estimation_2d2d(std::vector<KeyPoint>keypoints_1,
                                                std::vector<KeyPoint>keypoints_2,
                                                std::vector<DMatch>matches,
                                                Mat&R ,Mat& t){
    // 相机内参,TUM Feriburg2
    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);
    }

    // 计算基础矩阵:使用的8点法，但是书上说8点法是用来计算本质矩阵的呀，这两个有什么关系吗
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);
    cout<<"fundamental_matrix is "<<endl<<fundamental_matrix<<endl;

    // 计算本质矩阵：是由对极约束定义的：对极约束是等式为零的约束
    Point2d principal_point(325.1, 249.7);  // 相机光心,TUM dataset标定值
    double focal_length = 521;      // 相机焦距，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;

    // 从不本质矩阵中恢复旋转和平移信息
    // 这里的R,t组成的变换矩阵，满足的对极约束是:x2 = R * x1 + t,是第一个图到第二个图的坐标变换矩阵x2 = T21 * x1
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<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)
                    );
    
    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);  // 归一化
        Point3d p(
            x.at<float>(0,0),
            x.at<float>(1,0),
            x.at<float>(2,0)
        );
        points.push_back(p);
    }
}