opencv双目测距的简单代码实现

今年五月份开始研究双目，从刚开始的无从下手到渐渐熟悉，再到中途的想放弃，也算是历经波折，在此我把我自己东拼西凑的东西分享一下，算是我在CSDN成长的一个见证。首先说下遇到的问题：

1. 两个摄像头之间必须固定，不然不可能标定成功。
2. 标定板的数目尽量不要出现横排竖列相等的情况。
3. 采集图片的时候不要离摄像头太远。

#include <opencv2/opencv.hpp>
#include <iostream>

using namespace cv;
using namespace std;

VideoCapture capl;
VideoCapture capr;
Mat imgl, imgr,imgl_bm,imgr_bm;
Mat framel, framer,image;
Mat disp, disparityImage, pointClouds;
Mat R, T, R1, P1, R2, P2;
Mat M1, D1, M2, D2;
Mat Q,xyz;
float scale = 1.f;
const char* intrinsic_filename = "intrinsics.yml";
const char* extrinsic_filename = "extrinsics.yml";
Ptr<StereoBM> bm = StereoBM::create(16, 9);
Rect roi1, roi2;
int numberOfDisparities = 0, SADWindowSize = 0;
int pic_info[4];
bool left_mouse = false;
void updatecalibdata()
{
    cout << "开始加载标定参数。" << endl;
    FileStorage fs(intrinsic_filename, FileStorage::READ);
    if (!fs.isOpened())
    {
        printf("Failed to open file %s\n", intrinsic_filename);
        //return -1;
    }

    //Mat M1, D1, M2, D2;
    fs["M1"] >> M1;
    fs[&