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

1、基于OpenCV立体视觉标定和校正这几天学习双目视觉标定，分别使用了两种工具：OpenCV和Matlab。Matlab的效果非常稳定，但是一开始OpenCV的效果很糟糕，要不是出现中断就是标定出来的结果数值很大。经过了几天的不断调试和更改，终于把OpenCV的立体视觉标定和校正的程序写出来了。立体标定时计算空间上的两台摄像机几何关系的过程，立体校正则是对个体图像进行纠正，保证这些图像可以从平面对准的两幅图像获得。程序的框架如下：1.读取左右相机图片序列双目相机的图片序列放在Demon的路径下，左右相机的图像的名字分别存放在两个txt文件中，程序分别通过这两个txt文件读取对应的图片序列。主注意

2、：我们假设已经将摄像机排列好了，其图像扫描线是粗略物理对齐，从而使得每台摄像机本质上都具有相同的视场。2.提取图片角点，并分别标定左右相机内参矩阵和畸变向量调用cvFindChessboardCorners找出图像中的角点，然后调用cvFindCornerSubPix计算亚像素精度角点位置，将全部找出来的角点位置压入一个矩阵序列中，以及初始化角点在世界坐标系的对应位置序列，本程序的世界坐标系长度单位取标定板放个边长。然后用cvCalibrateCamera2分别标定做右相机的内参矩阵和畸变系数向量。将该过程封装成一个函数，具体过程请参考程序注释：cpp view plain copy/*单个相

3、机标定函数： 输入参数： 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 vo

4、id 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 bo

5、ard_sz = cvSize(board_w, board_h);/角点尺寸矩阵 CvPoint2D32f* corners = new CvPoint2D32fboard_w*board_h;/定义用于存放每张图片角点数量的一维点数组 CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);/向量，每个元素代表每张图片角点的数量 int successes = 0;/找到全部角点的图片数量 int step = 0;/用于记录每张图片角点的起始位置 /文件读取不成功: if (!f) fprintf(stderr, "ca

6、n not open file %sn", imageList);/要读写, 得知道从哪里读, 往哪里写吧?stderr - 标准错误输出设备 return; /利用i循环读取文件中的字符，然后用于读取图片 for (int i = 0; i+) /读取图片 char buf1024;/存放读取的字符数组 int count = 0, result = 0;/count找的的角点数量，result找角点结果标志，全部角点找到非零，否者为0； if (!fgets(buf, sizeof(buf)-3, f)/提取文件的字符存放到buf break; size_t len = strl

7、en(buf);/len为字符数组的长度 while (len > 0 && isspace(buflen - 1)/int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格(' ')、定位字符(' t ')、CR(' r ')、换行(' n ') buf-len = '0' /、垂直定位字符(' v ')或翻页(' f ')的情况。，既在非空白字符的后面以为添加0 if (buf0 = '#')/开头为'#&#

8、39;,结束此次循环 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 suff

9、er 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_TE

10、RMCRIT_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

11、赋值 for (int i = step, j = 0; j < board_n; +i, +j) CV_MAT_ELEM(*image_points, float, i, 0) = cornersj.x; CV_MAT_ELEM(*image_points, float, i, 1) = cornersj.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_poi

12、nts, 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; /标定相机的内参矩阵和畸变系数向

13、量 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步计算的做右相机内参矩阵和畸变系数向量。但是

14、处于个人习惯还是喜欢将该过程封装到一个函数里:cpp view plain copystatic 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_boar

15、ds, 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(用于调试) /

16、*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, le

17、ft_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); cpp view plain copy 计算得到如下的结果（到目前为止还是很顺利的嘛！）：4.上述标定结果和Matla

18、b的标定结果对比差别不大。下面通过另一种方法检查标定结果的误差，通过检查图像上的点与另一幅图像的极线的距离远近来评价标定精度。首先，通过cvUndistortPoints()对原始点进行去畸变处理，然后使用cvComputeCorrespondEplilines()来计算极线，然后计算这些点和极线的距离（理想情况下，距离为0），累计这些距离的绝对误差，然后求其平均值。具体过程我也把它封装成一个函数：cpp view plain copy/*计算标定误差函数 CvMat* left_image_points IN左相机图像坐标系点的矩阵 CvMat* right_image_points IN右

19、相机图像坐标系点的矩阵 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

20、方向角点数量 */ double Calib_Quality_Check(CvMat* _left_image_points, CvMat* _right_image_points, CvMat* left_intrinsic, CvMat* right_intrinsic, CvMat* left_distortion, CvMat* right_distortion, CvMat* _F, int _n_boards, int _board_w, int _board_h) int board_n = _board_w*_board_h;/每张图片中角点总数量 /定义极线 CvMat* L

21、ines1 = cvCreateMat(1, _n_boards*board_n, CV_32FC3); CvMat* Lines2 = cvCreateMat(1, _n_boards*board_n, CV_32FC3); /校正点的畸变 <span style="font-family: Arial, Helvetica, sans-serif;">cvUndistortPoints</span><span style="font-family: Arial, Helvetica, sans-serif;">(_

22、left_image_points, _left_image_points, left_intrinsic, D_left, 0, left_intrinsic);/代码运行到这里出现中断</span> <span style="font-family: Arial, Helvetica, sans-serif;">cvUndistortPoints</span><span style="font-family: Arial, Helvetica, sans-serif;">(_right_image_po

23、ints, _right_image_points, right_intrinsic, D_right, 0, right_intrinsic);</span> /计算极线 cvComputeCorrespondEpilines(_left_image_points, 1, _F, Lines1); cvComputeCorrespondEpilines(_right_image_points, 2, _F, Lines2); /计算左相机图像点和极线的距离 float a, b, c;/极线系数ax+by+c=0 double err, avgErr = 0;/误差变量 floa

24、t* p_temp = (float*)cvPtr2D(_left_image_points, 0, 0);/用于临时计算的点 float* l_temp = (float*)cvPtr2D(Lines2, 0, 0,0);/用于临时计算的极线 for (int i = 0; i < _n_boards*board_n; i+) /提取点坐标 x = *p_temp; y = *(p_temp + 1); p_temp += 2; /提取极线系数 a = *l_temp; b = *(l_temp + 1); c = *(l_temp + 2); l_temp += 3; /计算点到直线

25、的距离 err = fabs(a*x + b*y + c) / sqrt(a*a + b*b); /累加误差 avgErr += err; /计算右相机图像点和极线的距离 p_temp = (float*)cvPtr2D(_right_image_points, 0, 0);/用于临时计算的点 l_temp = (float*)cvPtr2D(Lines1, 0, 0);/用于临时计算的极线 for (int i = 0; i < _n_boards*board_n; i+) /提取点坐标 x = *p_temp; y = *(p_temp + 1); p_temp += 2; /提取极

26、线系数 a = *l_temp; b = *(l_temp + 1); c = *(l_temp + 2); l_temp += 3; /计算点到直线的距离 err = fabs(a*x + b*y + c) / sqrt(a*a + b*b); /累加误差 avgErr += err; /求误差的平均值 avgErr /= (_n_boards*board_n); return avgErr; 这时候代码运行到cvUndistortPoints() 出现中断，查看源码sources中对应的函数可以发现函数cpp view plain copyvoid cvUndisto(const CvMa

27、t* _src, CvMat* _dst, const CvMat* _cameraMatrix, const CvMat* _distCoeffs, const CvMat* _R, const CvMat* _P) 中有这么一句话：cpp view plain copyCV_ASSERT(CV_IS_MAT(_src) && CV_IS_MAT(_dst) && (_src->rows = 1 | _src->cols = 1) && (_dst->rows = 1 | _dst->cols = 1) &&am

28、p; CV_ARE_SIZES_EQ(_src, _dst) && (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); 既输入参数变量CvMat* _src, CvMat* _dst 必须符合以上的条件，具体代表什么自己查找相关宏的定义，这里不一一列出，我们输入的参数是cpp view pl

29、ain copyCvMat* left_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);/左相机图片坐标中的角点 CvMat* right_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);/右相机图片坐标中的角点 所以因为最后两句的判断而出错。cpp view plain copy(CV_MAT_TYPE(_src->type) = CV_32FC2 | CV_MAT_TYPE(_src->type) = CV_64FC2) &&(

30、CV_MAT_TYPE(_dst->type) = CV_32FC2 | CV_MAT_TYPE(_dst->type) = CV_64FC2) 所以需要先对参数进行调整，修改计算误差函数为：cpp view plain copy/*计算标定误差函数 CvMat* left_image_points IN左相机图像坐标系点的矩阵 CvMat* right_image_points IN右相机图像坐标系点的矩阵 CvMat* left_intrinsic INandOUT左相机的内参矩阵，经立体标定调整后输出 CvMat* right_intrinsic INandOUT右相机的内参

31、矩阵，经立体标定调整后输出 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_points, CvMat* _right_image_points, CvMat*

32、left_intrinsic, CvMat* right_intrinsic, CvMat* left_distortion, CvMat* right_distortion, CvMat* _F, int _n_boards, int _board_w, int _board_h) int board_n = _board_w*_board_h;/每张图片中角点总数量 /整理LeftPoints。因为_left_image_points数据类型是_n_boards*board_n*3*CV_32FC1，left数据类型是1*_n_boards*board_n*CV_32FC2，对参数进行转换

33、 CvMat* left = cvCreateMat(1, _n_boards*board_n, CV_32FC2); float *p = (float*)cvPtr2D(left, 0, 0); float x, y; for (int i = 0; i < _n_boards*board_n; +i) x = CV_MAT_ELEM(*_left_image_points, float, i, 0); y = CV_MAT_ELEM(*_left_image_points, float, i, 1); *p = x; p+; *p = y; p+; /整理LeftPoints。因为

34、_left_image_points数据类型是_n_boards*board_n*3*CV_32FC1，left数据类型是1*_n_boards*board_n*CV_32FC2，对参数进行转换 CvMat* right = cvCreateMat(1, _n_boards*board_n, CV_32FC2); p = (float*)cvPtr2D(right, 0, 0); for (int i = 0; i < _n_boards*board_n; +i) x = CV_MAT_ELEM(*_right_image_points, float, i, 0); y = CV_MAT

35、_ELEM(*_right_image_points, float, i, 1); *p = x; p+; *p = y; p+; /调整畸变向量。因为left_distortion数据类型是5*1的向量，D_left数据类型是1*5向量，对参数进行转换 CvMat* D_left = cvCreateMat(1, 5, CV_32FC1); for (int i = 0; i < 5; +i) CV_MAT_ELEM(*D_left, float, 0, i) = CV_MAT_ELEM(*left_distortion, float, i, 0); /调整畸变向量。因为right_d

36、istortion数据类型是5*1的向量，D_right数据类型是1*5向量，对参数进行转换 CvMat* D_right = cvCreateMat(1, 5, CV_32FC1); for (int i = 0; i < 5; +i) CV_MAT_ELEM(*D_right, float, 0, i) = CV_MAT_ELEM(*right_distortion, float, i, 0); /定义极线 CvMat* Lines1 = cvCreateMat(1, _n_boards*board_n, CV_32FC3); CvMat* Lines2 = cvCreateMat(

37、1, _n_boards*board_n, CV_32FC3); /校正点的畸变 cvUndistortPoints(_left_image_points, _left_image_points, left_intrinsic, D_left, 0, left_intrinsic); cvUndistortPoints(_right_image_points, _right_image_points, right_intrinsic, D_right, 0, right_intrinsic); /计算极线 cvComputeCorrespondEpilines(_left_image_poin

38、ts, 1, _F, Lines1); cvComputeCorrespondEpilines(_right_image_points, 2, _F, Lines2); /计算左相机图像点和极线的距离 float a, b, c;/极线系数ax+by+c=0 double err, avgErr = 0;/误差变量 float* p_temp = (float*)cvPtr2D(_left_image_points, 0, 0);/用于临时计算的点 float* l_temp = (float*)cvPtr2D(Lines2, 0, 0,0);/用于临时计算的极线 for (int i = 0

39、; i < _n_boards*board_n; i+) /提取点坐标 x = *p_temp; y = *(p_temp + 1); p_temp += 2; /提取极线系数 a = *l_temp; b = *(l_temp + 1); c = *(l_temp + 2); l_temp += 3; /计算点到直线的距离 err = fabs(a*x + b*y + c) / sqrt(a*a + b*b); /累加误差 avgErr += err; /计算右相机图像点和极线的距离 p_temp = (float*)cvPtr2D(_right_image_points, 0, 0)

40、;/用于临时计算的点 l_temp = (float*)cvPtr2D(Lines1, 0, 0);/用于临时计算的极线 for (int i = 0; i < _n_boards*board_n; i+) /提取点坐标 x = *p_temp; y = *(p_temp + 1); p_temp += 2; /提取极线系数 a = *l_temp; b = *(l_temp + 1); c = *(l_temp + 2); l_temp += 3; /计算点到直线的距离 err = fabs(a*x + b*y + c) / sqrt(a*a + b*b); /累加误差 avgErr

41、+= err; /求误差的平均值 avgErr /= (_n_boards*board_n); return avgErr; 修改函数之后计算得到如下的结果（单位为像素）：4.计算相机校正的左右相机的校正矩阵，使相机两平面完全行对准当相机两平面完全行对准时，计算立体视差是最简单的，为了使相机两平面完全行对准，OpenCV提供了非标定和标定的方法来计算相机校正的左右相机的校正矩阵。接下来，选择非标定（Hartley)方法cvStereoRectifyUncalibrated()或者标定(Bouguet)方法cvStereoRectify()来计算相机变换。如果选择非标定方法，在使用cpp vie

42、w plain copycvInitUndistortRectifyMap( 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计算左右相机对应的

43、旋转矩阵：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/*立体校正的映射矩阵mapx和mapy函数 CvMat* left_image_points IN左相机图像坐标系点的矩阵 CvMat* right_image_points IN右相

44、机图像坐标系点的矩阵 CvMat* left_intrinsic INandOUT左相机的内参矩阵，经立体标定调整后输出 CvMat* right_intrinsic INandOUT右相机的内参矩阵，经立体标定调整后输出 CvMat* left_distortion INandOUT左相机的畸变向量，经立体标定调整后输出 CvMat* right_distortion INandOUT右相机的畸变向量，经立体标定调整后输出 CvMat* R OUT相机相对旋转矩阵 CvMat* T OUT相机相对平移矩阵 CvMat* F OUT基础矩阵 CvSize* imgSize IN每张图片的像素尺

45、寸 int n_boards IN图片的数量 int board_w IN每张图片x方向角点数量 int board_h IN每张图片y方向角点数量 bool useUncalibrated IN是否采用非标定算法Hartly计算立体校正的映射矩阵mapx和mapy CvMat* mxl OUT左相机立体校正的映射矩阵mapx CvMat* myl OUT左相机立体校正的映射矩阵mapy CvMat* mxr OUT右相机立体校正的映射矩阵mapx CvMat* myr OUT右相机立体校正的映射矩阵mapy */ /*mapx和mapy是函数返回的查找映射表，。对目标图像上的每一个像素来说，

46、映射表说明应该从什么位置插值源像素； 并且映射表可以直接被Remap()嵌入使用。函数cvInitUndistortRectifyMap（）被左右摄像机分别调用，这样可以获得它们的各自不同的重映射参数mapx和mapy， 每次我们有新的左右立体图像需要校正时可以使用左右映射表*/ static void ComputeRectification(CvMat* imagePoints_left, CvMat* imagePoints_right, CvMat* _left_intrinsic, CvMat* _right_intrinsic, CvMat* _left_distortion, C

47、vMat* _right_distortion, 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) /定义和初始化左右相机的行对准旋转矩阵 CvMat* Rl = cvCreateMat(3, 3, CV_64FC1); cvZero(Rl); CvMat* Rr = cvCreateMat(3, 3, CV_64FC1); cvZero(Rr); /计算相机共面和行对

48、准的校正项 if (useUncalibrated = false)/useUncalibrated = 0，使用Bouguet方法计算校正项 /定义和初始化做右相机投影矩阵，该矩阵可以计算校正后的相机内参数矩阵 CvMat* Pl = cvCreateMat(3, 4, CV_64FC1); cvZero(Pl); CvMat* Pr = cvCreateMat(3, 4, CV_64FC1); cvZero(Pr); /计算相机校正项，计算得到 Rl, Rr, Pl, Pr cvStereoRectify(<span style="font-family: Arial, H

49、elvetica, 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;">