双目测距 SGBM算法 Python版
Posted 一颗小树x
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了双目测距 SGBM算法 Python版相关的知识，希望对你有一定的参考价值。
前言
首先进行双目定标，获取双目摄像头内部的参数后，进行测距。本次的双目视觉测距，基于SGBM算法。
注意：双目定标的效果会影响测距的精准度，建议大家在做双目定标时，做好一些（尽量让误差小）
如果不太了解双目视觉原理，建议先看看这篇文章：一篇文章认识《双目立体视觉》
目录
一、双目测距 效果
基于SGBM算法，生成视差图的效果
用鼠标点击视差图，程序会自动计算该点的世界坐标、距离，输出信息如下：
像素坐标 x = 523, y = 366
世界坐标xyz 是： 0.37038836669921876 0.24825479125976563 0.7842975463867188 m
距离是： 0.9021865798368828 m
这里的距离是双目相机中心（左右相机中心）到物体的实际距离，如上面的是以米为单位。
二、 双目测距 流程思路
程序流程图 如下
三、双目测距 前提准备
1）打开双目摄像头；
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）
双目数据转化可以参考：双目视觉 三维重建、测距 ---准备工作（数据转化）
四、双目测试 实现
完整代码 主要包括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一览 - 知乎
下面将一些实用性的，如何调整SGBM中参数，达到不同环境有好的效果。
OpenCV中SGBM函数，C++版：
Python 调用接口：参数同上
cv.StereoSGBM_create()参数含义:
| minDisparity 最小视差 | 最小可能的视差值。正常情况下为零，但有时校正算法会导致图像移位，因此需要相应调整此参数。 |
| numDisparities 数量差异 | 最大视差减去最小视差。该值始终大于零。在当前的实现中，这个参数必须能被 16 整除。 |
| blockSize 块大小 | 匹配块大小。它必须是奇数 >=1 。通常，它应该在 3..11 范围内的某个地方。 |
| P1 | 第一个参数控制视差平滑度。P1 是对相邻像素之间正负 1 的视差变化的惩罚。 |
| P2 | 第二个参数控制视差平滑度。值越大，视差越平滑。P2 是相邻像素之间视差变化超过 1 的惩罚。该算法需要 P2 > P1 。 |
| disp12MaxDiff | 左右视差检查中允许的最大差异（以整数像素为单位）。将其设置为非正值以禁用检查。 |
| preFilterCap 预过滤帽 | 预过滤图像像素的截断值。该算法首先计算每个像素的 x 导数，并按 [-preFilterCap, preFilterCap] 间隔裁剪其值。结果值传递给 Birchfield-Tomasi 像素成本函数。 |
| uniquenessRatio 唯一性比率 | 以百分比为单位的最佳（最小）计算成本函数值应“赢得”第二个最佳值以认为找到的匹配是正确的。通常，5-15 范围内的值就足够了。 |
| speckleWindowSize 斑点窗口大小 | 平滑视差区域的最大大小，以考虑它们的噪声斑点并使其无效。将其设置为 0 以禁用斑点过滤。否则，将其设置在 50-200 范围内的某个位置。 |
| speckleRange 散斑范围 | 每个连接组件内的最大视差变化。如果做散斑过滤，参数设置为正值，会隐式乘以16，正常情况下，1或2就足够了。 |
| mode 模式 | 将其设置为StereoSGBM::MODE_HH以运行完整的两遍动态编程算法。它将消耗 O(W*H*numDisparities) 字节，这对于 640x480 立体声来说很大，对于 HD 尺寸的图片来说很大。默认情况下，它设置为 false 。 |
参考：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等目标检测算法把物体框出来；然后计算物体的中心或质点，并在附近选取一点计算三维坐标和距离。
参考文献
欢迎交流；
以上是关于双目测距 SGBM算法 Python版的主要内容，如果未能解决你的问题，请参考以下文章