双目立体视觉三维重建

欢迎访问 持续更新：https://cgabc.xyz/posts/aa4d53ac/

双目立体视觉的整体流程包括：

双目相机采集 左右目图像

通过 双目标定工具 对双目相机进行标定，得到如下结果参数：

《Learning OpenCV》中对于 Translation 和 Rotation 的图示是这样的:

示例代码：

双目矫正 主要包括两方面：畸变矫正 和 立体矫正 。

利用 OpenCV的函数，主要分为

根据双目标定的结果 K 1 , K 2 , D 1 , D 2 , R , t K_1, K_2, D_1, D_2, R, t K1​,K2​,D1​,D2​,R,t，利用 OpenCV函数 stereoRectify，计算得到如下参数

其中，

左右目投影矩阵（horizontal stereo, c x 1 ′ = c x 2 ′ {c_x}_1'={c_x}_2' cx​1′​=cx​2′​ if CV_CALIB_ZERO_DISPARITY is set）

P 1 = [ f ′ 0 c x 1 ′ 0 0 f ′ c y ′ 0 0 0 1 0 ] P_1 = \begin{bmatrix} f' & 0 & {c_x}_1' & 0 \\ 0 & f' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} P1​=⎣ ⎡​f′00​0f′0​cx​1′​cy′​1​000​⎦ ⎤​

P 2 = [ f ′ 0 c x 2 ′ t x ′ ⋅ f ′ 0 f ′ c y ′ 0 0 0 1 0 ] P_2 = \begin{bmatrix} f' & 0 & {c_x}_2' & t_x' \cdot f' \\ 0 & f' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} P2​=⎣ ⎡​f′00​0f′0​cx​2′​cy′​1​tx′​⋅f′00​⎦ ⎤​

where

t x ′ = − B t_x' = -B tx′​=−B

disparity-to-depth 映射矩阵

Q = [ 1 0 0 − c x 1 ′ 0 1 0 − c y ′ 0 0 0 f ′ 0 0 − 1 t x ′ c x 1 ′ − c x 2 ′ t x ′ ] Q = \begin{bmatrix} 1 & 0 & 0 & -{c_x}_1' \\ 0 & 1 & 0 & -c_y' \\ 0 & 0 & 0 & f' \\ 0 & 0 & -\frac{1}{t_x'} & \frac{ {c_x}_1'-{c_x}_2'}{t_x'} \end{bmatrix} Q=⎣ ⎡​1000​0100​000−tx′​1​​−cx​1′​−cy′​f′tx′​cx​1′​−cx​2′​​​⎦ ⎤​

通过 P 2 P_2 P2​ 可计算出 基线 长度:

baseline = B = − t x ′ = − P 2 ( 03 ) f ′ \begin{aligned} \text{baseline} = B = - t_x' = - \frac{ {P_2}_{(03)} }{f'} \end{aligned} baseline=B=−tx′​=−f′P2​(03)​​​

示例代码：

参考：sensor_msgs/CameraInfo Message

D: distortion parameters.

K: Intrinsic camera matrix for the raw (distorted) images.

R: Rectification matrix (stereo cameras only).

P: Projection/camera matrix.

[ u v w ] = P ⋅ [ X Y Z 1 ] , { x = u w y = v w \begin{bmatrix} u \\ v \\ w \end{bmatrix} = \mathbf{P} \cdot \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} , \quad \begin{cases} x = \frac{u}{w} \\ y = \frac{v}{w} \end{cases} ⎣ ⎡​uvw​⎦ ⎤​=P⋅⎣ ⎡​XYZ1​⎦ ⎤​,{x=wu​y=wv​​

左右目 分别利用 OpenCV函数 initUndistortRectifyMap 计算 the undistortion and rectification transformation map，得到

示例代码：

左右目 分别利用 OpenCV函数 remap 并根据 左右目map 对左右目图像进行 去畸变 和 立体矫正，得到 左右目矫正图像

示例代码：

根据双目矫正图像，通过 BM或SGM等立体匹配算法 对其进行立体匹配，计算 视差图

通过 OpenCV函数 stereoBM (block matching algorithm)，生成 视差图(Disparity Map) (CV_16S or CV_32F)

disparity map from stereoBM of OpenCV : It has the same size as the input images. When disptype == CV_16S, the map is a 16-bit signed single-channel image, containing disparity values scaled by 16. To get the true disparity values from such fixed-point representation, you will need to divide each disp element by 16. If disptype == CV_32F, the disparity map will already contain the real disparity values on output.

So if you’ve chosen disptype = CV_16S during computation, you can access a pixel at pixel-position (X,Y) by: short pixVal = disparity.at(Y,X);, while the disparity value is float disparity = pixVal / 16.0f;; if you’ve chosen disptype = CV_32F during computation, you can access the disparity directly: float disparity = disparity.at(Y,X);

（1）算法1：根据视差图，利用 f ′ f' f′ 和 B B B 通过几何关系计算 深度值，并利用相机内参计算 三维坐标

根据上图相似三角形关系，得

Z B = Z − f B − d w ⟹ Z = B f d w \frac{Z}{B} = \frac{Z-f}{B-d_w} \quad \Longrightarrow \quad Z = \frac{Bf}{d_w} BZ​=B−dw​Z−f​⟹Z=dw​Bf​

其中， f f f 和 d w d_w dw​ 分别为 成像平面的焦距和视差，单位均为 物理单位，将其转换为 像素单位，上式写为

Z = B f ′ d p Z = \frac{B f'}{d_p} Z=dp​Bf′​

其中，

d p = ( O r − u r ) + ( u l − O l ) = ( u l − u r ) + ( O r − O l ) d_p = (O_r - u_r) + (u_l - O_l) = (u_l - u_r) + (O_r - O_l) dp​=(Or​−ur​)+(ul​−Ol​)=(ul​−ur​)+(Or​−Ol​)

最终，深度计算公式如下，通过遍历图像可生成 深度图

Z = depth = B ⋅ f ′ d p with d p = disp ( u , v ) + ( c x 2 ′ − c x 1 ′ ) Z = \text{depth} = \frac{B \cdot f'}{d_p} \quad \text{with} \quad d_p = \text{disp}(u,v) + ({c_x}_2' - {c_x}_1') Z=depth=dp​B⋅f′​withdp​=disp(u,v)+(cx​2′​−cx​1′​)

根据 小孔成像模型，已知 Z Z Z 和 相机内参 可计算出 三维点坐标，从而可生成 三维点云

{ Z = depth = f ′ ⋅ B d p X = u − c x 1 ′ f ′ ⋅ Z Y = v − c y ′ f ′ ⋅ Z 或 { bd = B d p Z = depth = f ′ ⋅ bd X = ( u − c x 1 ′ ) ⋅ bd Y = ( u − c y ′ ) ⋅ bd \begin{aligned} \begin{cases} Z = \text{depth} = \frac{f' \cdot B}{d_p} \\ X = \frac{u-{c_x}_1'}{f'} \cdot Z \\ Y = \frac{v-{c_y}'}{f'} \cdot Z \end{cases} \end{aligned} \text{或} \begin{aligned} \begin{cases} \text{bd} = \frac{B}{d_p}\\ Z = \text{depth} = f' \cdot \text{bd} \\ X = (u-{c_x}_1') \cdot \text{bd} \\ Y = (u-{c_y}') \cdot \text{bd} \end{cases} \end{aligned} ⎩ ⎨ ⎧​Z=depth=dp​f′⋅B​X=f′u−cx​1′​​⋅ZY=f′v−cy​′​⋅Z​​或⎩ ⎨ ⎧​bd=dp​B​Z=depth=f′⋅bdX=(u−cx​1′​)⋅bdY=(u−cy​′)⋅bd​​

其中， disp ( u , v ) \text{disp}(u,v) disp(u,v) 代表 视差图 坐标值

（2）算法2：根据视差图，利用 Q Q Q 矩阵 计算 三维点坐标（reprojectImageTo3D）

[ X ′ Y ′ Z ′ W ] = Q ⋅ [ u v disp ( u , v ) 1 ] \begin{bmatrix} X' \\ Y' \\ Z' \\ W \end{bmatrix} = Q \cdot \begin{bmatrix} u \\ v \\ \text{disp}(u,v) \\ 1 \end{bmatrix} ⎣ ⎡​X′Y′Z′W​⎦ ⎤​=Q⋅⎣ ⎡​uvdisp(u,v)1​⎦ ⎤​

最终，三维点坐标为

[ X Y Z ] = [ X ′ W Y ′ W Z ′ W ] \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} \frac{X'}{W} \\[2ex] \frac{Y'}{W} \\[2ex] \frac{Z'}{W} \end{bmatrix} ⎣ ⎡​XYZ​⎦ ⎤​=⎣ ⎡​WX′​WY′​WZ′​​⎦ ⎤​