由两张视差图生成深度图

由两张视差图生成深度图，SGBM方法输入的是未矫正的图片，BM输入的是校正后的图片

在这里插入代码片#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

Mat left_img= imread("E:\\picture\\stereo_camera\\重建左\\left_4.jpg");
Mat right_img = imread("E:\\picture\\stereo_camera\\重建右\\right_4.jpg");
Mat disp, xyz;	

//SGBM参数
int mindisparity = 0;
int ndisparities = 64;
int SADWindowSize = 9;
int PreFilterCap = 5;
int UniquenessRatio = 5;
int SpeckleRange = 2;
int Disp12MaxDiff = 1;
//BM参数
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Mat disp8;
Mat Q;//深度视差映射矩阵
Rect left_valid_roi, right_valid_roi;//图像校正之后，会对图像进行裁剪，这里的validRoi就是指裁剪之后的区域
Mat left_rectified_gray, right_rectified_gray;//左右摄像头校准后图像
Mat img;
Mat left_rectified_img, right_rectified_img;//左右摄像头校准后图像

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象
Vec3f  point3;
float d;

void stereo_SGBM_match(int, void*)
{
	//SGBM
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
	int P1 = 8 * left_img.channels() * SADWindowSize * SADWindowSize;
	int P2 = 10 * left_img.channels() * SADWindowSize * SADWindowSize;
	sgbm->setP1(P1);
	sgbm->setP2(P2);
	sgbm->setPreFilterCap(PreFilterCap);
	sgbm->setUniquenessRatio(UniquenessRatio);
	sgbm->setSpeckleRange(SpeckleRange);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setDisp12MaxDiff(Disp12MaxDiff);
	//sgbm->setMode(cv::StereoSGBM::MODE_HH);
	sgbm->compute(left_img, right_img, disp);
	disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
	Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
	normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
	reprojectImageTo3D(disp8U, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	
	namedWindow("disparity_SGBM", WINDOW_AUTOSIZE);
	imshow("disparity_SGBM", disp8U);

}

void stereo_BM_match(int, void*)
{
    Ptr<StereoBM> bm = StereoBM::create(16, 9);
    bm->setBlockSize(2 * blockSize + 5);     //SAD窗口大小，5~21之间为宜
    bm->setROI1(left_valid_roi);
    bm->setROI2(right_valid_roi);
    bm->setPreFilterCap(31); 
    bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
    bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp;
    bm->compute(left_rectified_gray, right_rectified_gray, disp);//输入图像必须为灰度图
    disp.convertTo(disp8, CV_8UC1, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    imshow("disparity_BM", disp8);
    //cout << disp8.rows << "\n" << disp8.cols << endl;

}

/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
    if (selectObject)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
    }

    switch (event)
    {
    case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
        origin = Point(x, y);
        selection = Rect(x, y, 0, 0);
        selectObject = true;
        //cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
        point3 = xyz.at<Vec3f>(origin);
        point3[0];
        //cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]<<endl;
        cout << "世界坐标：" << endl;
        cout << "x: " << point3[0] << "  y: " << point3[1] << "  z: " << point3[2] << endl;
        d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2];
        d = sqrt(d);   //mm
       // cout << "距离是:" << d << "mm" << endl;

        d = d / 10.0;   //cm
        cout << "距离是:" << d << "cm" << endl;

        // d = d/1000.0;   //m
        // cout << "距离是:" << d << "m" << endl;

        break;
    case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            break;
    }
}


int main()
{
    Mat left_cameraMatrix = Mat::eye(3, 3, CV_64F);//左相机内参矩阵
    left_cameraMatrix.at<double>(0, 0) = 5.780149331601065e+02;//Fx
    left_cameraMatrix.at<double>(0, 1) = 0;
    left_cameraMatrix.at<double>(0, 2) = 3.555745068743920e+02;//Cx
    left_cameraMatrix.at<double>(1, 1) = 5.792595377241396e+02;//Fy
    left_cameraMatrix.at<double>(1, 2) = 2.534936001929042e+02;//Cy

    Mat left_distCoeffs = Mat::zeros(5, 1, CV_64F);//畸变系数
    left_distCoeffs.at<double>(0, 0) = 0.060326909619728;//k1
    left_distCoeffs.at<double>(1, 0) = -0.006338890383364;//k2
    left_distCoeffs.at<double>(2, 0) = -4.984238272469574e-05;//p1
    left_distCoeffs.at<double>(3, 0) = -0.001636185247379;//p2
    left_distCoeffs.at<double>(4, 0) = -0.247991841327280;

    Mat right_cameraMatrix = Mat::eye(3, 3, CV_64F);//右相机内参矩阵
    right_cameraMatrix.at<double>(0, 0) = 5.778378235199527e+02;//Fx
    right_cameraMatrix.at<double>(0, 1) = 0;
    right_cameraMatrix.at<double>(0, 2) = 3.484120454356872e+02;//Cx
    right_cameraMatrix.at<double>(1, 1) = 5.793064373178438e+02;//Fy
    right_cameraMatrix.at<double>(1, 2) = 2.522698803266952e+02;//Cy

    Mat right_distCoeffs = Mat::zeros(5, 1, CV_64F);//畸变系数
    right_distCoeffs.at<double>(0, 0) = 0.069036951737383;//k1
    right_distCoeffs.at<double>(1, 0) = -0.074429302261481;//k2
    right_distCoeffs.at<double>(2, 0) = 5.694882132841171e-04;//p1
    right_distCoeffs.at<double>(3, 0) = -0.002668327489554;//p2
    right_distCoeffs.at<double>(4, 0) = -0.144742509783022;


    Mat rotation_matrix = Mat::zeros(3, 3, CV_64F);//旋转矩阵
    rotation_matrix.at<double>(0, 0) = 0.999996782379644;
    rotation_matrix.at<double>(0, 1) = 5.706428486117677e-04;
    rotation_matrix.at<double>(0, 2) = -0.002471759919034;
    rotation_matrix.at<double>(1, 0) = -5.720218896103840e-04;
    rotation_matrix.at<double>(1, 1) = 0.999999681132956;
    rotation_matrix.at<double>(1, 2) = -5.572476509248812e-04;
    rotation_matrix.at<double>(2, 0) = 0.002471441141484;
    rotation_matrix.at<double>(2, 1) = 5.586597586930527e-04;
    rotation_matrix.at<double>(2, 2) = 0.999996789933827;

    Mat rotation_vector;//旋转矩阵
    Rodrigues(rotation_matrix, rotation_vector);//旋转矩阵转化为旋转向量，罗德里格斯变换

    Mat translation_vector = Mat::zeros(3, 1, CV_64F);//平移向量
    translation_vector.at<double>(0, 0) = -59.684781615760150;
    translation_vector.at<double>(1, 0) = -0.319025755946363;
    translation_vector.at<double>(2, 0) = 0.275826997339757;

    Mat R1, R2;//左右相机的3x3矫正变换(旋转矩阵)
    Mat P1, P2;//左右相机新的坐标系统(矫正过的)输出 3x4 的投影矩阵
   // Mat Q;//深度视差映射矩阵

   // Rect left_valid_roi, right_valid_roi;//图像校正之后，会对图像进行裁剪，这里的validRoi就是指裁剪之后的区域
    Mat rmap[2][2];//映射表 必须为：CV_16SC2/CV_32FC1/CV_32FC2
    Size imageSize;
    imageSize = Size(left_img.cols, right_img.rows);

    /*
        立体校正的时候需要两幅图像共面并且行对准 以使得立体匹配更加的可靠
        使得两幅图像共面的方法就是把两个摄像头的图像投影到一个公共成像面上，这样每幅图像从本图像平面投影到公共图像平面都需要一个旋转矩阵R
        stereoRectify 这个函数计算的就是从图像平面投影都公共成像平面的旋转矩阵R1,R2。 R1,R2即为左右相机平面行对准的校正旋转矩阵。
        左相机经过R1旋转，右相机经过R2旋转之后，两幅图像就已经共面并且行对准了。
        其中P1,P2为两个相机的投影矩阵，其作用是将3D点的坐标转换到图像的2D点的坐标:P*[X Y Z 1]' =[x y w]
        Q矩阵为重投影矩阵，即矩阵Q可以把2维平面(图像平面)上的点投影到3维空间的点:Q*[x y d 1] = [X Y Z W]。其中d为左右两幅图像的视差
    */
    //双目校准
    stereoRectify(left_cameraMatrix, left_distCoeffs,//左摄像头内参和畸变系数
        right_cameraMatrix, right_distCoeffs,//右摄像头内参和畸变系数
        imageSize, rotation_vector, translation_vector,//图像大小，右摄像头相对于左摄像头旋转矩阵，平移向量
        R1, R2, P1, P2, Q,//输出的参数
        CALIB_ZERO_DISPARITY, -1, imageSize, &left_valid_roi, &right_valid_roi);
    //Precompute maps for cv::remap().
    //用来计算畸变映射，输出的X / Y坐标重映射参数,remap把求得的映射应用到图像上。
    initUndistortRectifyMap(left_cameraMatrix, left_distCoeffs, R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
    initUndistortRectifyMap(right_cameraMatrix, right_distCoeffs, R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
    //经过remap之后，左右相机的图像已经共面并且行对准了
    remap(left_img, left_rectified_img, rmap[0][0], rmap[0][1], INTER_LINEAR);
    remap(right_img, right_rectified_img, rmap[1][0], rmap[1][1], INTER_LINEAR);

    //立体匹配生成深度图需要用到灰度图
    cvtColor(left_rectified_img, left_rectified_gray, COLOR_BGR2GRAY);
    cvtColor(right_rectified_img, right_rectified_gray, COLOR_BGR2GRAY);

	stereo_SGBM_match(0, 0);
	 //创建SAD窗口 Trackbar
	createTrackbar("SADWindowSize:\n", "disparity_SGBM", &SADWindowSize, 50, stereo_SGBM_match);
	createTrackbar("mindisparity:\n", "disparity_SGBM", &mindisparity, 50, stereo_SGBM_match);
	 //创建视差唯一性百分比窗口 Trackbar
	createTrackbar("Disp12MaxDiff:\n", "disparity_SGBM", &Disp12MaxDiff, 13, stereo_SGBM_match);
	 //创建视差窗口 Trackbar
	createTrackbar("UniquenessRatio:\n", "disparity_SGBM", &UniquenessRatio, 55, stereo_SGBM_match);

    stereo_BM_match(0, 0);
    if (waitKey(10)==32)
{
    imwrite("‪C:\\Users\\fhlhc\\Desktop\\depth.png", disp8);
}
    // 创建SAD窗口 Trackbar
        createTrackbar("BlockSize:\n", "disparity_BM", &blockSize, 8, stereo_BM_match);
        // 创建视差唯一性百分比窗口 Trackbar
        createTrackbar("UniquenessRatio:\n", "disparity_BM", &uniquenessRatio, 50, stereo_BM_match);
        // 创建视差窗口 Trackbar
        createTrackbar("NumDisparities:\n", "disparity_BM", &numDisparities, 16, stereo_BM_match);
        setMouseCallback("disparity_BM", onMouse, 0);
        setMouseCallback("disparity_SGBM", onMouse, 0);
    while(1)
    {
        if (waitKey(0) == 32)
        {
            imwrite("C:\\Users\\fhlhc\\Desktop\\depth_4.png", disp8);
            break;
        }
        int key = waitKey(0);
        if (key == 27) break;
        //按下Esc退出
    }

	return 0;
}