双目测距 单目标点测距工程实践

一、双目测距原理

双目测距的原理可以简单归结为相似三角形测距法，如下图所示：

 根据上面的公式，要获得距离Z，需要首先得到焦距f，两相机中心距B以及视差d。f和B可通过相机标定获得，而后面要做的大量的工作就是为了获得视差d。关于原理想要详细了解的童鞋建议去读 学习OpenCV3 中文版.pdf （下载链接在下方）中的第18、19章，里面对相机标定、三维视觉的内容作了详细讲解，此处就不再赘述。

链接：https://pan.baidu.com/s/1rUgr1ISDBbPbqepvN-BSCA 
提取码：0419 
--来自百度网盘超级会员V1的分享

二、基本流程

双目测距的基本流程一般包含以下几步：双目标定 --> 立体校正（含消除畸变） --> 立体匹配 --> 视差计算 --> 深度计算/3D坐标计算。

双目标定一般有两种方法，一是利用opencv自带的cvStereoCalibrate函数进行标定，另一种是利用matlab的工具箱进行标定。之前查阅的资料帖子中大都提到opencv的标定函数效果不好，建议使用matlab工具箱。因此笔者采用的是matlab工具箱进行相机标定，标定方法参考：https://blog.csdn.net/scyscyao/article/details/5443341。顺便提一句，笔者所用的双目摄像头是采用两个同型号的usb摄像头按照间距200mm进行固定的，就是下图这样：

标定后得到的结果如下，其中未显示出的符号是±，代表误差范围。

其中fc的两个元素为fx、fy，cc的两个元素为cx、cy，kc为畸变参数， om为两个相机之间的旋转向量，T为偏移向量。

立体校正过程需要用到opencv的stereoRectify函数，函数声明如下：

 其中前7个参数，即cameraMatrix1、distCoeffs1、...一直到T，都是需要输入的参数，上一步的标定结果就是这些参数，后缀1、2分别代表左右相机。cameraMatrix代表相机的投影矩阵，元素表示如下：

 distCoeffs代表畸变参数向量，也就是上一步计算结果中的kc。imageSize表示图片的大小，R为旋转矩阵，与上一步结果中的om略有不同，R是3X3矩阵的形式，因此om需要通过opencv的Rodrigues函数转化为R才能使用。T即上一步结果中的T。

R1、R2、P1、P2、Q作为输出参数下一步有用。其余参数可取默认值，其中alpha需要特别说明一下，当取值为0时，立体校正后的图片会自动裁剪为没有黑边的样子。

stereoRectify函数之后需要使用initUndistortRectifyMap函数来分别计算左右视图的校正矩阵。然后使用remap函数对左右视图的原始视图进行校正，到此立体校正完成。立体校正部分的参考代码如下：

Mat M1;
Mat M2;
Mat D1;
Mat D2;
Mat T;
Mat r;
Mat R;
Mat Q;
Mat R1,R2,P1,P2,map11,map12,map21,map22;

M1 = (Mat_<double>(3,3) << 890.96521, 0, 304.31243, 0, 885.63038, 250.13429, 0, 0, 1);
M2 = (Mat_<double>(3,3) << 888.61618, 0, 297.59885, 0, 883.89103, 281.06078, 0, 0, 1);
D1 = (Mat_<double>(1,5) << -0.38131,   0.19090,   0.00086,   -0.00182,  0.00000 );
D2 = (Mat_<double>(1,5) <<  -0.30419,   -0.30816,   -0.00090,   0.00003,  0.00000 );
T = (Mat_<double>(3,1) << -187.85942,   1.21140,  1.23769);
r = (Mat_<double>(3,1) << 0.00295,   -0.00286,  -0.00878);
R = Mat::eye(3,3,CV_64F);
Rodrigues(r,R);
stereoRectify(M1,D1,M2,D2,Size(640,480),R,T,R1,R2,P1,P2,Q,CALIB_ZERO_DISPARITY,0);
initUndistortRectifyMap(M1,D1,R1,P1,Size(640,480),CV_16SC2,map11,map12);
initUndistortRectifyMap(M2,D2,R2,P2,Size(640,480),CV_16SC2,map21,map22);


Mat frame;
L_capture->read(frame);
remap(frame,frame,map11,map12,cv::INTER_LINEAR);

Mat frame1;
R_capture->read(frame1);
remap(frame1,frame1,map21,map22,cv::INTER_LINEAR);

立体校正完成后需要做左右视图的立体匹配，立体匹配的作用是找出空间中某一点在左右视图上的不同位置，从而获取视差图。立体匹配的过程通常需要对整个视图进行匹配，常见的有BM、SGBM、GC等方法，一般计算量比较大，追求准确度的话就需要牺牲计算效率。

三、单目标点测距工程实践

笔者的项目仅需要对视场中感兴趣的目标进行测距，没必要耗时去进行整个视图的匹配。因此，初步考虑采用手动的方式在左右视图上点击同一目标点从而获得视差D（两个像素点的x坐标求差），然后根据公式计算距离。

说明：公式中的f为相机焦距，量纲为毫米，实际中除非拆开相机进行专门测量，否则无法获取其实际值。而我们在前期标定中获得的fx、fy在学习OpenCV3 中文版.pdf中解释如下：

 可以看到fx的量纲为像素，并且存在一个毫米与像素之间的转换系数sx。公式中的d为两个成像点的距离差，量纲也为毫米，但是我们在视图上获得的视差单位实际上是像素。显然存在以下转换关系：

原公式中的B为两相机光心的距离，也即标定后的T向量中的第一个元素Tx的绝对值。因此原计算公式可以替换为：

上式中所有值均为已知，则可以求出距离。通过测试，10米范围内的测量距离误差可维持在5%范围内，20米时误差最大可达到10%，由于摄像头焦距较短，远距离时分辨率已经很低，因此手动选点误差很大。 

 四、改进

纯手动的方式操作较为繁琐，且误差不易控制，因此考虑采用某种算法能够实现单目标点的匹配。经过查阅资料，选择归一化互相关（NCC）算法来进行单点匹配，算法参考：https://www.cnblogs.com/yepeichu/p/7354083.html。此时的操作方式改为在左视图手动点击需要测距的目标点，然后点击“匹配”按钮，通过算法找出右视图的匹配点，然后点击“计算”按钮进行距离计算。经过测试，大部分情况下可实现完美匹配，但是对于缺乏纹理、或重复纹理、镜面反射等等传统问题依旧无能为力。

在此基础上进一步改进，左视图中心位置进行十字线标记，测距时仅需将十字线中心对准目标点，距离进行实时计算显示，笔者将这种方式定义为自动模式，区别于前面的手动和半手动模式。

五、测试demo

界面如下，兼具了手动、半手动和自动模式：

 上图中用卷尺测得实际距离为1380mm，在距离不是特别远和目标点特征较明显的情况下，自动匹配的精度还是很高的。此处由于是自制的双目摄像头，两者的焦距很难手动调到一致，上图中右视图有些糊，可能是在移动过程中改变了焦距，由于已经标定过了，所以不敢轻易再去调焦，否则又需要重新标定。若需要测远距离，建议使用长焦距的双目摄像头。