基于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 board_sz = cvSize(

5、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, can not open file %sn, i

6、mageList);/要读写, 得知道从哪里读, 往哪里写吧?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 = strlen(buf);/len为字符数组的长度 while (

7、len 0 & isspace(buflen - 1)/int isspace(int c)检查参数c是否为空格字符，也就是判断是否为空格( )、定位字符( t )、CR( r )、换行( n ) buf-len = 0; /、垂直定位字符( v )或翻页( f )的情况。，既在非空白字符的后面以为添加0 if (buf0 = #)/开头为#,结束此次循环 continue; IplImage* img = cvLoadImage(buf, 0);/读取图片 if (!img) break; /获取图片尺寸 *imgSize = cvGetSize(img); /提取角点 result = c

8、vFindChessboardCorners(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), cv

9、Size(-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 则用

10、精度作为迭代条件， 如果为CV_TERMCRIT_ITER+CV_TERMCRIT_EPS则用最大迭代次数或者精度作为迭代条件，看哪个条件先满足。*/ /开始保存角点的其实位置 step = successes*board_n; /将角点从数组corners压入矩阵image_points；以及给对应的object_points赋值 for (int i = step, j = 0; j data.ptr); for (int i = 0; i _n_boards; +i) *pInt = board_n; pInt+; /Show(用于调试) /*for (int i = 0; i _n_b

11、oards; 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

12、, 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.上述标定结果和Matlab的标定结果对比差别不大。下面通过另一种方法检查标定结果的误差，通过检查图像上的点与另一幅图

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

14、内参矩阵，经立体标定调整后输出 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* _l

15、eft_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* Lines1 = cvCreateMat(1, _n_boards*board_n, CV_3

16、2FC3); CvMat* Lines2 = cvCreateMat(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); /计算

17、极线 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;/误差变量 float* p_temp = (float*)cvPtr2D(_left_image_points, 0, 0);/用于临时计算的点 float* l_temp = (float*)cv

18、Ptr2D(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; /计算点到直线的距离 err = fabs(a*x + b*y + c) / sqrt(a*a + b*b); /累加误差 avgErr += err; /计算右相机图像点和极线的距离 p_temp =

19、(float*)cvPtr2D(_right_image_points, 0, 0);/用于临时计算的点 l_temp = (float*)cvPtr2D(Lines1, 0, 0);/用于临时计算的极线 for (int i = 0; i rows = 1 | _src-cols = 1) & (_dst-rows = 1 | _dst-cols = 1) & CV_ARE_SIZES_EQ(_src, _dst) & (CV_MAT_TYPE(_src-type) = CV_32FC2 | CV_MAT_TYPE(_src-type) = CV_64FC2) & (CV_MAT_TYPE(

20、_dst-type) = CV_32FC2 | CV_MAT_TYPE(_dst-type) = CV_64FC2); 既输入参数变量CvMat* _src, CvMat* _dst 必须符合以上的条件，具体代表什么自己查找相关宏的定义，这里不一一列出，我们输入的参数是cpp view plain copyCvMat* left_image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);/左相机图片坐标中的角点 CvMat* right_image_points = cvCreateMat(n_boards*board_n, 2, CV

21、_32FC1);/右相机图片坐标中的角点 所以因为最后两句的判断而出错。cpp view plain copy(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/*计算标定误差函数 CvMat* left_image_points IN左相机图像坐标系点的矩阵 CvMat*

22、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方向角点

23、数量 int board_h IN每张图片y方向角点数量 */ 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;/每张图

24、片中角点总数量 /整理LeftPoints。因为_left_image_points数据类型是_n_boards*board_n*3*CV_32FC1，left数据类型是1*_n_boards*board_n*CV_32FC2，对参数进行转换 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_ima

25、ge_points, float, i, 0); y = CV_MAT_ELEM(*_left_image_points, float, i, 1); *p = x; p+; *p = y; p+; /整理LeftPoints。因为_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,

26、 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_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;

27、i 5; +i) CV_MAT_ELEM(*D_left, float, 0, i) = CV_MAT_ELEM(*left_distortion, float, i, 0); /调整畸变向量。因为right_distortion数据类型是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

28、, 0); /定义极线 CvMat* Lines1 = cvCreateMat(1, _n_boards*board_n, CV_32FC3); CvMat* Lines2 = cvCreateMat(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_poin

29、ts, right_intrinsic, D_right, 0, right_intrinsic); /计算极线 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;/误差变量 float* p_temp = (float*)cvPtr2D(_left_im

30、age_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; /计算点到直线的距离 err = fabs(a*x + b*y + c) / sqrt(a*a

31、+ 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; /提取极线系数 a = *l_temp; b = *(l_temp + 1); c = *(l_te

32、mp + 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; 修改函数之后计算得到如下的结果（单位为像素）：4.计算相机校正的左右相机的校正矩阵，使相机两平面完全行对准当相机两平面完全行对准时，计算立体视差是最简单的，为了使相机两平面完全行对准，OpenCV提供了非标定和标定的方法来计算相机校正的左右相机的校正矩阵。接下来，选择非标定（Hartley)

33、方法cvStereoRectifyUncalibrated()或者标定(Bouguet)方法cvStereoRectify()来计算相机变换。如果选择非标定方法，在使用cpp view 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_matr

34、ix等于前面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/*

35、立体校正的映射矩阵mapx和mapy函数 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右相机的畸变向量，经立体标定

36、调整后输出 CvMat* R OUT相机相对旋转矩阵 CvMat* T OUT相机相对平移矩阵 CvMat* F OUT基础矩阵 CvSize* imgSize IN每张图片的像素尺寸 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 OU

37、T右相机立体校正的映射矩阵mapx CvMat* myr OUT右相机立体校正的映射矩阵mapy */ /*mapx和mapy是函数返回的查找映射表，。对目标图像上的每一个像素来说，映射表说明应该从什么位置插值源像素； 并且映射表可以直接被Remap()嵌入使用。函数cvInitUndistortRectifyMap（）被左右摄像机分别调用，这样可以获得它们的各自不同的重映射参数mapx和mapy， 每次我们有新的左右立体图像需要校正时可以使用左右映射表*/ static void ComputeRectification(CvMat* imagePoints_left, CvMat* ima

38、gePoints_right, CvMat* _left_intrinsic, CvMat* _right_intrinsic, CvMat* _left_distortion, CvMat* _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); /计算相机共面和行对准的校正项 if (useUncalibrated = false)/useUncalibrated = 0，