三维重建学习(5)：简单地从数学原理层面理解双目立体视觉

前言

这是我前段时间学习双目视觉时做的笔记，这一篇文章不会进行过于细致的推导，仅仅会在一些理想情况下做一些简单的推导，目标是得到一个直观上的认识：双目视觉是如何得到三维立体坐标的。以后的博客还会再放上一些详细的推导，和对双目视觉提取景深的程序实现。嘛，先一步一步来吧。
后面默认都知道摄像机的针孔模型以及相机坐标系、世界坐标系、图像坐标系等等这些概念，如果不清楚请查看之前的博客：三维重建学习(2)：相机标定基础

先考虑一个很理想的情况

假设整个相机坐标系的原点为左相机坐标系原点。
两个摄像机同时观察某一点 P P ，假设P P 的坐标是 (xc,yc,zc) ( x c , y c , z c ) （注：坐标是在相机坐标系下），左右两个摄像机的成像平面上都会有点 P P 的投影（也就是相机中心与点P P 的交点，图中已标出），假设坐标分别是： Pl=(xl,yl) P l = ( x l , y l ) 、 Pr=(xr,yr) P r = ( x r , y r ) 。
为了方便推导，我们考虑一个比较理想的情况，假设左右图像所在的平面相互平行，即两摄像机平面平行，并且两个相机的光轴也位于同一水平面上。这个条件可以说是十分理想化了，而在立体视觉中我们要通过立体标定和立体校正才能得到这种情况下的两张图片，这里暂且不讨论，后面的博客再谈这些，现在只需要考虑这个理想的情况即可。
注意到两个相机平面平行，并且光轴也位于同一水平面，发现： yl=yr=y y l = y r = y ，由相似三角形原理很容易得出。即图中平面上投影点的y坐标相同。
左右坐标系中心的距离，即基线距为 B B ；焦距为f f 。
接着可以由几何关系导出下式，也是由相似三角形性质得到的：

⎧⎩⎨⎪⎪⎪⎪⎪⎪xl=xc⋅fzcxr=(xc−B)⋅fzcy=yc⋅fzc { x l = x c ⋅ f z c x r = ( x c − B ) ⋅ f z c y = y c ⋅ f z c

接下来要引出 视差(Disparity)的概念，设视差 D=xl−xr D = x l − x r ，即左坐标系中投影点的坐标减去右坐标系中对应投影点的坐标。由视差的公式： D=xl−xr D = x l − x r ，和上面的几个式子联立可以解出 xc,yc,zc x c , y c , z c 来。

⎧⎩⎨⎪⎪⎪⎪xc=B⋅xlDyc=B⋅yDzc=B⋅fD { x c = B ⋅ x l D y c = B ⋅ y D z c = B ⋅ f D

在这种理想情况下，B 可以通过测量两个相机的基线距离得到， 可 以 通 过 测 量 两 个 相 机 的 基 线 距 离 得 到 ， f$是焦距可以标定参数或是测量得到。根据两幅图像中的点的视差与坐标，可以很轻松地计算出三维坐标。

考虑一个更一般的情况

在实际中，我们的摄像机不可能完全平行，所以如上图所示，假设在左右相机的坐标系互相不平行。
假设左相机坐标系 o−xyz o − x y z 为世界坐标系；左图像坐标系为 o−xlyl o − x l y l ，左相机的焦距为 fl f l ；右图像坐标系为 o−xryr o − x r y r ，右相机的焦距为 fr f r 。
根据相机投射模型有：
左相机坐标系中的坐标 (x,y,z) ( x , y , z ) 到左相机图像坐标系中的坐标 xl,yl x l , y l ：

sl⎡⎣⎢xlyl1⎤⎦⎥=⎡⎣⎢fl000fl0001⎤⎦⎥⎡⎣⎢xyz⎤⎦⎥ s l [ x l y l 1 ] = [ f l 0 0 0 f l 0 0 0 1 ] [ x y z ]

右相机坐标系中的坐标 (x′,y′,z′) ( x ′ , y ′ , z ′ ) 到右相机图像坐标系中的坐标 xr,yr x r , y r ：

sr⎡⎣⎢xryr1⎤⎦⎥=⎡⎣⎢fr000fr0001⎤⎦⎥⎡⎣⎢x′y′z′⎤⎦⎥ s r [ x r y r 1 ] = [ f r 0 0 0 f r 0 0 0 1 ] [ x ′ y ′ z ′ ]

左相机坐标系与右相机坐标系之间的位置关系可以通过空间转换矩阵 Mlr=[Rt] M l r = [ R t ] ， R R 和t t 分别是两个相机坐标系之间的旋转矩阵和平移矩阵，可以通过立体相机标定得到，这里假设已知这两个东西。
两坐标系的坐标系我们可以用公式表示出来了：

⎡⎣⎢x′y′z′⎤⎦⎥=Mlr⎡⎣⎢⎢⎢xyz1⎤⎦⎥⎥⎥=⎡⎣⎢r1r4r7r2r5r8r3r6r9txtytz⎤⎦⎥⎡⎣⎢⎢⎢xyz1⎤⎦⎥⎥⎥ [ x ′ y ′ z ′ ] = M l r [ x y z 1 ] = [ r 1 r 2 r 3 t x r 4 r 5 r 6 t y r 7 r 8 r 9 t z ] [ x y z 1 ]

其中的 r1 r 1 到 r9 r 9 以及 tx,ty,tz t x , t y , t z 都已知。
接着把上式套入右相机模型中：

sr⎡⎣⎢xryr1⎤⎦⎥=⎡⎣⎢fr000fr0001⎤⎦⎥⎡⎣⎢r1r4r7r2r5r8r3r6r9txtytz⎤⎦⎥⎡⎣⎢⎢⎢xyz1⎤⎦⎥⎥⎥ s r [ x r y r 1 ] = [ f r 0 0 0 f r 0 0 0 1 ] [ r 1 r 2 r 3 t x r 4 r 5 r 6 t y r 7 r 8 r 9 t z ] [ x y z 1 ]

上面这一“坨”东西先放一放。
由左相机模型可以导出 (x,y,z) ( x , y , z ) 与 (xl,yl) ( x l , y l ) 的关系：

sl⎡⎣⎢xlyl1⎤⎦⎥=⎡⎣⎢fl000fl0001⎤⎦⎥⎡⎣⎢xyz⎤⎦⎥⇒⎧⎩⎨slxl=flxslyl=flysl=z⇒{x=zfl⋅xly=zfl⋅yl s l [ x l y l 1 ] = [ f l 0 0 0 f l 0 0 0 1 ] [ x y z ] ⇒ { s l x l = f l x s l y l = f l y s l = z ⇒ { x = z f l ⋅ x l y = z f l ⋅ y l

把 {x=zfl⋅xly=zfl⋅yl { x = z f l ⋅ x l y = z f l ⋅ y l 套入前面的式子中：

sr⎡⎣⎢xryr1⎤⎦⎥=⎡⎣⎢fr000fr0001⎤⎦⎥⎡⎣⎢r1r4r7r2r5r8r3r6r9txtytz⎤⎦⎥⎡⎣⎢⎢⎢⎢⎢zfl⋅xlzfl⋅ylz1⎤⎦⎥⎥⎥⎥⎥ s r [ x r y r 1 ] = [ f r 0 0 0 f r 0 0 0 1 ] [ r 1 r 2 r 3 t x r 4 r 5 r 6 t y r 7 r 8 r 9 t z ] [ z f l ⋅ x l z f l ⋅ y l z 1 ]

由上面的式子可得：

srxr=frr1zflxl+frr2zflyl+frr3z+frtx(1) (1) s r x r = f r r 1 z f l x l + f r r 2 z f l y l + f r r 3 z + f r t x

slyl=frr4zflxl+frr5zflyl+frr6z+frty(2) (2) s l y l = f r r 4 z f l x l + f r r 5 z f l y l + f r r 6 z + f r t y

sl=r7zflxl+r8zflyl+r9z+tz(3) (3) s l = r 7 z f l x l + r 8 z f l y l + r 9 z + t z

联立方程求解：

⎧⎩⎨⎪⎪式子(1)式子(3)式子(2)式子(3) { 式 子 ( 1 ) 式 子 ( 3 ) 式 子 ( 2 ) 式 子 ( 3 )

这里偷懒就不写步骤了，直接贴结果了：
解得：

⎧⎩⎨⎪⎪⎪⎪⎪⎪x=zflxly=zflylz=fl(frtx−xrtz)xr(r7xl+r8yl+flr9)−fr(rlxl+r2yl+flr3) { x = z f l x l y = z f l y l z = f l ( f r t x − x r t z ) x r ( r 7 x l + r 8 y l + f l r 9 ) − f r ( r l x l + r 2 y l + f l r 3 )

或（两者等价）

⎧⎩⎨⎪⎪⎪⎪⎪⎪x=zflxly=zflylz=fl(frty−yrtz)yr(r7xl+r8yl+flr9)−fr(r4xl+r5yl+flr6) { x = z f l x l y = z f l y l z = f l ( f r t y − y r t z ) y r ( r 7 x l + r 8 y l + f l r 9 ) − f r ( r 4 x l + r 5 y l + f l r 6 )

这东西还是比较长的，算出这个并没有太大的实际意义，仅仅是在数学原理层面上推导一下，实际中我们还是更倾向于使用程序实现，每次都要自己算这个还真的挺难受的。关键在于，我们通过这个结果知道了： 只要我们通过相机标定计数获得了左右相机的内外参数、焦距 fr,fl f r , f l ，和空间点在左右相机坐标系中的图像坐标系，就可以重构三维坐标。

后记

下次再把剩下的笔记整理一下，前段时间也实现了双目景深提取的程序，后面也会整理到博客上。