opencv sgbm 三维重建_opencv立体匹配总是不行，求大佬帮助

const int imageWidth = 640;                             //摄像头的分辨率

const int imageHeight = 480;

Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;

Mat rgbImageR, grayImageR;

Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域

Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表

Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q

Mat xyz;              //三维坐标

Point origin;         //鼠标按下的起始点

Rect selection;      //定义矩形选框

bool selectObject = false;    //是否选择对象

//int blockSize = 3, uniquenessRatio = 10, numDisparities = 16;

Ptr sgbm = StereoSGBM::create(0,16,3);

/*

事先标定好的相机的参数

fx 0 cx

0 fy cy

0 0  1

*/

Mat cameraMatrixL = (Mat_(3, 3) << 730.9859, 0.1427 , 294.2559,

0   , 735.3010, 261.9521,

0   ,     0   ,    1    );

Mat distCoeffL = (Mat_(5, 1) << -0.4740, 0.1766,0.0017, -0.0039,0.1152);

Mat cameraMatrixR = (Mat_(3, 3) << 734.7534, 0.2424  , 283.8769,

0   , 737.4809, 342.1307,

0   ,     0   ,    1    );

Mat distCoeffR = (Mat_(5, 1) << -0.5229, 0.6307,-0.0004, -0.0037,-0.7994);

Mat T = (Mat_(3, 1) << 9.1664, 0.0753, -0.2054);//T平移向量

//Mat rec = (Mat_(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量

Mat R = (Mat_(3, 3) << 0.9997, 0.0175, -0.0140,

-0.0172, 0.9996, 0.0207,

0.0144, -0.0204, 0.9997);

/*****立体匹配*****/

void stereo_match(int, void*)

{

sgbm->setPreFilterCap(15);

int sgbmWinSize = 5;//根据实际情况自己设定

int NumDisparities = 624;//根据实际情况自己设定

int UniquenessRatio = 10;//根据实际情况自己设定

sgbm->setBlockSize(sgbmWinSize);

int cn = rectifyImageL.channels();

sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);

sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);

sgbm->setMinDisparity(0);

sgbm->setNumDisparities(NumDisparities);

sgbm->setUniquenessRatio(UniquenessRatio);

sgbm->setSpeckleWindowSize(100);

sgbm->setSpeckleRange(10);

sgbm->setDisp12MaxDiff(1);

sgbm->setMode(StereoSGBM::MODE_SGBM);

Mat disp, dispf, disp8;

sgbm->compute(rectifyImageL, rectifyImageR, disp);

//去黑边

Mat img1p, img2p;

copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);

copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);

dispf = disp.colRange(NumDisparities, img2p.cols - NumDisparities);

dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));

reprojectImageTo3D(dispf, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。

xyz = xyz * 16;

imshow("disparity", disp8);

}

/*****描述：鼠标操作回调*****/

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 = abs(x - origin.x);

selection.height = 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(origin) << endl;

break;

case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件

selectObject = false;

if (selection.width > 0 && selection.height > 0)

break;

}

}

int main(int argc, char** argv)

{

stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,

-1, imageSize, &validROIL, &validROIR);

initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);

initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

/*

读取图片

*/

rgbImageL = imread("came04L.jpg", -1);

cvtColor(rgbImageL, grayImageL, COLOR_BGR2GRAY);

rgbImageR = imread("came04R.jpg", -1);

cvtColor(rgbImageR, grayImageR, COLOR_BGR2GRAY);

imshow("ImageL Before Rectify", grayImageL);

imshow("ImageR Before Rectify", grayImageR);

/*

经过remap之后，左右相机的图像已经共面并且行对准了

*/

remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);

remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

/*

把校正结果显示出来

*/

Mat rgbRectifyImageL, rgbRectifyImageR;

cvtColor(rectifyImageL, rgbRectifyImageL, COLOR_GRAY2BGR);  //伪彩色图

cvtColor(rectifyImageR, rgbRectifyImageR, COLOR_GRAY2BGR);

//单独显示

//rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);

//rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);

imshow("ImageL After Rectify", rgbRectifyImageL);

imshow("ImageR After Rectify", rgbRectifyImageR);

//显示在同一张图上

Mat canvas;

double sf;

int w, h;

sf = 600. / MAX(imageSize.width, imageSize.height);

w = cvRound(imageSize.width * sf);

h = cvRound(imageSize.height * sf);

canvas.create(h, w * 2, CV_8UC3);   //注意通道

//左图像画到画布上

Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分

resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小

Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域

cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));

rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形

cout << "Painted ImageL" << endl;

//右图像画到画布上

canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分

resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);

Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),

cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));

rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);

cout << "Painted ImageR" << endl;

//画上对应的线条

for (int i = 0; i < canvas.rows; i += 16)

line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);

imshow("rectified", canvas);

/*

立体匹配

*/

namedWindow("disparity", WINDOW_NORMAL);

setMouseCallback("disparity", onMouse, 0);

stereo_match(0,0);

waitKey(0);

return 0;

}

D:/18982/Pictures/94E02F691CB803306287A2309D13D459.jpg