三角测量，用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);
   }