OpenCV3.4.8实现立体视觉校正与标定

说明

1、先calibrateCamera()确定相机内参与畸变向量；再stereoCalirate()确定右相机相对于左相机的R、T，本征矩阵E，基本矩阵F；再立体标定stereoRectify()确定R1, R2, P1, P2, Q; 根据R1 , R2 重投影得到立体校正图像（共面行对齐）；再使用StereoBM or StereoSGBM计算Disparity ； 再reprojectImageTo3D()得到视差图的所有三维坐标（单位为mm，这里Tx(两相机之间的基线距离是以mm为单位)）
2、本征矩阵E，基本矩阵F的关系：本征矩阵包含了两个摄像机之间的所有几何信息，但不包含摄像机的任何信息，基本矩阵F在本征矩阵的基础上引入了两个相机的内参数信息；

1、本征矩阵E将左侧摄像机所观察到的点P的物理坐标位置与右侧摄像机所看到的
相同的点位置相关联起来；
2、基本矩阵F将图像坐标系（像素）中一个摄像机的像平面上的点与另一个摄像机
的像平面上的点关联起来；

3、立体标定函数stereoCalibrate()原理：不要使用该函数一次性计算出所有参数，可能导致异常结果；
①使用calibrateCamera()函数计算相机的内参数，畸变向量；
②根据如下图的关系，每幅图像对都可以计算出一对R、T；

③采用鲁棒的Levenberg-Marquardt迭代算法找到最小投影误差，计算出最佳R，T作为立体标定参数；
4、立体测量的数学原理：

5、三维深度的测量方法：测量三维坐标为(X/W , Y/W , Z/W)；实现函数：reporjectImageTo3D()；

Code

#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>

using namespace cv;
using namespace std;

void createCalibImg();
bool SingleCalib(string calibdata, string calibimgname, Size Pattern_size, int Square_size, int& imgNum,
				 Mat& CameraMatrix, Mat& DistCoeffs, vector<Mat>& Rvecs, vector<Mat>& Tvecs, double& totalerror,
				 vector<vector<Point3f>>& _objectPoints, vector<vector<Point2f>>& _imgPoints,  Size& _image_size);
double Stereo_Calib_Quanlity_Check(vector<vector<Point2f>>& _leftimgPoints, int& leftimgNum, vector<vector<Point2f>>& _rightimgPoints, int& rightimgNum,
								   const Mat& leftCameraMatrix, const Mat& leftDistCoeffs, const Mat& _R1, const Mat& _P1,
								   const Mat& rightCameraMatrix, const Mat& rightDistCoeffs, const Mat& _R2, const Mat& _P2,
								   Mat& _F, Size _pattern_size);

int main(int argc, char** argv)
{
   
   
/*1.生成读取用的照片序列文件*/
	//createCalibImg();
	
/*2.左右相机标定（内参矩阵、畸变向量）*/
	Size board_size = Size(6, 9);	//棋盘格行列角点数量
	int square_size = 50;			//棋盘格方块大小
	int leftimgNum = 0, rightimgNum = 0;
	Mat leftCamMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));
	Mat rightCamMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));
	Mat leftDistCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));
	Mat rightDistCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));	
	vector<Mat> leftRvec, leftTvec;
	vector<Mat> rightRvec, rightTvec;
	
	vector<vector<Point3f>>  leftobjPoints;			//这些数据供第三步立体标定使用
	vector<vector<Point3f>>  rightobjPoints;
	vector<vector<Point2f>>  leftimgPoints;
	vector<vector<Point2f>>  rightimgPoints;
	Size leftImgSize;
	Size rightImgSize;

	double leftTotalerror = 0.0, rightTotalerror = 0.0;
	SingleCalib("calibimg.yaml", "leftimg", board_size, square_size, leftimgNum,
				leftCamMatrix, leftDistCoeffs, leftRvec, leftTvec, leftTotalerror,leftobjPoints,leftimgPoints,leftImgSize); //左相机标定
	SingleCalib("calibimg.yaml", "rightimg", board_size, square_size, rightimgNum,
				rightCamMatrix, rightDistCoeffs, rightRvec, rightTvec, rightTotalerror,rightobjPoints,rightimgPoints, rightImgSize);  //右相机标定	

/*3.立体标定（右相机相对左相机的旋转矩阵与平移向量）*/
	Mat R = Mat(3, 3, CV_32FC1, Scalar::all(0));
	Mat T = Mat(3,1,CV_32FC1,Scalar::all(0));		
	Mat E = Mat(3, 3, CV_32FC1, Scalar::all(0));  //3 * 3 矩阵
	Mat F = Mat(3, 3, CV_32FC1, Scalar::all(0));

	stereoCalibrate(leftobjPoints, leftimgPoints, rightimgPoints, leftCamMatrix, leftDistCoeffs,
					rightCamMatrix, rightDistCoeffs, leftImgSize, R, T, E, F, CALIB_FIX_INTRINSIC);
	

/*4.立体校正（Bougut算法）Calculate R1 R2 P1 P2 Q */
	Mat R1, R2, P1, P2, Q;
	stereoRectify(leftCamMatrix, leftDistCoeffs, rightCamMatrix, rightDistCoeffs, leftImgSize,
				R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY); //立体校正，计算校正参数
	//4.1 衡量立体校正效果
	double Stereoerr = Stereo_Calib_Quanlity_Check(leftimgPoints, leftimgNum, rightimgPoints, rightimgNum, leftCamMatrix, leftDistCoeffs, R1, P1,
													rightCamMatrix, rightDistCoeffs, R2, P2, F, board_size);  
	cout << "stereo calib quanlity:" << Stereoerr << endl;
	Mat lmap1, lmap2;
	Mat rmap1, rmap2;
	initUndistortRectifyMap(leftCamMatrix, leftDistCoeffs, R1, P1, leftImgSize, CV_16SC2, lmap1, lmap2);	//计算左相机视图矫正映射
	initUndistortRectifyMap(rightCamMatrix, rightDistCoeffs, R2, P2, rightImgSize, CV_16SC2, rmap1, rmap2); //计算右相机视图矫正映射
	//4.2 生成立体校正后的图像（共面行对准）
	FileStorage fs2 = FileStorage("calibimg.yaml", FileStorage::READ);
	FileNode leftimgSeq = fs2["leftimg"]