基于OpenCV的双目测距系统实现
The BinocularMeasure System Based on OpenCV
Abstract:This passage mainly describes how to measure distanceby two camera,which bases on OpenCV library.
Key words: OpenCV; Measure Distance
摘 要：本文主要描述的是利用Intel 的开源计算机视觉库OpenCV实现双目测距系统。
关键词： OpenCV；双目测距
首先需要两个一样的摄像头，再使用其采集各大约20张左右摄像头的棋盘图，注意每张图片都要在不同的角度，不同的距离（相对摄像头而言）进行采集。然后进行双目摄像头标定，获得两个摄像头的内外参数，矫正摄像头获得的目标图像，最后进行立体匹配，通过鼠标点击获取该点的三维坐标（坐标原点是左摄像头的光心）。
首先我们需要标定板，标定板可以通过打印棋盘图再粘到平整的板上。注意棋盘角点数量尽可能多，里我使用的是7 X 9个角点，每个方格的尺寸是2.50cmX 2.50cm。在OpenCV中调用findChessboardCorners()函数找到棋盘角点信息，再使用drawChessboardCorners()函数画出角点，调用cornerSubPix()函数得到棋盘角点的亚像素坐标，最后使用stereoCalibrate()函数进行双目标定，得出两个摄像头的内外参数。
部分代码：
doublerms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
imageSize,R, T, E, F,
TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
CV_CALIB_SAME_FOCAL_LENGTH +
CV_CALIB_RATIONAL_MODEL +
CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
cout << "done with RMS error="<< rms << endl;
这里我使用的是立体校正Bouguet算法，其目的是能简单地使左右图像中的每幅重投影次数最小且重投影畸变最大，使左右图像观测面积最大，从而使立体匹配更加准确和快速。OpenCV通过stereoRectify()函数完成校正功能，最后调用函数initUndistortRectifyMap()生成图像矫正所需要的映射矩阵。
部分代码：
stereoRectify(M1,D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1,img_size, &roi1, &roi2);
Mat map11,map12, map21, map22;
initUndistortRectifyMap(M1,D1, R1, P1, img_size, CV_16SC2, map11, map12);
initUndistortRectifyMap(M2,D2, R2, P2, img_size, CV_16SC2, map21, map22);
在进行立体矫正后，立体匹配能在同一极线上检测左右摄像机图像的相同特征，并得出视差图，其中视差值是在匹配时具有相同特征点在x坐标轴的差值，利用该值可通过三角相似定理得出目标距离。在这里，我使用的是post-filtering过滤方法的stereobm算法生成视差图，该过滤方法能校准视差图的边缘与源图像和从高到低的置信区间半遮挡得到视差值。相对于普通的stereobm算法，从图中可以很清楚地看出增加post-filtering过滤方法的视差图非常的平滑，完整。
部分代码：
Ptr<StereoBM>left_matcher = StereoBM::create(max_disp,wsize);
wls_filter= createDisparityWLSFilter(left_matcher);
Ptr<StereoMatcher>right_matcher = createRightMatcher(left_matcher);
cvtColor(left_for_matcher, left_for_matcher, COLOR_BGR2GRAY);
cvtColor(right_for_matcher,right_for_matcher, COLOR_BGR2GRAY);
left_matcher->compute(left_for_matcher, right_for_matcher,left_disp);
right_matcher->compute(right_for_matcher,left_for_matcher,right_disp);
我们要获得物体的三维信息，只需要调用reprojectImageTo3D()函数，我们要输入利用post-filtering过滤方法的stereobm算法得出的视差图（Mat 类数据），以及上述标定得出的外参数Q，最后我们可以得出物体的三维坐标（保存的数据为Mat类）。最后我们要将得出Y轴坐标进行翻转。
部分代码：
reprojectImageTo3D(disparity,xyz, Q, true);
for(int y = 0; y < xyz.rows; ++y)
{
for (int x = 0; x < xyz.cols; ++x)
{
cv::Point3f point = xyz.at<cv::Point3f>(y,x);
point.y = -point.y;
xyz.at<cv::Point3f>(y, x) = point;
}
}
最后我们通过鼠标的点击来获取我们想要得到某个点的三维信息。
经过多次测量发现，在离摄像头20cm-30cm处，误差基本在0.5mm内。采取的操作方法是摄像头固定，按规律移动物体，测出三位坐标，发现得出原点坐标不是摄像头的光心处，但这并不影响实际的测量结果。摄像头不变，坐标原点固定。
该测量系统只在短范围内测量，实验还存在误差，接下来要做的是不断的改进程序和关注OpenCV_Contrib发布的最新算法。还有的是用Qt+OpenCV编写GUI程序，跟六轴机械臂结合，机械臂能通过摄像头识别和测量出物体的空间坐标，最终实现抓取过程。
参考文献：
[1] 基于OpenCV的双目测距系统_王浩.caj
[2] http://docs.opencv.org/3.1.0/d6/d55/tutorial_table_of_content_calib3d.html#gsc.tab=0
[3] http://docs.opencv.org/3.1.0/d3/d14/tutorial_ximgproc_disparity_filtering.html#gsc.tab=0
