双目测距 SGBM算法 Python版

短命女 2022-09-11 12:16 995阅读 0赞

## 前言 ##

首先进行**双目定标**，获取双目摄像头**内部的参数**后，进行测距。本次的双目视觉测距，基于**SGBM**算法。

注意：双目定标的效果会影响测距的精准度，建议大家在做双目定标时，做好一些（尽量让误差小）

如果不太了解双目视觉原理，建议先看看这篇文章：[一篇文章认识《双目立体视觉》][Link 1]

**目录**

一、双目测距 效果

二、 双目测距 流程思路

三、双目测距 前提准备

四、双目测试 实现

五、SGBM算法

小结

参考文献

--------------------

## 一、双目测距 效果 ##

基于SGBM算法，生成视差图的效果

![20210916105702920.png][]

用鼠标点击视差图，程序会自动计算该点的世界坐标、距离，输出信息如下：

> 像素坐标 x = 523, y = 366  
> 世界坐标xyz 是： 0.37038836669921876 0.24825479125976563 0.7842975463867188 m  
> 距离是： 0.9021865798368828 m

这里的距离是双目相机中心（左右相机中心）到物体的实际距离，如上面的是以米为单位。

## 二、 双目测距 流程思路 ##

程序流程图 如下

![20210916140028180.png][]

## 三、双目测距 前提准备 ##

1）打开双目摄像头；

参考这里：[OpenCV 打开双目摄像头（python版）][OpenCV _python]

2）双目摄像头标定；获取的参数：

左相机内参、左相机畸变系数:\[k1, k2, p1, p2, k3\]

右相机内参、右相机畸变系数:\[k1, k2, p1, p2, k3\]

左右相机之间的旋转矩阵、平移向量。并命名为：camera\_config.py，下面测距需要用到的。

import cv2
    import numpy as np
    
    # 左相机内参
    left_camera_matrix = np.array([[416.841180253704, 0.0, 338.485167779639],
                                             [0., 416.465934495134, 230.419201769346],
                                             [0., 0., 1.]])
    
    # 左相机畸变系数:[k1, k2, p1, p2, k3]
    left_distortion = np.array([[-0.0170280933781798, 0.0643596519467521, -0.00161785356900972, -0.00330684695473645, 0]])
    
    # 右相机内参
    right_camera_matrix = np.array([[417.765094485395, 0.0, 315.061245379892],
                                              [0., 417.845058291483, 238.181766936442],
                                                [0., 0., 1.]])
    # 右相机畸变系数:[k1, k2, p1, p2, k3]                                          
    right_distortion = np.array([[-0.0394089328586398, 0.131112076868352, -0.00133793245429668, -0.00188957913931929, 0]])
    
    # om = np.array([-0.00009, 0.02300, -0.00372])
    # R = cv2.Rodrigues(om)[0]
    
    # 旋转矩阵
    R = np.array([[0.999962872853149, 0.00187779299260463, -0.00840992323112715],
                               [ -0.0018408858041373, 0.999988651353238, 0.00439412154902114],
                               [ 0.00841807904053251, -0.00437847669953504, 0.999954981430194]])
    
    # 平移向量
    T = np.array([[-120.326603502087], [0.199732192805711], [-0.203594457929446]])
    
    size = (640, 480)
    
    R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
                                                                      right_camera_matrix, right_distortion, size, R,
                                                                      T)
    
    left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
    right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)

双目定标可以参考：[双目视觉 定标+矫正 （基于MATLAB）][_ _MATLAB]

双目数据转化可以参考：[双目视觉 三维重建、测距 ---准备工作（数据转化）][_ ---]

## 四、双目测试 实现 ##

** 完整代码 **主要包括main.py、camera\_config.py两个文件的代码；main.py是主函数，实现双目视觉测距。相机参数用 camera\_config.py表示。

main.py代码如下：

# -*- coding: utf-8 -*-
    
    import numpy as np
    import cv2
    import camera_config
    import random
    import math
    
    
    cap = cv2.VideoCapture(0)
    cap.set(3, 1280)
    cap.set(4, 480)  #打开并设置摄像头
    
    
    # 鼠标回调函数
    def onmouse_pick_points(event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            threeD = param
            print('\n像素坐标 x = %d, y = %d' % (x, y))
            # print("世界坐标是：", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm")
            print("世界坐标xyz 是：", threeD[y][x][0]/ 1000.0 , threeD[y][x][1]/ 1000.0 , threeD[y][x][2]/ 1000.0 , "m")
    
            distance = math.sqrt( threeD[y][x][0] **2 + threeD[y][x][1] **2 + threeD[y][x][2] **2 ) 
            distance = distance / 1000.0  # mm -> m
            print("距离是：", distance, "m")
    
    WIN_NAME = 'Deep disp'
    cv2.namedWindow(WIN_NAME,  cv2.WINDOW_AUTOSIZE)
    
    while True:
      ret, frame = cap.read()
      frame1 = frame[0:480, 0:640]
      frame2 = frame[0:480, 640:1280]  #割开双目图像
    
      imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)  # 将BGR格式转换成灰度图片
      imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
    
      # cv2.remap 重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
      # 依据MATLAB测量数据重建无畸变图片
      img1_rectified = cv2.remap(imgL, camera_config.left_map1, camera_config.left_map2, cv2.INTER_LINEAR)
      img2_rectified = cv2.remap(imgR, camera_config.right_map1, camera_config.right_map2, cv2.INTER_LINEAR)  
    
      imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)  
      imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)
    
      # SGBM
      blockSize = 8
      img_channels = 3
      stereo = cv2.StereoSGBM_create(minDisparity = 1,
                                       numDisparities = 64,
                                       blockSize = blockSize,
                                       P1 = 8 * img_channels * blockSize * blockSize,
                                       P2 = 32 * img_channels * blockSize * blockSize,
                                       disp12MaxDiff = -1,
                                       preFilterCap = 1,
                                       uniquenessRatio = 10,
                                       speckleWindowSize = 100,
                                       speckleRange = 100,
                                       mode = cv2.STEREO_SGBM_MODE_HH)
    
      disparity = stereo.compute(img1_rectified, img2_rectified) # 计算视差
    
      disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)  #归一化函数算法
    
      threeD = cv2.reprojectImageTo3D(disparity, camera_config.Q, handleMissingValues=True)  #计算三维坐标数据值
      threeD = threeD * 16 
    
      # threeD[y][x] x:0~640; y:0~480;   !!!!!!!!!!
      cv2.setMouseCallback(WIN_NAME, onmouse_pick_points, threeD) # 鼠标回调事件
    
      cv2.imshow("left", frame1)
      # cv2.imshow("right", frame2)
      # cv2.imshow("left_r", imgL)
      # cv2.imshow("right_r", imgR)
      cv2.imshow(WIN_NAME, disp)  #显示深度图的双目画面
    
      key = cv2.waitKey(1)
      if key == ord("q"):
        break
    
    cap.release()
    cv2.destroyAllWindows()

## 五、SGBM算法 ##

SGBM算法，全称是Stereo Processing by Semiglobal Matching and Mutual Information。

它是一种全局匹配算法，立体匹配的效果明显好于局部匹配算法，但是同时复杂度上也要远远大于局部匹配算法。

通过选取每个像素点的disparity，组成一个disparity map，设置一个和disparity map相关的全局能量函数，使这个能量函数最小化，以达到求解每个像素最优disparity的目的。

参考：[【关于立体视觉的一切】立体匹配成像算法BM，SGBM，GC，SAD一览 - 知乎][BM_SGBM_GC_SAD_ -]

下面将一些实用性的，如何调整SGBM中参数，达到不同环境有好的效果。

OpenCV中SGBM函数，C++版：

![20210916154106864.png][]

Python 调用接口：参数同上

cv.StereoSGBM_create()

参数含义:

<table> 
 <tbody> 
 <tr> 
 <td> minDisparity 最小视差 </td> 
 <td>最小可能的视差值。正常情况下为零，但有时校正算法会导致图像移位，因此需要相应调整此参数。</td> 
 </tr> 
 <tr> 
 <td> numDisparities 数量差异 </td> 
 <td>最大视差减去最小视差。该值始终大于零。在当前的实现中，这个参数必须能被 16 整除。</td> 
 </tr> 
 <tr> 
 <td> blockSize 块大小 </td> 
 <td>匹配块大小。它必须是奇数 &gt;=1 。通常，它应该在 3..11 范围内的某个地方。</td> 
 </tr> 
 <tr> 
 <td> P1 </td> 
 <td>第一个参数控制视差平滑度。P1 是对相邻像素之间正负 1 的视差变化的惩罚。</td> 
 </tr> 
 <tr> 
 <td>P2</td> 
 <td>第二个参数控制视差平滑度。值越大，视差越平滑。P2 是相邻像素之间视差变化超过 1 的惩罚。该算法需要 P2 &gt; P1 。</td> 
 </tr> 
 <tr> 
 <td>disp12MaxDiff</td> 
 <td>左右视差检查中允许的最大差异（以整数像素为单位）。将其设置为非正值以禁用检查。</td> 
 </tr> 
 <tr> 
 <td> preFilterCap 预过滤帽 </td> 
 <td>预过滤图像像素的截断值。该算法首先计算每个像素的 x 导数，并按 [-preFilterCap, preFilterCap] 间隔裁剪其值。结果值传递给 Birchfield-Tomasi 像素成本函数。</td> 
 </tr> 
 <tr> 
 <td> uniquenessRatio 唯一性比率 </td> 
 <td>以百分比为单位的最佳（最小）计算成本函数值应“赢得”第二个最佳值以认为找到的匹配是正确的。通常，5-15 范围内的值就足够了。</td> 
 </tr> 
 <tr> 
 <td> speckleWindowSize 斑点窗口大小 </td> 
 <td>平滑视差区域的最大大小，以考虑它们的噪声斑点并使其无效。将其设置为 0 以禁用斑点过滤。否则，将其设置在 50-200 范围内的某个位置。</td> 
 </tr> 
 <tr> 
 <td> speckleRange 散斑范围 </td> 
 <td>每个连接组件内的最大视差变化。如果做散斑过滤，参数设置为正值，会隐式乘以16，正常情况下，1或2就足够了。</td> 
 </tr> 
 <tr> 
 <td> mode 模式 </td> 
 <td>将其设置为<a href="https://docs.opencv.org/4.5.3/d2/d85/classcv_1_1StereoSGBM.html#a74120f5b7739495309c57f2da1c3e8cca0f746667febe92e1189e924c40752660" title="StereoSGBM::MODE_HH" rel="nofollow">StereoSGBM::MODE_HH</a>以运行完整的两遍动态编程算法。它将消耗 O(W*H*numDisparities) 字节，这对于 640x480 立体声来说很大，对于 HD 尺寸的图片来说很大。默认情况下，它设置为 false 。</td> 
 </tr> 
 </tbody> 
</table>

参考：[OpenCV: cv::StereoSGBM Class Reference][OpenCV_ cv_StereoSGBM Class Reference]

SGBM案例1：适合室外

# SGBM
      blockSize = 8
      img_channels = 3
      stereo = cv2.StereoSGBM_create(minDisparity = 1,
                                       numDisparities = 64,
                                       blockSize = blockSize,
                                       P1 = 8 * img_channels * blockSize * blockSize,
                                       P2 = 32 * img_channels * blockSize * blockSize,
                                       disp12MaxDiff = -1,
                                       preFilterCap = 1,
                                       uniquenessRatio = 10,
                                       speckleWindowSize = 100,
                                       speckleRange = 100,
                                       mode = cv2.STEREO_SGBM_MODE_HH)

SGBM案例2：

numberOfDisparities = ((640 // 8) + 15) & -16  # 640对应是分辨率的宽
    
    stereo = cv2.StereoSGBM_create(minDisparity=0, numDisparities=numberOfDisparities, blockSize=9, 
                                     P1=8*1*9*9, P2=32*1*9*9, disp12MaxDiff=1, uniquenessRatio=10, 
                                     speckleWindowSize=100, speckleRange=32, mode=cv2.STEREO_SGBM_MODE_SGBM)

## 小结 ##

通常双目视觉测距可以结合目标检测，首先用YOLO、SSD等目标检测算法把物体框出来；然后计算物体的中心或质点，并在附近选取一点计算三维坐标和距离。

## 参考文献 ##

[一篇文章认识《双目立体视觉》][Link 1]

[OpenCV 打开双目摄像头（python版）][OpenCV _python]

[双目视觉 定标+矫正 （基于MATLAB）][_ _MATLAB]

[双目视觉 三维重建、测距 ---准备工作（数据转化）][_ ---]

欢迎交流；

[Link 1]: https://blog.csdn.net/qq_41204464/article/details/115387812?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163177493816780269834817%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=163177493816780269834817&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_v2~rank_v29-4-115387812.pc_v2_rank_blog_default&utm_term=%E5%8F%8C%E7%9B%AE%E6%89%93%E5%BC%80&spm=1018.2226.3001.4450
[20210916105702920.png]: /images/20220828/3792d64c0d034b2c833ab091cd7962bf.png
[20210916140028180.png]: /images/20220828/8c7cf2e12a8c40eea65b8ad8dd8b6676.png
[OpenCV _python]: https://blog.csdn.net/qq_41204464/article/details/113090604?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163177493816780269834817%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=163177493816780269834817&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_v2~rank_v29-1-113090604.pc_v2_rank_blog_default&utm_term=%E5%8F%8C%E7%9B%AE%E6%89%93%E5%BC%80&spm=1018.2226.3001.4450
[_ _MATLAB]: https://blog.csdn.net/qq_41204464/article/details/86602452
[_ ---]: https://blog.csdn.net/qq_41204464/article/details/86710737
[BM_SGBM_GC_SAD_ -]: https://zhuanlan.zhihu.com/p/32752535
[20210916154106864.png]: /images/20220828/c56b20a60c1f4598ae7de15990d4b668.png
[OpenCV_ cv_StereoSGBM Class Reference]: https://docs.opencv.org/4.5.3/d2/d85/classcv_1_1StereoSGBM.html