三角测量,用Opencv计算获得两图片匹配点的深度

  1. 函数

    cv::triangulatePoints(T1, T2, points1, points2, points_4d);
    输入:
    T1: 3*4的cv::Mat ,就是第一张图片的 R和t组合一起的
    T2: 3*4的cv::Mat ,就是第二张图片的 R和t组合一起的
    points1: vector<cv::Point2f> 前一张图片的匹配点
    points2: vector<cv::Point2f> 后一张图片的匹配点,肯定和上一个size()一样
    
    输出:
    points_3d: cv::Mat类型
    会输出4行,points1.size()列的Mat,一列代表一个点,把每一列的点都除以第4行的数,就是归一化的三维坐标了
    
  2. 示例
    先特征匹配,获得前后两张图片匹配的特征点对,并计算的到旋转矩阵R,和平移向量t,此部分内容可以参考上篇博客

    //根据两张图片的匹配点,计算三维点的函数
    void triangulation(vector<cv::KeyPoint>& keypoints1, vector<cv::KeyPoint>& keypoints2, 
                        vector<cv::DMatch>& matches, cv::Mat& R, cv::Mat& t, 
                        vector<cv::Point3f>& points3D,
                        cv::Mat& image1, cv::Mat& image2)
    {
        cv::Mat T1 = (cv::Mat_<float>(3,4)<<1,0,0,0,  //这里以第一张图片为世界坐标系
                                            0,1,0,0,
                                            0,0,1,0);
        R.convertTo(R,CV_64FC1);  //R, t已经算得,可以参考之前篇内容,根据匹配点计算R, t
        t.convertTo(t,CV_64FC1);
    
        cv::Mat T2 = (cv::Mat_<float>(3,4)<<  //第二张图片的位姿,[R,t]
        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));
    
        cv::Mat K = (cv::Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);  //相机内参矩阵
    
        vector<cv::Point2f> pt1, pt2;  //匹配点的归一化坐标 x1, x2
        for(cv::DMatch m:matches)
        {
            cv::Point2f p1 = keypoints1[m.queryIdx].pt;  //匹配点的像素坐标
            cv::Point2f p2 = keypoints2[m.trainIdx].pt;
            pt1.push_back(cv::Point2f((p1.x-K.at<double>(0,2))/K.at<double>(0,0),(p1.y-K.at<double>(1,2))/K.at<double>(1,1)));  //归一化坐标
            pt2.push_back(cv::Point2f((p2.x-K.at<double>(0,2))/K.at<double>(0,0),(p2.y-K.at<double>(1,2))/K.at<double>(1,1)));
        }
    
        cv::Mat pts_4d;  //保存计算得到的三维点结果
        //!!!!!!!!!!!!!!!!!!!!!!!OpenCV自带函数
        cv::triangulatePoints(T1,T2,pt1,pt2,pts_4d);  //pst_4d 是4行的,每一列表示一个点
        cout<<"**********************"<<endl;
        cout<<pts_4d.size<<endl;  // 4*18
    
        for(int i=0;i<pts_4d.cols;i++)
        {
            cv::Mat x = pts_4d.col(i).clone();  //四维
            x = x/x.at<float>(3,0);
            cv::Point3f p(x.at<float>(0,0),x.at<float>(1,0),x.at<float>(2,0));  //归一化的三维点
            points3D.push_back(p);
        }
    
        cv::Mat img1 = image1.clone();
        cv::Mat img2 = image2.clone();
        for(int i=0; i<points3D.size();i++)
        {
            float depth = points3D[i].z;
            cout<<"depth: "<<i<<" : "<<depth<<endl;
            cv::Point2d pt1_cam = pt1[i];
            cv::circle(img1,keypoints1[matches[i].queryIdx].pt,2,cv::Scalar(255,255,0,0),2);
        }
        cv::imshow("img1",img1);
        cv::waitKey(-1);
    }
    

你可能感兴趣的