双摄像头测距的opencv实现
转自http://blog.sina.com.cn/s/blog_51e9ac4f01011l0l.html
虽然最近注意力已经不可遏制地被神经科学、大脑记忆机制和各种毕业活动吸引过去了，但是还是觉得有必要把这段时间双目视觉方面的进展总结一下。毕竟从上一篇博文发表之后，很多同仁发E-mail来与我讨论，很多原来的疑团，也在讨论和一步步的试验中逐渐解决了。
开篇之前，首先要感谢maxwellsdemon和wobject，没有和你们的讨论，也就没有此篇的成文。
说到双摄像头测距，首先要复习一下测距原理，把LearningOpenCV翻到416和418页，可以看到下面两幅图
图1.双摄像头模型俯视图
图2,双摄像头模型立体视图
图1解释了双摄像头测距的原理，书中Z的公式如下：
在OpenCV中，f的量纲是像素点，Tx的量纲由定标棋盘格的实际尺寸和用户输入值确定，一般总是设成毫米，当然为了精度提高也可以设置为0.1毫米量级，d=xl-xr的量纲也是像素点。因此分子分母约去，z的量纲与Tx相同
图2解释了双摄像头获取空间中某点三维坐标的原理。
可以看到，实际的坐标计算利用的都是相似三角形的原理，其表达式就如同Q矩阵所示。
空间中某点的三维坐标就是(X/W, Y/W,Z/W)。
因此，为了精确地求得某个点在三维空间里的距离，我们需要获得的参数有焦距f、视差d、摄像头中心距Tx。
如果还需要获得X坐标和Y坐标的话，那么还需要额外知道左右像平面的坐标系与立体坐标系中原点的偏移cx和cy。其中f, Tx,cx和cy可以通过立体标定获得初始值，并通过立体校准优化，使得两个摄像头在数学上完全平行放置，并且左右摄像头的cx,cy和f相同(也就是实现图2中左右视图完全平行对准的理想形式)。而立体匹配所做的工作，就是在之前的基础上，求取最后一个变量：视差d(这个d一般需要达到亚像素精度)。从而最终完成求一个点三维坐标所需要的准备工作。
在清楚了上述原理之后，我们也就知道了，所有的这几步：标定、校准和匹配，都是围绕着如何更精确地获得f, d, Tx,cx和cy而设计的。
双目测距的原理就说到这里，为了避免大家看到大段纯叙述性的文字头晕，下面的行文将会以FAQ的形式围绕着实现双摄像头测距过程中碰到的几点疑惑展开。当然，其中的解答也只是我的个人理解，如有不当，敬请指正。
Q1：标定时棋盘格的大小如何设定，对最后结果有没有影响？
A：当然有。在标定时，需要指定一个棋盘方格的长度，这个长度(一般以毫米为单位，如果需要更精确可以设为0.1毫米量级)与实际长度相同，标定得出的结果才能用于实际距离测量。一般如果尺寸设定准确的话，通过立体标定得出的Translation的向量的第一个分量Tx的绝对值就是左右摄像头的中心距。一般可以用这个来验证立体标定的准确度。比如我设定的棋盘格大小为270(27mm)，最终得出的Tx大小就是602.8 (60.28mm)，相当精确。
Q2：通过立体标定得出的Tx符号为什么是负的？
A：这个其实我也不是很清楚。个人的解释是，立体标定得出的T向量指向是从右摄像头指向左摄像头(也就是Tx为负)，而在OpenCV坐标系中，坐标的原点是在左摄像头的。因此，用作校准的时候，要把这个向量的三个分量符号都要换一下，最后求出的距离才会是正的。
但是这里还有一个问题，就是LearningOpenCV中Q的表达式，第四行第三列元素是-1/Tx，而在具体实践中，求出来的实际值是1/Tx。这里我和maxwellsdemon讨论下来的结果是，估计书上Q表达式里的这个负号就是为了抵消T向量的反方向所设的，但在实际写OpenCV代码的过程中，那位朋友却没有把这个负号加进去。(一家之言，求更详细的解释)
Q3：cvFindStereoCorrespondenceBM的输出结果好像不是以像素点为单位的视差？
A：在OpenCV2.0中，BM函数得出的结果是以16位符号数的形式的存储的，出于精度需要，所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下：
dptr[y*dstep] =(short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d :0) + 15) >> 4);
可以看到，原始视差在左移8位(256)并且加上一个修正值之后又右移了4位，最终的结果就是左移4位
因此，在实际求距离时，cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息
Q4：利用双摄像头进行测距的时候世界坐标的原点究竟在哪里？
A：世界坐标系的原点是左摄像头凸透镜的光心。
说起这个，就不得不提到针孔模型。如图3所示，针孔模型是凸透镜成像的一种简化模型。当物距足够远时(远大于两倍焦距)，凸透镜成像可以看作是在焦距处的小孔成像。(ref:http://bak1.beareyes.com.cn/2/lib/200110/04/20011004006.htm)
图3.针孔模型
在实际计算过程中，为了计算方便，我们将像平面翻转平移到针孔前，从而得到一种数学上更为简单的等价形式（方便相似三角形的计算），如图4所示。
图4.针孔模型的数学等价形式
因此，对应图2就可以知道，世界坐标系原点就是左摄像头针孔模型的针孔，也就是左摄像头凸透镜的光心
Q5：f和d的单位是像素，那这个像素到底表示什么，它与毫米之间又是怎样换算的？
A：这个问题也与针孔模型相关。在针孔模型中，光线穿过针孔（也就是凸透镜中心）在焦距处上成像，因此，图3的像平面就是摄像头的CCD传感器的表面。每个CCD传感器都有一定的尺寸，也有一定的分辨率，这个就确定了毫米与像素点之间的转换关系。举个例子，CCD的尺寸是8mmX 6mm，分辨率是640X480，那么毫米与像素点之间的转换关系就是80pixel/mm。
在实际运用中，我们在数学上将这个像平面等效到小孔前（图4），这样就相当于将在透镜中心点之前假设了一块虚拟的CCD传感器。
Q6：为什么cvStereoRectify求出的Q矩阵cx, cy, f都与原来的不同？
A：这个在前文有提到过。在实际测量中，由于摄像头摆放的关系，左右摄像头的f, cx,cy都是不相同的。而为了使左右视图达到完全平行对准的理想形式从而达到数学上运算的方便，立体校准所做的工作事实上就是在左右像重合区域最大的情况下，让两个摄像头光轴的前向平行，并且让左右摄像头的f, cx,cy相同。因此，Q矩阵中的值与两个instrinsic矩阵的值不一样就可以理解了。
实验结果：
实验下来，虽然Block Matching算法本身对精度有所限制，但测距基本能达到能让人接受的精度，结果如下图5所示
图5.OpenCV双摄像头测距结果
上图中，中、左、右三个物体分别被放在离摄像头50cm,75cm和90cm的位置。可以看出测距的结果相当不错。当然，上面这幅图是比较好的结果。由于BM算法的限制，同一点云中相同距离的点一般会有正负2厘米之内的误差。
图6是利用双目摄像头测物体长宽的结果，可以看出结果似乎不太准确。。。
图6.OpenCV双摄像头测边长结果
其中，物体宽为117-88=29mm，但实际宽度为5.2cm，物体高位71-13=58mm，但实际高度为13cm。这方面的误差还是比较难以理解
此外，还有一个问题至今尚未完全理解，就是双目摄像头的中心距，为什么采用Tx而不是T向量的长度。因为如果要左右视图重合区域最大化的话两个摄像头的光轴都要与T垂直才是(如图7)，这样的话，校正后两个摄像头的中心距应该是T才对。不知道我这样的理解对不对？
图7.双摄像头立体校准俯视图