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

函数

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行的数，就是归一化的三维坐标了

示例

//根据两张图片的匹配点，计算三维点的函数
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);
}