基于OpenCV立体视觉标定和校正

这几天学习双目视觉标定，分别使用了两种工具：OpenCV和Matlab。Matlab的效果非常稳定，但是一开始OpenCV的效果很糟糕，要不是出现中断就是标定出来的结果数值很大。经过了几天的不断调试和更改，终于把OpenCV的立体视觉标定和校正的程序写出来了。立体标定时计算空间上的两台摄像机几何关系的过程，立体校正则是对个体图像进行纠正，保证这些图像可以从平面对准的两幅图像获得。程序的框架如下：

1.读取左右相机图片序列

双目相机的图片序列放在Demon的路径下，左右相机的图像的名字分别存放在两个txt文件中，程序分别通过这两个txt文件读取对应的图片序列。主注意：我们假设已经将摄像机排列好了，其图像扫描线是粗略物理对齐，从而使得每台摄像机本质上都具有相同的视场。

2.提取图片角点，并分别标定左右相机内参矩阵和畸变向量

调用cvFindChessboardCorners找出图像中的角点，然后调用cvFindCornerSubPix计算亚像素精度角点位置，将全部找出来的角点位置压入一个矩阵序列中，以及初始化角点在世界坐标系的对应位置序列，本程序的世界坐标系长度单位取标定板放个边长。然后用cvCalibrateCamera2分别标定做右相机的内参矩阵和畸变系数向量。将该过程封装成一个函数，具体过程请参考程序注释：

/*单个相机标定函数：
输入参数：
const char* imageList      IN保存图片名的txt文件
CvMat* object_points       OUT世界坐标系点的矩阵
CvMat* image_points        OUT图像坐标系矩阵
CvMat* intrinsic_matrix    OUT返回的内参数矩阵
CvMat* distortion_coeffs   OUT返回的畸变向量
int n_boards               IN图片的数量
int board_w                IN每张图片x方向角点数量
int board_h                IN每张图片y方向角点数量
CvSize* imgSize            OUT每张图片的像素尺寸
*/
static void SingleCalib(const char* imageList, CvMat* object_points, CvMat* image_points, CvMat* intrinsic_matrix, CvMat* distortion_coeffs,
	int n_boards, int board_w, int board_h, CvSize* imgSize)
{
	//定义文件类
	FILE* f;
	fopen_s(&f, imageList, "rt");

	int board_n = board_w*board_h;//每张图片中角点总数量
	CvSize board_sz = cvSize(board_w, board_h);//角点尺寸矩阵

	CvPoint2D32f* corners = new CvPoint2D32f[board_w*board_h];//定义用于存放每张图片角点数量的一维点数组
	CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);//向量，每个元素代表每张图片角点的数量
	int successes = 0;//找到全部角点的图片数量
	int step = 0;//用于记录每张图片角点的起始位置

	//文件读取不成功:
	if (!f)
	{
		fprintf(stderr, "can not open file %s\n", imageList);//要读写, 得知道从哪里读, 往哪里写吧?stderr -- 标准错误输出设备
		return;
	}
	//利用i循环读取文件中的字符，然后用于读取图片
	for (int i = 0;; i++)
	{
		//读取图片
		char buf[1024];//存放读取的字符数组
		int count = 0, result = 0;//count找的的角点数量，result找角点结果标志，全部角点找到非零，否者为0；

		if (!fgets(buf, sizeof(buf)-3, f))//提取文件的字符存放到buf
			break;
		size_t len = strlen(buf);//len为字符数组的长度
		while (len > 0 && isspace(buf[len - 1]))//int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格(' ')、定位字符(' \t ')、CR(' \r ')、换行(' \n ')
			buf[--len] = '\0';					//、垂直定位字符(' \v ')或翻页(' \f ')的情况。，既在非空白字符的后面以为添加‘\0’
		if (buf[0] == '#')//开头为'#',结束此次循环
			continue;
		IplImage* img = cvLoadImage(buf, 0);//读取图片
		if (!img)
			break;
		//获取图片尺寸
		*imgSize = cvGetSize(img);
		//提取角点
		result = cvFindChessboardCorners(img, cvSize(board_w, board_h), corners, &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);

		if (result)
		{
			//Calibration will suffer without subpixel interpolation
			//函数 cvFindCornerSubPix 通过迭代来发现具有亚象素精度的角点位置
			cvFindCornerSubPix(img, corners, count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
			/*TermCriteria迭代算法的终止准则:
			typedef struct CvTermCriteria0d
			{
			int    type;  CV_TERMCRIT_ITER 和CV_TERMCRIT_EPS二值之一，或者二者的组合
			int    max_iter; 最大迭代次数
			double epsilon; 结果的精确性
			}
			一般表示迭代终止的条件，如果为CV_TERMCRIT_ITER，则用最大迭代次数作为终止条件，如果为CV_TERMCRIT_EPS 则用精度作为迭代条件，
			如果为CV_TERMCRIT_ITER+CV_TERMCRIT_EPS则用最大迭代次数或者精度作为迭代条件，看哪个条件先满足。*/

			//开始保存角点的其实位置
			step = successes*board_n;
			//将角点从数组corners压入矩阵image_points；以及给对应的object_points赋值
			for (int i = step, j = 0; j < board_n; ++i, ++j)
			{
				CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
				CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
				CV_MAT_ELEM(*object_points, float, i, 0) = (j / board_w);
				CV_MAT_ELEM(*object_points, float, i, 1) = (j % board_w);
				CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
			}
			//给对应图片的point_counts赋值
			CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;
			successes++;
		}
		//释放该角点图像
		cvReleaseImage(&img);
	}
	//关闭文件
	fclose(f);

	//初始化相机内参矩阵
	CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;
	CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;
	//标定相机的内参矩阵和畸变系数向量
	cvCalibrateCamera2(object_points, image_points, point_counts, *imgSize, intrinsic_matrix, distortion_coeffs, NULL, NULL, 0);

}

3.立体标定，计算两摄像机相对旋转矩阵 R,平移向量 T, 本征矩阵E, 基础矩阵F

第2步获得了在世界坐标系和图片坐标系下角点位置序列，以及两个相机的内参矩阵和畸变系数向量，然后调用cvStereoCalibrate计算摄像机相对旋转矩阵 R,平移向量 T, 本征矩阵E, 基础矩阵F,，并同时调整第2步计算的做右相机内参矩阵和畸变系数向量。但是处于个人习惯还是喜欢将该过程封装到一个函数里:

static void StereoCalib(CvMat* _left_object_points, CvMat* _left_image_points, CvMat* _right_image_points, CvMat* left_intrinsic, CvMat* right_intrinsic,
	CvMat* left_distortion, CvMat* right_distortion, CvMat* _R, CvMat* _T, CvMat* _E, CvMat* _F, int _n_boards, int _board_w, int _board_h, CvSize _imgSize)
{
	int board_n = _board_w*_board_h;//每张图片中角点总数量

	//初始化Number_perImg
	CvMat* Number_perImg = cvCreateMat(1, _n_boards, CV_32SC1);
	int* pInt;
	pInt = (int*)(Number_perImg->data.ptr);
	for (int i = 0; i < _n_boards; ++i)
	{
		*pInt = board_n;
		pInt++;
	}
	//Show(用于调试)
	/*for (int i = 0; i < _n_boards; i++)
	{
	cout << CV_MAT_ELEM(*Number_perImg, int, 0, i) << endl;
	}*/

	
	//立体标定.计算 _R, _T, _E, _F,，并同时调整Number_perImg, left_intrinsic, D_left,right_intrinsic, D_right
	cvStereoCalibrate(_left_object_points, _left_image_points, _right_image_points, Number_perImg, left_intrinsic, left_distortion,
		right_intrinsic, right_distortion, _imgSize, _R, _T, _E, _F,
		cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH);
}

计算得到如下的结果（到目前为止还是很顺利的嘛！）：

4.上述标定结果和Matlab的标定结果对比差别不大。下面通过另一种方法检查标定结果的误差，通过检查图像上的点与另一幅图像的极线的距离远近来评价标定精度。首先，通过cvUndistortPoints()对原始点进行去畸变处理，然后使用cvComputeCorrespondEplilines()来计算极线，然后计算这些点和极线的距离（理想情况下，距离为0），累计这些距离的绝对误差，然后求其平均值。具体过程我也把它封装成一个函数：

/*计算标定误差函数
CvMat* left_image_points     IN左相机图像坐标系点的矩阵
CvMat* right_image_points    IN右相机图像坐标系点的矩阵
CvMat* left_intrinsic        INandOUT左相机的内参矩阵，经立体标定调整后输出
CvMat* right_intrinsic       INandOUT右相机的内参矩阵，经立体标定调整后输出
CvMat* left_distortion       INandOUT左相机的畸变向量，经立体标定调整后输出
CvMat* right_distortion      INandOUT右相机的畸变向量，经立体标定调整后输出
CvMat* _F                    OUT基础矩阵
int n_boards                 IN图片的数量
int board_w                  IN每张图片x方向角点数量
int board_h                  IN每张图片y方向角点数量
*/
double Calib_Quality_Check(CvMat* _left_image