【双目视觉探索路4】分析整理Learning OpenCV3书中立体标定、校正以及对应代码（2）之部分验证

本文链接：https://blog.youkuaiyun.com/u010369450/article/details/78823502

前一部分展示了工作流程及修改的主程序内容，这一部分为剩下的代码解读。

本次解读主要是功能性验证，对于实现的细节问题会略有提及，但不是主要重心。

回忆一下整个流程的五个步骤：

1，读入立体图像对(stereo image pairs)并获得亚像素精度级别的位置信息。

2，调用stereoCalibrate()函数进行立体标定获得本征矩阵

3，评定精度（采用点与极线之间的距离进行评价）

4，校正图像（去除畸变，输出行对准图像）

5，由StereoSGBM获得视差图

子程序解读

第一步：读入立体图像对

这一步的目的是为了获取亚像素精度级别的位置信息，为后续的立体标定提供数据上的支持，程序如下：

static void StereoCalib(const char *imageList, int nx, int ny,
	bool useUncalibrated) {
	bool displayCorners = true;
	bool showUndistorted = true;
	bool isVerticalStereo = false; // horiz or vert cams
	const int maxScale = 1;
	const float squareSize = 1.f;

	// actual square size
	FILE *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;
		cv::Mat img = cv::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++) {
			cv::Mat timg = img;
			if (s > 1)
				resize(img, timg, cv::Size(), s, s, cv::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 images
			found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]);

			if (found[lr] || s == maxScale) {
				cv::Mat mcorners(corners[lr]);
				mcorners *= (1. / s);
			}
			if (found[lr])
				break;
		}
		if (displayCorners) {
			cout << buf << endl;
			cv::Mat cimg;
			cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR);

			// draw chessboard corners works for circle grids too
			cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]);
			cv::imshow("Corners", cimg);
			if ((cv::waitKey(0) & 255) == 27) // Allow ESC to quit
				exit(-1);
		}
		else
			cout << '.';
		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);
}

运行完该步程序，每张标定板的图像可得下图

第二步，调用stereoCalibrate获得本征矩阵

源代码的bug出在参数表写倒了，在这里重新进行调整，贴一下

	// CALIBRATE THE STEREO CAMERAS
	Mat 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
	);

第三步，评定精度

	vector<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;

第四步，计算显示校正结果

	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 matrix
			vector<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);
		}

第五步，校正图像并寻找视差图

		cv::Mat pair;
		if (!isVerticalStereo)
			pair.create(imageSize.height, imageSize.width * 2, CV_8UC3);
		else
			pair.create(imageSize.height * 2, imageSize.width, CV_8UC3);