视觉双目测距

#include “opencv2/core/core.hpp“
#include “cv.h“
#include 
#include “opencv2/imgproc/imgproc.hpp“
#include “opencv2/features2d/features2d.hpp“
#include “opencv2/nonfree/nonfree.hpp“
#include “opencv2/highgui/highgui.hpp“
#include 
#include 

using namespace cv;
using namespace std;
float m1[3][3] = {{7.173874e+02 0. 3.084055e+02}{0.7.193689e+02 2.296618e+02}{0. 0. 1.}};
float m2[3][3] = {{7.293169e+02 0. 3.151903e+02}{0.7.312317e+02 2.277928e+02}{0. 0. 1.}};
float r[3][3] = {{9.999e-01 9.7e-03 1.37e-02}{-9.8e-03 9.999e-01 4.9e-03}{-1.36e-02 -5.0e-03 9.999e-01}};//如果自己写这些参数的话注意里面的e不要漏了啊。被坑了好多次。
float t[3][1] = {-7.89819e+01 -2.88e-029.2785e+00};
Mat M1 = Mat（33CV_32Fm1）;
Mat M2 = Mat（33CV_32Fm2）;
Mat R = Mat（33CV_32Fr）;
Mat T = Mat（31CV_32Ft）;

Mat mapLx mapLy mapRx mapRy;                         //映射表
Rect validROIL validROIR;
Point3f uv2xyz（Point2f uvLeftPoint2f uvRight）;

int main（int argc char** argv）
{
    //    FileStorage fs（“intrinsics.yml“ CV_STORAGE_READ）;

    //    if（!fs.isOpened（））
    //    {
    //        printf（“Failed to open file %s\n“ “intrinsics.yml“）;
    //        return -1;
    //    }

    //    fs[“M1“] >> M1;
    //    fs[“D1“] >> D1;
    //    fs[“M2“] >> M2;
    //    fs[“D2“] >> D2;
    //    fs.open（“extrinsics.yml“ CV_STORAGE_READ）;
    //    if（!fs.isOpened（））
    //    {
    //        printf（“Failed to open file %s\n“ “extrinsics.yml“）;
    //        return -1;
    //    }
    //    fs[“R“] >> R;
    //    fs[“T“] >> T;
    cout << “M1 = “ << M1.type（） << endl;
    cout << “M2 = “ << M2.type（） << endl;
    cout << “T = “ << T.type（） << endl;
    cout << “R = “ << R.type（） << endl;


    Point3f world_points;

    Mat src1 = imread（“frame1.jpg“）;
    Mat src2 = imread（“frame0.jpg“）;

    SIFT sift;    //实例化SIFT类

    vector keypoints1keypoints2;
    Mat descriptors1descriptors2;
    // descriptors为描述符，mascara为掩码矩阵
    Mat mascara1 mascara2;

    sift（src1mascara1keypoints1descriptors1）;    //执行SIFT运算
    sift（src2mascara2keypoints2descriptors2）;    //执行SIFT运算

    SiftDescriptorExtractor extractor;

    extractor.compute（src1 keypoints1descriptors1）;
    extractor.compute（src2 keypoints2descriptors2）;

    FlannbasedMatcher matcher;
    vector matches;
    matcher.match（descriptors1descriptors2matches）;
    double max_dist = 0;
    double min_dist = 1000;

    for（int i = 0; i < descriptors1.rows; i++）
    {
        double dist = matches[i].distance;
        if（dist < min_dist） min_dist = dist;
        if（dist > max_dist） max_dist = dist;
    }

    cout << “max = “ << max_dist << endl;
    //   cout << “min = “ << min_dist << endl;

    vector good_matches;

    int a = 0;
    for（int i = 0; i < matches.size（）; i++）
    {
        if（matches[i].distance < 2*min_dist）
        {
            if（fabs（keypoints1[matches[i].queryIdx].pt.y - keypoints2[matches[i].trainIdx].pt.y） < 5）
            {