由两张视差图生成深度图
由两张视差图生成深度图，SGBM方法输入的是未矫正的图片，BM输入的是校正后的图片
在这里插入代码片#include <opencv2/highgui/highgui.hpp>
#include (origin) << endl;
point3 = xyz.at<Vec3f>(origin);
point3[0];
//cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]<
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;
}