立体视觉标定和校正

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

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

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

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

[cpp]  view plain  copy

1. /*单个相机标定函数： 
2. 输入参数： 
3. const char* imageList      IN保存图片名的txt文件 
4. CvMat* object_points       OUT世界坐标系点的矩阵 
5. CvMat* image_points        OUT图像坐标系矩阵 
6. CvMat* intrinsic_matrix    OUT返回的内参数矩阵 
7. CvMat* distortion_coeffs   OUT返回的畸变向量 
8. int n_boards               IN图片的数量 
9. int board_w                IN每张图片x方向角点数量 
10. int board_h                IN每张图片y方向角点数量 
11. CvSize* imgSize            OUT每张图片的像素尺寸 
12. */  
13. static void SingleCalib(const char* imageList, CvMat* object_points, CvMat* image_points, CvMat* intrinsic_matrix, CvMat* distortion_coeffs,  
14.     int n_boards, int board_w, int board_h, CvSize* imgSize)  
15. {  
16.     //定义文件类  
17.     FILE* f;  
18.     fopen_s(&f, imageList, "rt");  
19.   
20.     int board_n = board_w*board_h;//每张图片中角点总数量  
21.     CvSize board_sz = cvSize(board_w, board_h);//角点尺寸矩阵  
22.   
23.     CvPoint2D32f* corners = new CvPoint2D32f[board_w*board_h];//定义用于存放每张图片角点数量的一维点数组  
24.     CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);//向量，每个元素代表每张图片角点的数量  
25.     int successes = 0;//找到全部角点的图片数量  
26.     int step = 0;//用于记录每张图片角点的起始位置  
27.   
28.     //文件读取不成功:  
29.     if (!f)  
30.     {  
31.         fprintf(stderr, "can not open file %s\n", imageList);//要读写, 得知道从哪里读, 往哪里写吧?stderr -- 标准错误输出设备  
32.         return;  
33.     }  
34.     //利用i循环读取文件中的字符，然后用于读取图片  
35.     for (int i = 0;; i++)  
36.     {  
37.         //读取图片  
38.         char buf[1024];//存放读取的字符数组  
39.         int count = 0, result = 0;//count找的的角点数量，result找角点结果标志，全部角点找到非零，否者为0；  
40.   
41.         if (!fgets(buf, sizeof(buf)-3, f))//提取文件的字符存放到buf  
42.             break;  
43.         size_t len = strlen(buf);//len为字符数组的长度  
44.         while (len > 0 && isspace(buf[len - 1]))//int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格(' ')、定位字符(' \t ')、CR(' \r ')、换行(' \n ')  
45.             buf[--len] = '\0';                  //、垂直定位字符(' \v ')或翻页(' \f ')的情况。，既在非空白字符的后面以为添加‘\0’  
46.         if (buf[0] == '#')//开头为'#',结束此次循环  
47.             continue;  
48.         IplImage* img = cvLoadImage(buf, 0);//读取图片  
49.         if (!img)  
50.             break;  
51.         //获取图片尺寸  
52.         *imgSize = cvGetSize(img);  
53.         //提取角点  
54.         result = cvFindChessboardCorners(img, cvSize(board_w, board_h), corners, &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);  
55.   
56.         if (result)  
57.         {  
58.             //Calibration will suffer without subpixel interpolation  
59.             //函数 cvFindCornerSubPix 通过迭代来发现具有亚象素精度的角点位置  
60.             cvFindCornerSubPix(img, corners, count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));  
61.             /*TermCriteria迭代算法的终止准则: 
62.             typedef struct CvTermCriteria0d 
63.             { 
64.             int    type;  CV_TERMCRIT_ITER 和CV_TERMCRIT_EPS二值之一，或者二者的组合 
65.             int    max_iter; 最大迭代次数 
66.             double epsilon; 结果的精确性 
67.             } 
68.             一般表示迭代终止的条件，如果为CV_TERMCRIT_ITER，则用最大迭代次数作为终止条件，如果为CV_TERMCRIT_EPS 则用精度作为迭代条件， 
69.             如果为CV_TERMCRIT_ITER+CV_TERMCRIT_EPS则用最大迭代次数或者精度作为迭代条件，看哪个条件先满足。*/  
70.   
71.             //开始保存角点的其实位置  
72.             step = successes*board_n;  
73.             //将角点从数组corners压入矩阵image_points；以及给对应的object_points赋值  
74.             for (int i = step, j = 0; j < board_n; ++i, ++j)  
75.             {  
76.                 CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;  
77.                 CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;  
78.                 CV_MAT_ELEM(*object_points, float, i, 0) = (j / board_w);  
79.                 CV_MAT_ELEM(*object_points, float, i, 1) = (j % board_w);  
80.                 CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;  
81.             }  
82.             //给对应图片的point_counts赋值  
83.             CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;  
84.             successes++;  
85.         }  
86.         //释放该角点图像  
87.         cvReleaseImage(&img);  
88.     }  
89.     //关闭文件  
90.     fclose(f);  
91.   
92.     //初始化相机内参矩阵  
93.     CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;  
94.     CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;  
95.     //标定相机的内参矩阵和畸变系数向量  
96.     cvCalibrateCamera2(object_points, image_points, point_counts, *imgSize, intrinsic_matrix, distortion_coeffs, NULL, NULL, 0);  
97.   
98. }  

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

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

[cpp]  view plain  copy

1. static void StereoCalib(CvMat* _left_object_points, CvMat* _left_image_points, CvMat* _right_image_points, CvMat* left_intrinsic, CvMat* right_intrinsic,  
2.     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)  
3. {  
4.     int board_n = _board_w*_board_h;//每张图片中角点总数量  
5.   
6.     //初始化Number_perImg  
7.     CvMat* Number_perImg = cvCreateMat(1, _n_boards, CV_32SC1);  
8.     int* pInt;  
9.     pInt = (int*)(Number_perImg->data.ptr);  
10.     for (int i = 0; i < _n_boards; ++i)  
11.     {  
12.         *pInt = board_n;  
13.         pInt++;  
14.     }  
15.     //Show(用于调试)  
16.     /*for (int i = 0; i < _n_boards; i++) 
17.     { 
18.     cout << CV_MAT_ELEM(*Number_perImg, int, 0, i) << endl; 
19.     }*/  
20.   
21.       
22.     //立体标定.计算 _R, _T, _E, _F,，并同时调整Number_perImg, left_intrinsic, D_left,right_intrinsic, D_right  
23.     cvStereoCalibrate(_left_object_points, _left_image_points, _right_image_points, Number_perImg, left_intrinsic, left_distortion,  
24.         right_intrinsic, right_distortion, _imgSize, _R, _T, _E, _F,  
25.         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);  
26. }  

[cpp]  view plain  copy

1.   

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

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

[cpp]  view plain  copy

1. /*计算标定误差函数 
2. CvMat* left_image_points     IN左相机图像坐标系点的矩阵 
3. CvMat* right_image_points    IN右相机图像坐标系点的矩阵 
4. CvMat* left_intrinsic        INandOUT左相机的内参矩阵，经立体标定调整后输出 
5. CvMat* right_intrinsic       INandOUT右相机的内参矩阵，经立体标定调整后输出 
6. CvMat* left_distortion       INandOUT左相机的畸变向量，经立体标定调整后输出 
7. CvMat* right_distortion      INandOUT右相机的畸变向量，经立体标定调整后输出 
8. CvMat* _F                    OUT基础矩阵 
9. int n_boards                 IN图片的数量 
10. int board_w                  IN每张图片x方向角点数量 
11. int board_h                  IN每张图片y方向角点数量 
12. */  
13. double Calib_Quality_Check(CvMat* _left_image_points, CvMat* _right_image_points, CvMat* left_intrinsic, CvMat* right_intrinsic,  
14.     CvMat* left_distortion, CvMat* right_distortion, CvMat* _F, int _n_boards, int _board_w, int _board_h)  
15. {  
16.     int board_n = _board_w*_board_h;//每张图片中角点总数量  
17.       
18.     //定义极线  
19.     CvMat* Lines1 = cvCreateMat(1, _n_boards*board_n, CV_32FC3);  
20.     CvMat* Lines2 = cvCreateMat(1, _n_boards*board_n, CV_32FC3);  
21.     //校正点的畸变  
22.     <span style="font-family: Arial, Helvetica, sans-serif;">cvUndistortPoints</span><span style="font-family: Arial, Helvetica, sans-serif;">(_left_image_points, _left_image_points, left_intrinsic, D_left, 0, left_intrinsic);//代码运行到这里出现中断</span>  
23.     <span style="font-family: Arial, Helvetica, sans-serif;">cvUndistortPoints</span><span style="font-family: Arial, Helvetica, sans-serif;">(_right_image_points, _right_image_points, right_intrinsic, D_right, 0, right_intrinsic);</span>  
24.     //计算极线  
25.     cvComputeCorrespondEpilines(_left_image_points, 1, _F, Lines1);  
26.     cvComputeCorrespondEpilines(_right_image_points, 2, _F, Lines2);  
27.     //计算左相机图像点和极线的距离  
28.     float a, b, c;//极线系数ax+by+c=0  
29.     double err, avgErr = 0;//误差变量  
30.     float* p_temp = (float*)cvPtr2D(_left_image_points, 0, 0);//用于临时计算的点  
31.     float* l_temp = (float*)cvPtr2D(Lines2, 0, 0,0);//用于临时计算的极线  
32.     for (int i = 0; i < _n_boards*board_n; i++)  
33.     {  
34.         //提取点坐标  
35.         x = *p_temp;  
36.         y = *(p_temp + 1);  
37.         p_temp += 2;  
38.         //提取极线系数  
39.         a = *l_temp;  
40.         b = *(l_temp + 1);  
41.         c = *(l_temp + 2);  
42.         l_temp += 3;  
43.         //计算点到直线的距离  
44.         err = fabs(a*x + b*y + c) / sqrt(a*a + b*b);  
45.         //累加误差  
46.         avgErr += err;  
47.     }  
48.     //计算右相机图像点和极线的距离  
49.     p_temp = (float*)cvPtr2D(_right_image_points, 0, 0);//用于临时计算的点  
50.     l_temp = (float*)cvPtr2D(Lines1, 0, 0);//用于临时计算的极线  
51.     for (int i = 0; i < _n_boards*board_n; i++)  
52.     {  
53.         //提取点坐标  
54.         x = *p_temp;  
55.         y = *(p_temp + 1);  
56.         p_temp += 2;  
57.         //提取极线系数  
58.         a = *l_temp;  
59.         b = *(l_temp + 1);  
60.         c = *(l_temp + 2);  
61.         l_temp += 3;  
62.         //计算点到直线的距离  
63.         err = fabs(a*x + b*y + c) / sqrt(a*a + b*b);  
64.         //累加误差  
65.         avgErr += err;  
66.     }  
67.     //求误差的平均值  
68.     avgErr /= (_n_boards*board_n);  
69.     return avgErr;  
70. }  

这时候代码运行到 cvUndistortPoints()    出现中断，查看源码sources中对应的函数可以发现函数

[cpp]  view plain  copy

1. void  
2. cvUndisto(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,  
3. const CvMat* _distCoeffs,  
4. const CvMat* _R, const CvMat* _P)  

中有这么一句话：

[cpp]  view plain  copy

1. CV_ASSERT(CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&  
2.         (_src->rows == 1 || _src->cols == 1) &&  
3.         (_dst->rows == 1 || _dst->cols == 1) &&  
4.         CV_ARE_SIZES_EQ(_src, _dst) &&  
5.         (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&  
6.         (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));  

既输入参数变量 CvMat* _src, CvMat* _dst  必须符合以上的条件，具体代表什么自己查找相关宏的定义，这里不一一列出，我们输入的参数是

[cpp]  view plain  copy

1. CvMat* left_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);//左相机图片坐标中的角点  
2. CvMat* right_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);//右相机图片坐标中的角点  

所以因为最后两句的判断而出错。

[cpp]  view plain  copy

1. (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2))  

所以需要先对参数进行调整，修改计算误差函数为：

[cpp]  view plain  copy

1. /*计算标定误差函数 
2. CvMat* left_image_points     IN左相机图像坐标系点的矩阵 
3. CvMat* right_image_points    IN右相机图像坐标系点的矩阵 
4. CvMat* left_intrinsic        INandOUT左相机的内参矩阵，经立体标定调整后输出 
5. CvMat* right_intrinsic       INandOUT右相机的内参矩阵，经立体标定调整后输出 
6. CvMat* left_distortion       INandOUT左相机的畸变向量，经立体标定调整后输出 
7. CvMat* right_distortion      INandOUT右相机的畸变向量，经立体标定调整后输出 
8. CvMat* _F                    OUT基础矩阵 
9. int n_boards                 IN图片的数量 
10. int board_w                  IN每张图片x方向角点数量 
11. int board_h                  IN每张图片y方向角点数量 
12. */  
13. double Calib_Quality_Check(CvMat* _left_image_points, CvMat* _right_image_points, CvMat* left_intrinsic, CvMat* right_intrinsic,  
14.     CvMat* left_distortion, CvMat* right_distortion, CvMat* _F, int _n_boards, int _board_w, int _board_h)  
15. {  
16.     int board_n = _board_w*_board_h;//每张图片中角点总数量  
17.     //整理LeftPoints。因为_left_image_points数据类型是_n_boards*board_n*3*CV_32FC1，left数据类型是1*_n_boards*board_n*CV_32FC2，对参数进行转换  
18.     CvMat* left = cvCreateMat(1, _n_boards*board_n, CV_32FC2);  
19.     float *p = (float*)cvPtr2D(left, 0, 0);  
20.     float x, y;  
21.     for (int i = 0; i < _n_boards*board_n; ++i)  
22.     {  
23.         x = CV_MAT_ELEM(*_left_image_points, float, i, 0);  
24.         y = CV_MAT_ELEM(*_left_image_points, float, i, 1);  
25.         *p = x;  
26.         p++;  
27.         *p = y;  
28.         p++;  
29.     }  
30.     //整理LeftPoints。因为_left_image_points数据类型是_n_boards*board_n*3*CV_32FC1，left数据类型是1*_n_boards*board_n*CV_32FC2，对参数进行转换  
31.     CvMat* right = cvCreateMat(1, _n_boards*board_n, CV_32FC2);  
32.     p = (float*)cvPtr2D(right, 0, 0);  
33.     for (int i = 0; i < _n_boards*board_n; ++i)  
34.     {  
35.         x = CV_MAT_ELEM(*_right_image_points, float, i, 0);  
36.         y = CV_MAT_ELEM(*_right_image_points, float, i, 1);  
37.         *p = x;  
38.         p++;  
39.         *p = y;  
40.         p++;  
41.     }  
42.     //调整畸变向量。因为left_distortion数据类型是5*1的向量，D_left数据类型是1*5向量，对参数进行转换  
43.     CvMat* D_left = cvCreateMat(1, 5, CV_32FC1);  
44.     for (int i = 0; i < 5; ++i)  
45.     {  
46.         CV_MAT_ELEM(*D_left, float, 0, i) = CV_MAT_ELEM(*left_distortion, float, i, 0);  
47.     }  
48.     //调整畸变向量。因为right_distortion数据类型是5*1的向量，D_right数据类型是1*5向量，对参数进行转换  
49.     CvMat* D_right = cvCreateMat(1, 5, CV_32FC1);  
50.     for (int i = 0; i < 5; ++i)  
51.     {  
52.         CV_MAT_ELEM(*D_right, float, 0, i) = CV_MAT_ELEM(*right_distortion, float, i, 0);  
53.     }  
54.     //定义极线  
55.     CvMat* Lines1 = cvCreateMat(1, _n_boards*board_n, CV_32FC3);  
56.     CvMat* Lines2 = cvCreateMat(1, _n_boards*board_n, CV_32FC3);  
57.     //校正点的畸变  
58.     cvUndistortPoints(_left_image_points, _left_image_points, left_intrinsic, D_left, 0, left_intrinsic);  
59.     cvUndistortPoints(_right_image_points, _right_image_points, right_intrinsic, D_right, 0, right_intrinsic);  
60.     //计算极线  
61.     cvComputeCorrespondEpilines(_left_image_points, 1, _F, Lines1);  
62.     cvComputeCorrespondEpilines(_right_image_points, 2, _F, Lines2);  
63.     //计算左相机图像点和极线的距离  
64.     float a, b, c;//极线系数ax+by+c=0  
65.     double err, avgErr = 0;//误差变量  
66.     float* p_temp = (float*)cvPtr2D(_left_image_points, 0, 0);//用于临时计算的点  
67.     float* l_temp = (float*)cvPtr2D(Lines2, 0, 0,0);//用于临时计算的极线  
68.     for (int i = 0; i < _n_boards*board_n; i++)  
69.     {  
70.         //提取点坐标  
71.         x = *p_temp;  
72.         y = *(p_temp + 1);  
73.         p_temp += 2;  
74.         //提取极线系数  
75.         a = *l_temp;  
76.         b = *(l_temp + 1);  
77.         c = *(l_temp + 2);  
78.         l_temp += 3;  
79.         //计算点到直线的距离  
80.         err = fabs(a*x + b*y + c) / sqrt(a*a + b*b);  
81.         //累加误差  
82.         avgErr += err;  
83.     }  
84.     //计算右相机图像点和极线的距离  
85.     p_temp = (float*)cvPtr2D(_right_image_points, 0, 0);//用于临时计算的点  
86.     l_temp = (float*)cvPtr2D(Lines1, 0, 0);//用于临时计算的极线  
87.     for (int i = 0; i < _n_boards*board_n; i++)  
88.     {  
89.         //提取点坐标  
90.         x = *p_temp;  
91.         y = *(p_temp + 1);  
92.         p_temp += 2;  
93.         //提取极线系数  
94.         a = *l_temp;  
95.         b = *(l_temp + 1);  
96.         c = *(l_temp + 2);  
97.         l_temp += 3;  
98.         //计算点到直线的距离  
99.         err = fabs(a*x + b*y + c) / sqrt(a*a + b*b);  
100.         //累加误差  
101.         avgErr += err;  
102.     }  
103.     //求误差的平均值  
104.     avgErr /= (_n_boards*board_n);  
105.     return avgErr;  
106. }  

修改函数之后计算得到如下的结果（单位为像素）：

4.计算相机校正的左右相机的校正矩阵，使相机两平面完全行对准

当相机两平面完全行对准时，计算立体视差是最简单的，为了使相机两平面完全行对准，OpenCV提供了非标定和标定的方法来计算相机校正的左右相机的校正矩阵。接下来，选择非标定（Hartley)方法cvStereoRectifyUncalibrated()或者标定(Bouguet)方法cvStereoRectify()来计算相机变换。

如果选择非标定方法，在使用

[cpp]  view plain  copy

1. cvInitUndistortRectifyMap( const CvMat* camera_matrix,const CvMat* dist_coeffs,const CvMat *R, const CvMat* new_camera_matrix,CvArr* mapx, CvArr* mapy );  

计算校正映射mapx和mapy之前，需要先预处理一下单应矩阵：令camera_matrix等于前面cvCalibrateCamera2或cvStereoCalibrate计算的相机内参数矩阵，然后利用相机内参矩阵camera_matrix和单应矩阵H计算左右相机对应的旋转矩阵：Rl=inv(camera_matrix_l)Hl(camera_matrix_l)和Rr=inv(camera_matrix_r)Hr(camera_matrix_r)，其中inv表示求矩阵的逆。

而选择标定方法则可以直接从cvStereoRectify()读出cvInitUndistortRectifyMap()的输入参数。

我还是把这个过程封装成一个函数的形式：

[cpp]  view plain  copy

1. /*立体校正的映射矩阵mapx和mapy函数 
2. CvMat* left_image_points     IN左相机图像坐标系点的矩阵 
3. CvMat* right_image_points    IN右相机图像坐标系点的矩阵 
4. CvMat* left_intrinsic        INandOUT左相机的内参矩阵，经立体标定调整后输出 
5. CvMat* right_intrinsic       INandOUT右相机的内参矩阵，经立体标定调整后输出 
6. CvMat* left_distortion       INandOUT左相机的畸变向量，经立体标定调整后输出 
7. CvMat* right_distortion      INandOUT右相机的畸变向量，经立体标定调整后输出 
8. CvMat* R                     OUT相机相对旋转矩阵 
9. CvMat* T                     OUT相机相对平移矩阵 
10. CvMat* F                     OUT基础矩阵 
11. CvSize* imgSize              IN每张图片的像素尺寸 
12. int n_boards                 IN图片的数量 
13. int board_w                  IN每张图片x方向角点数量 
14. int board_h                  IN每张图片y方向角点数量 
15. bool useUncalibrated         IN是否采用非标定算法Hartly计算立体校正的映射矩阵mapx和mapy 
16. CvMat* mxl                   OUT左相机立体校正的映射矩阵mapx 
17. CvMat* myl                   OUT左相机立体校正的映射矩阵mapy 
18. CvMat* mxr                   OUT右相机立体校正的映射矩阵mapx 
19. CvMat* myr                   OUT右相机立体校正的映射矩阵mapy 
20. */  
21. /*mapx和mapy是函数返回的查找映射表，。对目标图像上的每一个像素来说，映射表说明应该从什么位置插值源像素； 
22. 并且映射表可以直接被Remap()嵌入使用。函数cvInitUndistortRectifyMap（）被左右摄像机分别调用，这样可以获得它们的各自不同的重映射参数mapx和mapy， 
23. 每次我们有新的左右立体图像需要校正时可以使用左右映射表*/  
24. static void ComputeRectification(CvMat* imagePoints_left, CvMat* imagePoints_right, CvMat* _left_intrinsic, CvMat* _right_intrinsic, CvMat* _left_distortion, CvMat* _right_distortion,  
25.     CvMat* R, CvMat* T, CvMat* F, CvSize imageSize, int _n_boards, int _board_w, int _board_h, CvMat* mxl, CvMat* myl, CvMat* mxr, CvMat* myr)  
26. {  
27.       
28.     //定义和初始化左右相机的行对准旋转矩阵  
29.     CvMat* Rl = cvCreateMat(3, 3, CV_64FC1);  
30.     cvZero(Rl);  
31.     CvMat* Rr = cvCreateMat(3, 3, CV_64FC1);  
32.     cvZero(Rr);  
33.     //计算相机共面和行对准的校正项  
34.     if (useUncalibrated == false)//useUncalibrated == 0，使用Bouguet方法计算校正项  
35.     {  
36.         //定义和初始化做右相机投影矩阵，该矩阵可以计算校正后的相机内参数矩阵  
37.         CvMat* Pl = cvCreateMat(3, 4, CV_64FC1);  
38.         cvZero(Pl);  
39.         CvMat* Pr = cvCreateMat(3, 4, CV_64FC1);  
40.         cvZero(Pr);  
41.         //计算相机校正项，计算得到 Rl, Rr, Pl, Pr  
42.         cvStereoRectify(<span style="font-family: Arial, Helvetica, sans-serif;">_left_intrinsic</span><span style="font-family: Arial, Helvetica, sans-serif;">, </span><span style="font-family: Arial, Helvetica, sans-serif;">_right_intrinsic</span><span style="font-family: Arial, Helvetica, sans-serif;">, </span><span style="font-family: Arial, Helvetica, sans-serif;">_left_distortion</span><span style="font-family: Arial, Helvetica, sans-serif;">, </span><span style="font-family: Arial, Helvetica, sans-serif;">_right_distortion</span><span style="font-family: Arial, Helvetica, sans-serif;">, imageSize,</span><span style="font-family: Arial, Helvetica, sans-serif;">R, T, Rl, Rr, Pl, Pr, 0, 0, );//运行到此处出现中断</span><span style="font-family: Arial, Helvetica, sans-serif;">  
43. </span>       //Show（用于调试）  
44.         /*cout << endl << "Pr:" << endl; 
45.         for (int i = 0; i < 3; i++) 
46.         { 
47.             const double *p = (const double*)(Pr->data.ptr + i*Pr->step); 
48.                 cout << *p << "    " << *(p + 1) << "       " << *(p + 2)<<"        "<<*(p+3) << endl; 
49.         }*/  
50.         /*通过比较转换矩阵P位置[0][3]和位置[1][3]的大小 判断相机是水平布置还是垂直布置。 
51.         因为这两个位置的元素分别表示右相机坐标系原点移动到左相机坐标系原点的x和y方向移动距离。*/  
52.         isVerticalStereo = fabs(CV_MAT_ELEM(*Pr, double, 1, 3)) > fabs(CV_MAT_ELEM(*Pr, double, 0, 3));  
53.         //Precompute maps for cvRemap()  
54.         //下面通过函数cvInitUndistortRectifyMap()计算立体校正的映射矩阵mx和my  
55.         cvInitUndistortRectifyMap(_left_intrinsic, D_left, Rl, Pl, mxl, myl);  
56.         cvInitUndistortRectifyMap(_right_intrinsic, D_right, Rr, Pr, mxr, myr);  
57.     }  
58.     else//useUncalibrated！ = 0，使用Hartley方法计算校正项  
59.     {  
60.         int board_n = _board_w*_board_h;//每张图片中角点总数量  
61.         CvMat* Hl = cvCreateMat(3, 3, CV_64F);//Hartley方法计算得到的校正项  
62.         CvMat* Hr = cvCreateMat(3, 3, CV_64F);//Hartley方法计算得到的校正项  
63.         CvMat *invM = cvCreateMat(3, 3, CV_64F);//用于临时存放相机内参数的逆  
64.         //整理imagePoints_left和imagePoints_right  
65.         float x, y;//用于传值的临时变量  
66.         float* p;//用于传值的临时变量  
67.         //LeftPoints  
68.         CvMat* left = cvCreateMat(1, _n_boards*board_n, CV_32FC2);  
69.         p = (float*)cvPtr2D(left, 0, 0);  
70.         for (int i = 0; i < _n_boards*board_n; ++i)  
71.         {  
72.             x = CV_MAT_ELEM(*imagePoints_left, float, i, 0);  
73.             y = CV_MAT_ELEM(*imagePoints_left, float, i, 1);  
74.             *p = x;  
75.             p++;  
76.             *p = y;  
77.             p++;  
78.         }  
79.         //Show  
80.         /*p = (float*)cvPtr2D(left, 0, 0); 
81.         for (int i = 0; i < n_boards*board_n; i++) 
82.         { 
83.         cout << *p << "    " << *(p + 1) << endl; 
84.         p += 2; 
85.         }*/  
86.         //RightPoints  
87.         CvMat* right = cvCreateMat(1, _n_boards*board_n, CV_32FC2);  
88.         p = (float*)cvPtr2D(right, 0, 0);  
89.         for (int i = 0; i < _n_boards*board_n; ++i)  
90.         {  
91.             x = CV_MAT_ELEM(*imagePoints_right, float, i, 0);  
92.             y = CV_MAT_ELEM(*imagePoints_right, float, i, 1);  
93.             *p = x;  
94.             p++;  
95.             *p = y;  
96.             p++;  
97.         }  
98.         //Show  
99.         /*p = (float*)cvPtr2D(right, 0, 0); 
100.         for (int i = 0; i < n_boards*board_n; i++) 
101.         { 
102.         cout << *p << "    " << *(p + 1) << endl; 
103.         p += 2; 
104.         }*/  
105.         //通过两个相机的图像点计算基础矩阵F  
106.         cvFindFundamentalMat(left, right, F);  
107.         //使用非标定算法Hartley立体校正，获得做右相机的校正单应性矩阵H1,H2  
108.         cvStereoRectifyUncalibrated(left, right, F, imageSize, Hl, Hr, 3);  
109.         /*使用cvStereoRectifyUncalibrated()的结果H1,H2来校正立体摄像机，必须先预处理一下单应性矩阵Hl和我Hr， 
110.         如果没有先前标定中获得Mrect，令Mrect=M,分别计算左右校正旋转矩阵Rl=inv(Mrect_l)Hl(Ml)和Rr=inv(Mrect_r)Hr(Mr)， 
111.         最后还需要分别计算左右相机的畸变系数向量*/  
112.         //Show（用于调试）  
113.         /*cout << endl << "M1:"; 
114.         for (int i = 0; i < 3; i++) 
115.         { 
116.             cout << endl; 
117.             for (int j = 0; j < 3; j++) 
118.             { 
119.                 cout << CV_MAT_ELEM(*_left_intrinsic, float, i, j) << "     "; 
120.             } 
121.         }*/  
122.         //预处理Rl=inv(Mrect_l)Hl(Ml)  
123.         cvInvert(M_left, invM);  
124.         cvMatMul(Hl, M_left, Rl);  
125.         cvMatMul(invM, Rl, Rl);  
126.         //预处理Rr=inv(Mrect_r)Hr(Mr)  
127.         cvInvert(M_right, invM);//_iM为_M1的逆矩阵  
128.         cvMatMul(Hr, M_right, Rr);//_R1=_H1*_M1  
129.         cvMatMul(invM, Rr, Rr);//_R2=_iM*_R1  
130.         //Precompute map for cvRemap()，用预处理后的_R1和_R2，以及前面计算的相机参数代入cvInitUndistortRectifyMap计算mapx和mapy  
131.         cvInitUndistortRectifyMap(M_left, D_left, Rl, M_left, mxl, myl);  
132.         cvInitUndistortRectifyMap(M_right, D_right, Rr, M_right, mxr, myr);  
133.   
134.     }  
135. }  

当useUncalibrated == false时，函数运行到cvStereoRectify会出现程序中断，再次查看OpenCV中对应该函数的源代码，可以发现里面定义的矩阵变量都是CV_64F，而且接下来进行了罗德里格斯变换：

[cpp]  view plain  copy

1. cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )  

查看了一下该函数的源代码发现里面的一句话会引起中断：

[cpp]  view plain  copy

1. if (!CV_ARE_DEPTHS_EQ(src, dst))  
2.         CV_Error(CV_StsUnmatchedFormats, "All the matrices must have the same data type");  

所以我接下来索性把函数 cvStereoRectify（） 的所有参数变量都转换成CV_64FC1，以及将cvStereoRectifyUncalibrated（）的参数变量也全部改为CV_64FC1,则都可以顺利运行，修改后的自定义函数为：

[cpp]  view plain  copy

1. /*立体校正的映射矩阵mapx和mapy函数 
2. CvMat* left_image_points     IN左相机图像坐标系点的矩阵 
3. CvMat* right_image_points    IN右相机图像坐标系点的矩阵 
4. CvMat* left_intrinsic        INandOUT左相机的内参矩阵，经立体标定调整后输出 
5. CvMat* right_intrinsic       INandOUT右相机的内参矩阵，经立体标定调整后输出 
6. CvMat* left_distortion       INandOUT左相机的畸变向量，经立体标定调整后输出 
7. CvMat* right_distortion      INandOUT右相机的畸变向量，经立体标定调整后输出 
8. CvMat* R                     OUT相机相对旋转矩阵 
9. CvMat* T                     OUT相机相对平移矩阵 
10. CvMat* F                     OUT基础矩阵 
11. CvSize* imgSize              IN每张图片的像素尺寸 
12. int n_boards                 IN图片的数量 
13. int board_w                  IN每张图片x方向角点数量 
14. int board_h                  IN每张图片y方向角点数量 
15. bool useUncalibrated         IN是否采用非标定算法Hartly计算立体校正的映射矩阵mapx和mapy 
16. CvMat* mxl                   OUT左相机立体校正的映射矩阵mapx 
17. CvMat* myl                   OUT左相机立体校正的映射矩阵mapy 
18. CvMat* mxr                   OUT右相机立体校正的映射矩阵mapx 
19. CvMat* myr                   OUT右相机立体校正的映射矩阵mapy 
20. */  
21. /*mapx和mapy是函数返回的查找映射表，。对目标图像上的每一个像素来说，映射表说明应该从什么位置插值源像素； 
22. 并且映射表可以直接被Remap()嵌入使用。函数cvInitUndistortRectifyMap（）被左右摄像机分别调用，这样可以获得它们的各自不同的重映射参数mapx和mapy， 
23. 每次我们有新的左右立体图像需要校正时可以使用左右映射表*/  
24. static void ComputeRectification(CvMat* imagePoints_left, CvMat* imagePoints_right, CvMat* _left_intrinsic, CvMat* _right_intrinsic, CvMat* _left_distortion, CvMat* _right_distortion,  
25.     CvMat* R, CvMat* T, CvMat* F, CvSize imageSize, int _n_boards, int _board_w, int _board_h, CvMat* mxl, CvMat* myl, CvMat* mxr, CvMat* myr)  
26. {  
27.     /*将参数变量重新整理成CV_64F类型的，我也不知道为什么，cvStereoRectify如果传入CV_32FC1的参数会中断，改为CV_64FC1则成功*/  
28.     //整理左相机内参矩阵。因为_left_intrinsic数据类型为CV_32FC1，M_left为CV_64FC1  
29.     CvMat* M_left = cvCreateMat(3, 3, CV_64FC1);  
30.     for (int i = 0; i < 3; i++)  
31.     for (int j = 0; j < 3; j++)  
32.         CV_MAT_ELEM(*M_left, double, i, j) = CV_MAT_ELEM(*_left_intrinsic, float, i, j);  
33.     //整理右相机内参矩阵。因为_left_intrinsic数据类型为CV_32FC1，M_left为CV_64FC1  
34.     CvMat* M_right = cvCreateMat(3, 3, CV_64FC1);  
35.     for (int i = 0; i < 3; i++)  
36.     for (int j = 0; j < 3; j++)  
37.         CV_MAT_ELEM(*M_right, double, i, j) = CV_MAT_ELEM(*_right_intrinsic, float, i, j);  
38.     //调整畸变向量。因为left_distortion数据类型是5*1的向量，D_left数据类型是1*5向量，同时转换为CV_64FC1。  
39.     CvMat* D_left = cvCreateMat(1, 5, CV_64FC1);  
40.     for (int i = 0; i < 5; ++i)  
41.     {  
42.         CV_MAT_ELEM(*D_left, double, 0, i) = CV_MAT_ELEM(*_left_distortion, float, i, 0);  
43.     }  
44.     //调整畸变向量。因为right_distortion数据类型是5*1的向量，D_right数据类型是1*5向量，同时转换为CV_64FC1。  
45.     CvMat* D_right = cvCreateMat(1, 5, CV_64FC1);  
46.     for (int i = 0; i < 5; ++i)  
47.     {  
48.         CV_MAT_ELEM(*D_right, double, 0, i) = CV_MAT_ELEM(*_right_distortion, float, i, 0);  
49.     }  
50.     //转换旋转矩阵。CV_32FC1转换为CV_64FC1  
51.     CvMat* RR = cvCreateMat(3, 3, CV_64FC1);  
52.     for (int i = 0; i < 3; i++)  
53.     for (int j = 0; j < 3; j++)  
54.         CV_MAT_ELEM(*RR, double, i, j) = CV_MAT_ELEM(*R, float, i, j);  
55.     //定义平移向量。CV_32FC1转换为CV_64FC1  
56.     CvMat* TT = cvCreateMat(3, 1, CV_64FC1);  
57.     for (int i = 0; i < 3; ++i)  
58.     {  
59.         CV_MAT_ELEM(*TT, double, i, 0) = CV_MAT_ELEM(*T, float, i, 0);  
60.     }  
61.     //定义和初始化左右相机的行对准旋转矩阵  
62.     CvMat* Rl = cvCreateMat(3, 3, CV_64FC1);  
63.     cvZero(Rl);  
64.     CvMat* Rr = cvCreateMat(3, 3, CV_64FC1);  
65.     cvZero(Rr);  
66.     //计算相机共面和行对准的校正项  
67.     if (useUncalibrated == false)//useUncalibrated == 0，使用Bouguet方法计算校正项  
68.     {  
69.         //定义和初始化做右相机投影矩阵，该矩阵可以计算校正后的相机内参数矩阵  
70.         CvMat* Pl = cvCreateMat(3, 4, CV_64FC1);  
71.         cvZero(Pl);  
72.         CvMat* Pr = cvCreateMat(3, 4, CV_64FC1);  
73.         cvZero(Pr);  
74.         //计算相机校正项，计算得到 Rl, Rr, Pl, Pr  
75.         cvStereoRectify(M_left, M_right, D_left, D_right, imageSize,  
76.             RR, TT, Rl, Rr, Pl, Pr, 0, 0, -1.0, cvSize(0, 0), (CvRect*)0, (CvRect*)0);  
77.         //Show（用于调试）  
78.         /*cout << endl << "Pr:" << endl; 
79.         for (int i = 0; i < 3; i++) 
80.         { 
81.             const double *p = (const double*)(Pr->data.ptr + i*Pr->step); 
82.                 cout << *p << "    " << *(p + 1) << "       " << *(p + 2)<<"        "<<*(p+3) << endl; 
83.         }*/  
84.         /*通过比较转换矩阵P位置[0][3]和位置[1][3]的大小 判断相机是水平布置还是垂直布置。 
85.         因为这两个位置的元素分别表示右相机坐标系原点移动到左相机坐标系原点的x和y方向移动距离。*/  
86.         isVerticalStereo = fabs(CV_MAT_ELEM(*Pr, double, 1, 3)) > fabs(CV_MAT_ELEM(*Pr, double, 0, 3));  
87.         //Precompute maps for cvRemap()  
88.         //下面通过函数cvInitUndistortRectifyMap()计算立体校正的映射矩阵mx和my  
89.         cvInitUndistortRectifyMap(_left_intrinsic, D_left, Rl, Pl, mxl, myl);  
90.         cvInitUndistortRectifyMap(_right_intrinsic, D_right, Rr, Pr, mxr, myr);  
91.     }  
92.     else//useUncalibrated！ = 0，使用Hartley方法计算校正项  
93.     {  
94.         int board_n = _board_w*_board_h;//每张图片中角点总数量  
95.         CvMat* Hl = cvCreateMat(3, 3, CV_64FC1);//Hartley方法计算得到的校正项  
96.         CvMat* Hr = cvCreateMat(3, 3, CV_64FC1);//Hartley方法计算得到的校正项  
97.         CvMat *invM = cvCreateMat(3, 3, CV_64FC1);//用于临时存放相机内参数的逆  
98.         //整理imagePoints_left和imagePoints_right  
99.         float x, y;//用于传值的临时变量  
100.         double* p;//用于传值的临时变量  
101.         //LeftPoints  
102.         CvMat* left = cvCreateMat(1, _n_boards*board_n, CV_64FC2);  
103.         p = (double*)cvPtr2D(left, 0, 0);  
104.         for (int i = 0; i < _n_boards*board_n; ++i)  
105.         {  
106.             x = CV_MAT_ELEM(*imagePoints_left, float, i, 0);  
107.             y = CV_MAT_ELEM(*imagePoints_left, float, i, 1);  
108.             *p = x;  
109.             p++;  
110.             *p = y;  
111.             p++;  
112.         }  
113.         //Show  
114.         /*p = (float*)cvPtr2D(left, 0, 0); 
115.         for (int i = 0; i < n_boards*board_n; i++) 
116.         { 
117.         cout << *p << "    " << *(p + 1) << endl; 
118.         p += 2; 
119.         }*/  
120.         //RightPoints  
121.         CvMat* right = cvCreateMat(1, _n_boards*board_n, CV_64FC2);  
122.         p = (double*)cvPtr2D(right, 0, 0);  
123.         for (int i = 0; i < _n_boards*board_n; ++i)  
124.         {  
125.             x = CV_MAT_ELEM(*imagePoints_right, float, i, 0);  
126.             y = CV_MAT_ELEM(*imagePoints_right, float, i, 1);  
127.             *p = x;  
128.             p++;  
129.             *p = y;  
130.             p++;  
131.         }  
132.         //Show  
133.         /*p = (float*)cvPtr2D(right, 0, 0); 
134.         for (int i = 0; i < n_boards*board_n; i++) 
135.         { 
136.         cout << *p << "    " << *(p + 1) << endl; 
137.         p += 2; 
138.         }*/  
139.         //通过两个相机的图像点计算基础矩阵F  
140.         cvFindFundamentalMat(left, right, F);  
141.         //使用非标定算法Hartley立体校正，获得做右相机的校正单应性矩阵H1,H2  
142.         cvStereoRectifyUncalibrated(left, right, F, imageSize, Hl, Hr, 3);  
143.         /*使用cvStereoRectifyUncalibrated()的结果H1,H2来校正立体摄像机，必须先预处理一下单应性矩阵Hl和我Hr， 
144.         如果没有先前标定中获得Mrect，令Mrect=M,分别计算左右校正旋转矩阵Rl=inv(Mrect_l)Hl(Ml)和Rr=inv(Mrect_r)Hr(Mr)， 
145.         最后还需要分别计算左右相机的畸变系数向量*/  
146.         //Show（用于调试）  
147.         /*cout << endl << "M1:"; 
148.         for (int i = 0; i < 3; i++) 
149.         { 
150.             cout << endl; 
151.             for (int j = 0; j < 3; j++) 
152.             { 
153.                 cout << CV_MAT_ELEM(*_left_intrinsic, float, i, j) << "     "; 
154.             } 
155.         }*/  
156.         //预处理Rl=inv(Mrect_l)Hl(Ml)  
157.         cvInvert(M_left, invM);  
158.         cvMatMul(Hl, M_left, Rl);  
159.         cvMatMul(invM, Rl, Rl);  
160.         //预处理Rr=inv(Mrect_r)Hr(Mr)  
161.         cvInvert(M_right, invM);//_iM为_M1的逆矩阵  
162.         cvMatMul(Hr, M_right, Rr);//_R1=_H1*_M1  
163.         cvMatMul(invM, Rr, Rr);//_R2=_iM*_R1  
164.         //Precompute map for cvRemap()，用预处理后的_R1和_R2，以及前面计算的相机参数代入cvInitUndistortRectifyMap计算mapx和mapy  
165.         cvInitUndistortRectifyMap(M_left, D_left, Rl, M_left, mxl, myl);  
166.         cvInitUndistortRectifyMap(M_right, D_right, Rr, M_right, mxr, myr);  
167.   
168.     }  
169. }  

5.校正图片和计算视差映射矩阵

这一步没碰上什么异常，直接上自定义函数代码。

[cpp]  view plain  copy

1. /*校正图片和计算视察映射的函数 
2. const char* imageList_left       IN保存左相机图片名的txt文件 
3. const char* imageList_right      IN保存右相机图片名的txt文件 
4. CvSize* imgSize                  IN每张图片的像素尺寸 
5. CvMat* mxl                       IN左相机立体校正的映射矩阵mapx 
6. CvMat* myl                       IN左相机立体校正的映射矩阵mapy 
7. CvMat* mxr                       IN右相机立体校正的映射矩阵mapx 
8. CvMat* myr                       IN右相机立体校正的映射矩阵mapy 
9. */  
10. static void Rectify_and_ComputeDisparity(const char* imageList_left, const char* imageList_right, CvSize imSize, CvMat* mxl, CvMat* myl, CvMat* mxr, CvMat* myr)  
11. {  
12.     FILE *f_left, *f_right;//定义两个文件操作对象  
13.     fopen_s(&f_left, imageList_left, "rt");//打开左相机图像的图像名文件  
14.     fopen_s(&f_right, imageList_right, "rt");//打开右相机图像的图像名文件  
15.     IplImage *img_left, *img_right;//定义两个图像对象  
16.     //定义两个矩阵用于存放校正后的图像  
17.     CvMat* img1r = cvCreateMat(imSize.height, imSize.width, CV_8U);  
18.     CvMat* img2r = cvCreateMat(imSize.height, imSize.width, CV_8U);  
19.   
20.     CvMat* disp = cvCreateMat(imSize.height, imSize.width, CV_16S);//视察映射  
21.     CvMat* vdisp = cvCreateMat(imSize.height, imSize.width, CV_8U);//归一化后的视差映射  
22.     /*创建和初始化一个结构体CvStereoBMState用于cvFindStereoCorrespondenceBM()的匹配过程的参数。因为如果每次调用cvFindStereoCorrespondenceBM() 
23.     都离散分配内部离散缓存会影响程序的运行速度。*/  
24.     CvStereoBMState *BMState = cvCreateStereoBMState();  
25.     assert(BMState != 0);//如果创建失败则终止程序  
26.     BMState->preFilterSize = 41;//预过滤窗口的尺寸，使图像亮度归一化并加强纹理  
27.     BMState->preFilterCap = 31;//预过滤的阈值  
28.     BMState->SADWindowSize = 41;//"绝对误差累计"小窗口(SAD)的尺寸  
29.     BMState->minDisparity = -64;//匹配搜索起始点位置  
30.     BMState->numberOfDisparities = 128;//匹配搜索 的视差范围  
31.     BMState->textureThreshold = 10;//后过滤阈值，响应小于该值的匹配不予考虑  
32.     BMState->uniquenessRatio = 15;//后过滤比例阈值，uniquenessRatio>(match_val-min_match)/min_match的地方过滤掉匹配值  
33.   
34.     cvNamedWindow("disparity");//创建一个窗口用于显示视差映射  
35.     cvNamedWindow("rectified", 1);//创建窗口，用于显示校正后的图像  
36.     CvMat* pair;//定义一个矩阵用于存放校正后的两张图片  
37.     CvMat part;//定义一个矩阵用于存放校正后的一张图片  
38.     if (!isVerticalStereo)//相机水平布置则创建尺寸为（imageSize.height, imageSize.width * 2）的图片用于显示左右相机立体校正后的图片  
39.         pair = cvCreateMat(imSize.height, imSize.width * 2, CV_8UC3);  
40.     else//相机竖直布置则创建尺寸为（imageSize.height * 2, imageSize.width）的图片用于显示上下相机立体校正后的图片  
41.         pair = cvCreateMat(imSize.height * 2, imSize.width, CV_8UC3);  
42.   
43.     //循环读取左右相机的图像  
44.     for (int i = 0;; i++)  
45.     {  
46.         //读取图片  
47.         char buf_left[1024], buf_right[1024];//存放读取的字符数组  
48.         if ((!fgets(buf_left, sizeof(buf_left)-3, f_left)) || (!fgets(buf_right, sizeof(buf_right)-3, f_right)))//提取文件的字符存放到buf  
49.             break;  
50.         size_t len = strlen(buf_left);//len为字符数组的长度  
51.         while (len > 0 && isspace(buf_left[len - 1]))//int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格(' ')、定位字符(' \t ')、CR(' \r ')、换行(' \n ')  
52.             buf_left[--len] = '\0';                 //、垂直定位字符(' \v ')或翻页(' \f ')的情况。，既在非空白字符的后面以为添加‘\0’  
53.         len = strlen(buf_right);//len为字符数组的长度  
54.         while (len > 0 && isspace(buf_right[len - 1]))//int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格(' ')、定位字符(' \t ')、CR(' \r ')、换行(' \n ')  
55.             buf_right[--len] = '\0';                    //、垂直定位字符(' \v ')或翻页(' \f ')的情况。，既在非空白字符的后面以为添加‘\0’  
56.         if ((buf_left[0] == '#') || (buf_right[0] == '#'))//开头为'#',结束此次循环  
57.             continue;  
58.         img_left = cvLoadImage(buf_left, 0);//读取图片  
59.         img_right = cvLoadImage(buf_right, 0);//读取图片  
60.         if ((!img_left) || (!img_right))  
61.             break;  
62.         //使用用前面计算的mx和my立体校正图片，两个相机的图片处于同一个相机平面且行对准  
63.         cvRemap(img_left, img1r, mxl, myl);  
64.         cvRemap(img_right, img2r, mxr, myr);  
65.         /*接下来用立体校准后的图片计算双目视觉模型的视察映射。分为相机水平布置和垂直布置两种情况， 
66.         对于摄像机垂直布置的情形，如果没有自己添加图像转置的代码，函数cvFindStereoCorrespondenceBM() 
67.         只能计算非标定校正的视察，对于摄像机水平对齐的情形，函数cvFindStereoCorrespondenceBM()可以计算 
68.         标定和非标定的校正立体像对的视差。*/  
69.         if (!isVerticalStereo || useUncalibrated != 0)  
70.         {  
71.             cvFindStereoCorrespondenceBM(img1r, img2r, disp, BMState);//计算视差映射  
72.             cvNormalize(disp, vdisp, 0, 256, CV_MINMAX);//归一化视差映射  
73.             //显示视差映射  
74.             cvShowImage("disparity", vdisp);  
75.         }  
76.         //下面显示立体校正后的两个相机对应的图片  
77.         if (!isVerticalStereo)//水平布置相机的情形  
78.         {  
79.             //pair的左侧复制左相机立体校正后的照片  
80.             cvGetCols(pair, &part, 0, imSize.width);/*CvMat* cvGetCols( const CvArr* arr, CvMat* submat, int start_col, int end_col ); 
81.                                                        返回数组的一定跨度内的列 
82.                                                        arr-输入数组 
83.                                                        submat-指向结果子数组头指针. 
84.                                                        start_col-跨度的开始列（包括该列）索引下标，该下标以0为基准。 
85.                                                        end_col-跨度的结束列（不包括该列）索引下标，该下标以0为基准。*/  
86.             cvCvtColor(img1r, &part, CV_GRAY2BGR);//GRAY->RGB  
87.             //pair右侧复制右相机立体校正后的照片  
88.             cvGetCols(pair, &part, imSize.width, imSize.width * 2);  
89.             cvCvtColor(img2r, &part, CV_GRAY2BGR);//GRAY->RGB  
90.             //在一定距离间隔处绘制水平直线  
91.             for (int j = 0; j < imSize.height; j += 16)  
92.                 cvLine(pair, cvPoint(0, j), cvPoint(imSize.width * 2, j), CV_RGB(0, 255, 0));  
93.         }  
94.         else//竖直布置相机的情况  
95.         {  
96.             //pair的上侧复制上相机立体校正后的照片  
97.             cvGetRows(pair, &part, 0, imSize.height);  
98.             cvCvtColor(img1r, &part, CV_GRAY2BGR);//GRAY->RGB  
99.             //pair的下侧复制下相机立体校正后的照片  
100.             cvGetRows(pair, &part, imSize.height, imSize.height * 2);  
101.             cvCvtColor(img2r, &part, CV_GRAY2BGR);//GRAY->RGB  
102.             //在一定距离间隔处绘制竖直的直线  
103.             for (int j = 0; j < imSize.width; j += 16)  
104.                 cvLine(pair, cvPoint(j, 0), cvPoint(j, imSize.height * 2), CV_RGB(0, 255, 0));  
105.         }  
106.         //显示pair  
107.         cvShowImage("rectified", pair);  
108.         //等待用户输入。。。  
109.         if (cvWaitKey() == 27)  
110.             break;  
111.     }  
112. }  

输出了校正后的左右相机的图像以及视差映射矩阵，如图所示：

校正的图片

视察映射图

6.程序入口：main()函数

[cpp]  view plain  copy

1. int _tmain(int argc, _TCHAR* argv[])  
2. {  
3.     int n_boards = 13;//标定板图片数量  
4.     int board_w = 8;//x方向角点数目  
5.     int board_h = 6;//y方向角点数目  
6.     int board_n = board_w*board_h;//n_boards张图片中角点总数量  
7.   
8.     //标定左相机  
9.     const char* left_imageList = "left_ImgName.txt";  
10.     CvMat* left_object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);//左相机对应的世界坐标中的角点  
11.     CvMat* left_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);//左相机图片坐标中的角点  
12.     CvMat* left_intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);//左相机内参数矩阵  
13.     CvMat* left_distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);//左相机畸变系数向量  
14.     CvSize* left_imgSize = new CvSize;//图片尺寸  
15.     //标定左相机内参数和畸变向量  
16.     SingleCalib(left_imageList, left_object_points, left_image_points, left_intrinsic_matrix, left_distortion_coeffs,  
17.         n_boards, board_w, board_h, left_imgSize);  
18.     //Show(用于调试)  
19.     /*int cout_step = left_intrinsic_matrix->step / sizeof(float); 
20.     float* data = left_intrinsic_matrix->data.fl; 
21.     for (int i = 0; i < 3; i++) 
22.     { 
23.     cout << "\n"; 
24.     for (int j = 0; j < 3; j++) 
25.     { 
26.     cout << (data + i*cout_step)[j] << "      "; 
27.     } 
28.     }*/  
29.   
30.     //标定右相机  
31.     const char* right_imageList = "right_ImgName.txt";  
32.     CvMat* right_object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);//右相机对应的世界坐标中的角点  
33.     CvMat* right_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);//右相机图片坐标中的角点  
34.     CvMat* right_intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);//右相机内参数矩阵  
35.     CvMat* right_distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);//右相机畸变系数向量  
36.     CvSize* right_imgSize = new CvSize;//图片尺寸  
37.     //标定右相机内参数和畸变向量  
38.     SingleCalib(right_imageList, right_object_points, right_image_points, right_intrinsic_matrix, right_distortion_coeffs,  
39.         n_boards, board_w, board_h, right_imgSize);  
40.     //输出标定的内参数矩阵  
41.     /*cout_step = right_intrinsic_matrix->step / sizeof(float); 
42.     data = right_intrinsic_matrix->data.fl; 
43.     for (int i = 0; i < 3; i++) 
44.     { 
45.     cout << "\n"; 
46.     for (int j = 0; j < 3; j++) 
47.     { 
48.     cout << (data + i*cout_step)[j] << "      "; 
49.     } 
50.     }*/  
51.     //图片尺寸（取左右相机的都一样）  
52.     CvSize imgSize;  
53.     imgSize = *right_imgSize;  
54.     CvMat* R = cvCreateMat(3, 3, CV_32FC1);//旋转矩阵  
55.     CvMat* T = cvCreateMat(3, 1, CV_32FC1);//平移矩阵  
56.     CvMat* E = cvCreateMat(3, 3, CV_32FC1);//本征矩阵  
57.     CvMat* F = cvCreateMat(3, 3, CV_32FC1);//基础矩阵  
58.     /*立体标定，得到R, T, E, F，同时可以校正right_image_points, left_intrinsic_matrix, right_intrinsic_matrix, 
59.     left_distortion_coeffs*/  
60.     StereoCalib(left_object_points, left_image_points, right_image_points, left_intrinsic_matrix, right_intrinsic_matrix,  
61.         left_distortion_coeffs, right_distortion_coeffs, R, T, E, F, n_boards, board_w, board_h, imgSize);  
62.   
63.     //输出立体标定结果  
64.     cout << endl << "M1:";  
65.     for (int i = 0; i < 3; i++)  
66.     {  
67.         cout << endl;  
68.         for (int j = 0; j < 3; j++)  
69.         {  
70.             cout << CV_MAT_ELEM(*left_intrinsic_matrix, float, i, j) << "     ";  
71.         }  
72.     }  
73.     cout << endl << "D1:";  
74.     for (int i = 0; i < 5; i++)  
75.     {  
76.         cout << endl;  
77.         cout << CV_MAT_ELEM(*left_distortion_coeffs, float, i, 0) << "     ";  
78.     }  
79.     cout << endl << "M2:";  
80.     for (int i = 0; i < 3; i++)  
81.     {  
82.         cout << endl;  
83.         for (int j = 0; j < 3; j++)  
84.         {  
85.             cout << CV_MAT_ELEM(*right_intrinsic_matrix, float, i, j) << "     ";  
86.         }  
87.     }  
88.     cout << endl << "D2:";  
89.     for (int i = 0; i < 5; i++)  
90.     {  
91.         cout << endl;  
92.         cout << CV_MAT_ELEM(*right_distortion_coeffs, float, i, 0) << "     ";  
93.     }  
94.     cout << endl << "R:";  
95.     for (int i = 0; i < 3; i++)  
96.     {  
97.         cout << endl;  
98.         for (int j = 0; j < 3; j++)  
99.         {  
100.             cout << CV_MAT_ELEM(*R, float, i, j) << "     ";  
101.         }  
102.     }  
103.     cout << endl << "T:";  
104.     for (int i = 0; i < 3; i++)  
105.     {  
106.         cout << endl;  
107.         cout << CV_MAT_ELEM(*T, float, i, 0) << "     ";  
108.     }  
109.     cout << endl << "E:";  
110.     for (int i = 0; i < 3; i++)  
111.     {  
112.         cout << endl;  
113.         for (int j = 0; j < 3; j++)  
114.         {  
115.             cout << CV_MAT_ELEM(*E, float, i, j) << "     ";  
116.         }  
117.     }  
118.     cout << endl << "F:";  
119.     for (int i = 0; i < 3; i++)  
120.     {  
121.         cout << endl;  
122.         for (int j = 0; j < 3; j++)  
123.         {  
124.             cout << CV_MAT_ELEM(*F, float, i, j) << "     ";  
125.         }  
126.     }  
127.   
128.     //计算标定结果的误差  
129.     double Error = 0;  
130.     Error = Calib_Quality_Check(left_image_points, right_image_points, left_intrinsic_matrix, right_intrinsic_matrix,  
131.         left_distortion_coeffs, right_distortion_coeffs, F, n_boards, board_w, board_h);  
132.     //输出标定误差  
133.     cout << endl << "Error:" << Error << endl;  
134.   
135.     //保存标定结果  
136.     CvFileStorage* fs = cvOpenFileStorage("Result.xml", 0, CV_STORAGE_WRITE);  
137.     cvWrite(fs, "left_intrinsic_matrix", left_intrinsic_matrix, cvAttrList(0, 0));  
138.     cvWrite(fs, "left_distortion", right_distortion_coeffs, cvAttrList(0, 0));  
139.     cvWrite(fs, "right_intrinsic_matrix", right_intrinsic_matrix, cvAttrList(0, 0));  
140.     cvWrite(fs, "right_distortion", right_distortion_coeffs, cvAttrList(0, 0));  
141.     cvWrite(fs, "R", R, cvAttrList(0, 0));  
142.     cvWrite(fs, "T", T, cvAttrList(0, 0));  
143.     cvWrite(fs, "E", E, cvAttrList(0, 0));  
144.     cvWrite(fs, "F", F, cvAttrList(0, 0));  
145.     cvWriteReal(fs, "Error", Error);  
146.     cvReleaseFileStorage(&fs);  
147.   
148.     //计算立体校正的映射矩阵mapx和mapy  
149.     CvMat* mxl = cvCreateMat(imgSize.height, imgSize.width, CV_32F);  
150.     CvMat* myl = cvCreateMat(imgSize.height, imgSize.width, CV_32F);  
151.     CvMat* mxr = cvCreateMat(imgSize.height, imgSize.width, CV_32F);  
152.     CvMat* myr = cvCreateMat(imgSize.height, imgSize.width, CV_32F);  
153.     ComputeRectification(left_image_points, right_image_points, left_intrinsic_matrix, right_intrinsic_matrix, left_distortion_coeffs, right_distortion_coeffs,  
154.         R, T, F, imgSize, n_boards, board_w, board_h, mxl, myl, mxr, myr);  
155.   
156.     //校正图片和计算视察映射并显示  
157.     Rectify_and_ComputeDisparity(left_imageList, right_imageList, imgSize, mxl, myl, mxr, myr);  
158.   
159.   
160.     return 0;  
161. }