【双目视觉探索路5】分析整理Learning OpenCV3书中立体标定、校正以及对应代码（3）之SGBM算法

第六步,SGBM匹配

上一章部分对标定、校正的部分代码进行展示，本部分将对匹配这一块进行学习。

SGBM相关学习链接：

1，原始文献：Heiko Hirschmuller. Stereo processing by semiglobal matching and mutual information.Pattern Analysis and Machine Intelligence, IEEE Transactions on, 30(2):328–341, 2008.

2，cv::StereoSGBM Class Reference

3，opencvSGBM版半全局立体匹配算法的研究

SGBM的基本原理

SGBM的基本步骤涉及：预处理、代价计算、动态规划以及后处理

StereoSGBM的原型

static Ptr<StereoSGBM> cv::StereoSGBM::create(int minDisparity = 0,int numDisparities = 16,int blockSize = 3,int P1 = 0,int P2 = 0,int disp12MaxDiff = 0,int preFilterCap = 0,int uniquenessRatio = 0,int speckleWindowSize = 0,int speckleRange = 0,int mode = StereoSGBM::MODE_SGBM )

第一个参数minDisparity，一般情况下为0，但有可能矫正算法会移动图像，因此，参数需要进行调整

第二个参数numDisparities,最大视差减最小视差，现在的算法中，参数必须为16所整除

第三个参数blockSize，块匹配的大小，应该为奇数，在3~11的范围

第四、五个参数P1，P2：控制视差图的光滑度

由于缺乏很多计算机及图像处理、计算机视觉的基础知识，比如路径规划、团块匹配等等

虽然很多细节没有搞懂，但通过连猜带蒙，把这部分程序搞的没有bug了

贴个其中一幅的视差图

SGBM部分程序

先放上SGBM的程序（原谅我很多不知道怎么处理，有些程序在瞎掰，比如传说中的cvLoadImage与Mat齐飞）

StereoSGBM sgbm;int SADWindowSize = 9;//预处理sobel，获得图像梯度信息，用于计算代价sgbm.preFilterCap = 63;//代价参数，得到SAD代价int numberOfDisparities = 64;sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;sgbm.minDisparity = 0;sgbm.numberOfDisparities = numberOfDisparities;//动态规划参数，默认四条路径IplImage *img1= cvLoadImage("left01.jpg", 0);int cn = img1->nChannels;sgbm.P1 = 8 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;sgbm.P2 = 32 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;//后处理参数，唯一性检测、亚像素插值、左右一致性检测、连通区域检测sgbm.uniquenessRatio = 10;sgbm.speckleWindowSize = 100;sgbm.speckleRange = 32;sgbm.disp12MaxDiff = 1;Mat disp, disp8;for (i = 0; i < nframes; i++) {cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0);cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0);cv::Mat img1r, img2r, disp, vdisp;if (img1.empty() || img2.empty())continue;cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR);cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR);if (!isVerticalStereo || !useUncalibrated) {// When the stereo camera is oriented vertically,// Hartley method does not transpose the// image, so the epipolar lines in the rectified// images are vertical. Stereo correspondence// function does not support such a case.sgbm(img1r, img2r, disp);cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U);cv::imshow("disparity", vdisp);}if (!isVerticalStereo) {cv::Mat part = pair.colRange(0, imageSize.width);cvtColor(img1r, part, cv::COLOR_GRAY2BGR);part = pair.colRange(imageSize.width, imageSize.width * 2);cvtColor(img2r, part, cv::COLOR_GRAY2BGR);for (j = 0; j < imageSize.height; j += 16)cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j),cv::Scalar(0, 255, 0));}else {cv::Mat part = pair.rowRange(0, imageSize.height);cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR);part = pair.rowRange(imageSize.height, imageSize.height * 2);cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR);for (j = 0; j < imageSize.width; j += 16)line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2),cv::Scalar(0, 255, 0));}cv::imshow("rectified", pair);if ((cv::waitKey() & 255) == 27)break;}

整体程序

再扔一个整体程序（请无视那一堆恶心的头文件，我只是为了调bug，瞎扔的0.0!!）

#include <opencv2/opencv.hpp>#include<opencv2/calib3d/calib3d.hpp>#include<opencv2/core/core.hpp>#include<opencv2/core/core.hpp>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>#include <iostream>#include <string.h>#include <stdlib.h>#include <stdio.h>#include <math.h>using namespace cv;using namespace std;void help(char *argv[]) {cout<< "\n\nExample 19-3. Stereo calibration, rectification, and ""correspondence"<< "\n    Reads in list of locations of a sequence of checkerboard ""calibration"<< "\n    objects from a left,right stereo camera pair. Calibrates, ""rectifies and then"<< "\n    does stereo correspondence."<< "\n"<< "\n    This program will run on default parameters assuming you ""created a build directory"<< "\n    directly below the Learning-OpenCV-3 directory and are ""running programs there.   NOTE: the list_of_stereo_pairs> must"<< "\n    give the full path name to the left right images, in ""alternating"<< "\n    lines: left image, right image, one path/filename per line, see"<< "\n    stereoData/example_19-03_list.txt file, you can comment out ""lines"<< "\n    there by starting them with #."<< "\n"<< "\nDefault Call (with parameters: board_w = 9, board_h = 6, list = ""../stereoData_19-03_list.txt):"<< "\n" << argv[0] << "\n"<< "\nManual call:"<< "\n" << argv[0] << " [<board_w> <board_h> <path/list_of_stereo_pairs>]"<< "\n\n PRESS ANY KEY TO STEP THROUGH RESULTS AT EACH STAGE."<< "\n" << endl;}static void StereoCalib(const char *imageList, int nx, int ny,bool useUncalibrated) {bool displayCorners = true;bool showUndistorted = true;bool isVerticalStereo = false; // horiz or vert camsconst int maxScale = 1;const float squareSize = 1.f;// actual square sizeFILE *f;fopen_s(&f,imageList, "rt");int i, j, lr;int N = nx * ny;cv::Size board_sz = cv::Size(nx, ny);vector<string> imageNames[2];vector<cv::Point3f> boardModel;vector<vector<cv::Point3f> > objectPoints;vector<vector<cv::Point2f> > points[2];vector<cv::Point2f> corners[2];bool found[2] = { false, false };cv::Size imageSize;// READ IN THE LIST OF CIRCLE GRIDS://if (!f) {cout << "Cannot open file " << imageList << endl;return;}for (i = 0; i < ny; i++)for (j = 0; j < nx; j++)boardModel.push_back(cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f));i = 0;for (;;) {char buf[1024];lr = i % 2;if (lr == 0)found[0] = found[1] = false;if (!fgets(buf, sizeof(buf) - 3, f))break;size_t len = strlen(buf);while (len > 0 && isspace(buf[len - 1]))buf[--len] = '\0';if (buf[0] == '#')continue;Mat img = imread(buf, 0);if (img.empty())break;imageSize = img.size();imageNames[lr].push_back(buf);i++;// If we did not find board on the left image,// it does not make sense to find it on the right.//if (lr == 1 && !found[0])continue;// Find circle grids and centers therein:for (int s = 1; s <= maxScale; s++) {Mat timg = img;if (s > 1)resize(img, timg, Size(), s, s, INTER_CUBIC);// Just as example, this would be the call if you had circle calibration// boards ...//      found[lr] = cv::findCirclesGrid(timg, cv::Size(nx, ny),//      corners[lr],//                                      cv::CALIB_CB_ASYMMETRIC_GRID |//                                          cv::CALIB_CB_CLUSTERING);//...but we have chessboards in our imagesfound[lr] = findChessboardCorners(timg, board_sz, corners[lr]);if (found[lr] || s == maxScale) {Mat mcorners(corners[lr]);mcorners *= (1. / s);}if (found[lr])break;}if (displayCorners) {cout << buf << endl;Mat cimg;cvtColor(img, cimg, COLOR_GRAY2BGR);// draw chessboard corners works for circle grids toodrawChessboardCorners(cimg,  Size(nx, ny), corners[lr], found[lr]);imshow("Corners", cimg);if ((waitKey(0) & 255) == 27) // Allow ESC to quitexit(-1);}elsecout << '.';if (lr == 1 && found[0] && found[1]) {objectPoints.push_back(boardModel);points[0].push_back(corners[0]);points[1].push_back(corners[1]);}}fclose(f);// CALIBRATE THE STEREO CAMERASMat M1 = Mat::eye(3, 3, CV_64F);Mat M2 = Mat::eye(3, 3, CV_64F);Mat D1, D2, R, T, E, F;cout << "\nRunning stereo calibration ...\n";stereoCalibrate(objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F,TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 100, 1e-5),CALIB_FIX_ASPECT_RATIO | CALIB_ZERO_TANGENT_DIST |CALIB_SAME_FOCAL_LENGTH);// CALIBRATION QUALITY CHECK// because the output fundamental matrix implicitly// includes all the output information,// we can check the quality of calibration using the// epipolar geometry constraint: m2^t*F*m1=0vector<cv::Point3f> lines[2];double avgErr = 0;int nframes = (int)objectPoints.size();for (i = 0; i < nframes; i++) {vector<cv::Point2f> &pt0 = points[0][i];vector<cv::Point2f> &pt1 = points[1][i];cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1);cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2);cv::computeCorrespondEpilines(pt0, 1, F, lines[0]);cv::computeCorrespondEpilines(pt1, 2, F, lines[1]);for (j = 0; j < N; j++) {double err = fabs(pt0[j].x * lines[1][j].x + pt0[j].y * lines[1][j].y +lines[1][j].z) +fabs(pt1[j].x * lines[0][j].x + pt1[j].y * lines[0][j].y +lines[0][j].z);avgErr += err;}}cout << "avg err = " << avgErr / (nframes * N) << endl;// COMPUTE AND DISPLAY RECTIFICATION//if (showUndistorted) {cv::Mat R1, R2, P1, P2, map11, map12, map21, map22;// IF BY CALIBRATED (BOUGUET'S METHOD)//if (!useUncalibrated) {stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2,cv::noArray(), 0);isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));// Precompute maps for cvRemap()initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,map12);initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,map22);}// OR ELSE HARTLEY'S METHOD//else {// use intrinsic parameters of each camera, but// compute the rectification transformation directly// from the fundamental matrixvector<cv::Point2f> allpoints[2];for (i = 0; i < nframes; i++) {copy(points[0][i].begin(), points[0][i].end(),back_inserter(allpoints[0]));copy(points[1][i].begin(), points[1][i].end(),back_inserter(allpoints[1]));}cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT);cv::Mat H1, H2;cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize,H1, H2, 3);R1 = M1.inv() * H1 * M1;R2 = M2.inv() * H2 * M2;// Precompute map for cvRemap()//cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,map12);cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,map22);}// RECTIFY THE IMAGES AND FIND DISPARITY MAPS//cv::Mat pair;if (!isVerticalStereo)pair.create(imageSize.height, imageSize.width * 2, CV_8UC3);elsepair.create(imageSize.height * 2, imageSize.width, CV_8UC3);// Setup for finding stereo correspondences//StereoSGBM sgbm;int SADWindowSize = 9;//预处理sobel，获得图像梯度信息，用于计算代价sgbm.preFilterCap = 63;//代价参数，得到SAD代价int numberOfDisparities = 64;sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;sgbm.minDisparity = 0;sgbm.numberOfDisparities = numberOfDisparities;//动态规划参数，默认四条路径IplImage *img1= cvLoadImage("left01.jpg", 0);int cn = img1->nChannels;sgbm.P1 = 8 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;sgbm.P2 = 32 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;//后处理参数，唯一性检测、亚像素插值、左右一致性检测、连通区域检测sgbm.uniquenessRatio = 10;sgbm.speckleWindowSize = 100;sgbm.speckleRange = 32;sgbm.disp12MaxDiff = 1;Mat disp, disp8;for (i = 0; i < nframes; i++) {cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0);cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0);cv::Mat img1r, img2r, disp, vdisp;if (img1.empty() || img2.empty())continue;cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR);cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR);if (!isVerticalStereo || !useUncalibrated) {// When the stereo camera is oriented vertically,// Hartley method does not transpose the// image, so the epipolar lines in the rectified// images are vertical. Stereo correspondence// function does not support such a case.sgbm(img1r, img2r, disp);cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U);cv::imshow("disparity", vdisp);}if (!isVerticalStereo) {cv::Mat part = pair.colRange(0, imageSize.width);cvtColor(img1r, part, cv::COLOR_GRAY2BGR);part = pair.colRange(imageSize.width, imageSize.width * 2);cvtColor(img2r, part, cv::COLOR_GRAY2BGR);for (j = 0; j < imageSize.height; j += 16)cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j),cv::Scalar(0, 255, 0));}else {cv::Mat part = pair.rowRange(0, imageSize.height);cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR);part = pair.rowRange(imageSize.height, imageSize.height * 2);cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR);for (j = 0; j < imageSize.width; j += 16)line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2),cv::Scalar(0, 255, 0));}cv::imshow("rectified", pair);if ((cv::waitKey() & 255) == 27)break;}}}int main(int argc, char **argv) {help(argv);int board_w = 9, board_h = 6;const char *board_list = "read.txt";if (argc == 4) {board_list = argv[1];board_w = atoi(argv[2]);board_h = atoi(argv[3]);}StereoCalib(board_list, board_w, board_h, true);return 0;}

下一部分将是进一步的深入这一部分