最近在学习双目测距,下面分享一下我的实现过程。
主要是新版本的MATLAB标定的参数变了,然后教大家怎么找出对应的参数,然后踩了一个大坑,所以就出个教程。
首先说明一下我的硬件,如果想在过程中没有差异的实现也可以购买相同的硬件。我使用的双目摄像头是这一款:
先说说双目的应用,我的打算是用来双目测距后识别障碍物然后避障,双目摄像头在避障这方面有着独特的优势:不仅可以识别距离,还可以障碍物,因为有图像数据,可以通过类聚或者图像识别识别出障碍物。很多的深度相机都会有双目相机在内部,但是对比双目相机,深度相机会多出激光射频雷达,imu数据等。但是只使用双目也可以有比较好的识别效果了,所以双目相机是一个低成本的摄像头测距方案。
只要是有双摄像头的相机,就可以按照双目测距的流程实现测距。双目测距的流程主要是:标定->立体匹配->视差计算->类聚或者识别然后完成你想要的功能。
标定
首先需要拍摄标定的数据。我使用的是一个python程序拍摄照片,这个python程序需要调整的地方有:照片源数据 在这句话中“ cap = cv2.VideoCapture(0) ”的0表示系统中的相机0,如果使用的系统中有多个摄像头可能就会报错说摄像头有问题,所以可能需要修改为1或者其他。然后就是双目相机的图像,一般的双目相机都是两张图像合成一张图片然后获取的,就像是这样:
所以获取图像后需要对图像进行切割,将一张图像分为两张图像,所以需要你知道自己的双目相机的图像大小是多少,怎么切割,我的 是两张640x480图像,就是用下面中的代码中的参数进行切割。使用的是opencv的库,所以要提前安装。切割完就会将摄像头的数据保存到left和right两个文件夹到当前目录,left和right中就存放着左右摄像头采集到的图像,并且序号是一一对应的,如果重新拍照的话,图像的序号也是重新开始并覆盖原来的图片的,所以如果要重新拍照的话,最好将原来的left和right文件夹删掉。
拍摄也是有要求的,我是这样做的:首先将图像分为一个九宫格,然后在每个格子中都拍摄标定板(角度正对、向下倾斜、向上倾斜、向左、向右倾斜)的照片,注意倾斜的角度不要太大、标定板也不要超出任意摄像头的边界。拍摄完就可以标定了。
import cv2
import os
cap = cv2.VideoCapture(0)
# set the video frame width and height
cap.set(3,1280)
cap.set(4,480)
if not os.path.exists("left"):
os.makedirs("left")
if not os.path.exists("right"):
os.makedirs("right")
i = 1
try:
while True:
ret, frame = cap.read()
if not ret:
print("Failed to capture video")
break
# split the frame into left and right
left_frame = frame[:, :640, :]
right_frame = frame[:, 640:, :]
cv2.cvtColor(left_frame, cv2.COLOR_BGR2GRAY)
cv2.imshow("Left Camera", left_frame)
cv2.imshow("Right Camera", right_frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
elif key == ord('s'):
left_filename = "left" + str(i) + ".png"
right_filename = "right" + str(i) + ".png"
cv2.imwrite("left/" + left_filename, left_frame)
cv2.imwrite("right/" + right_filename, right_frame)
print("Image saved! left image:", left_filename)
print("Image saved! right image:", right_filename)
i += 1
except KeyboardInterrupt:
cv2.destroyAllWindows()
cap.release()
我使用的标定工具是MATLAB,首先下载MATLAB,下载参考:MATLAB R2023b安装包下载链接_matlab2023b下载-CSDN博客
MATLAB的大小还是挺大的,下载需要时间。下载完成后打开标定工具。在MATLAB中的命令行窗口中输入:stereoCameraCalibrator,等待片刻就打开了。
然后就可以添加刚刚拍摄的左右相机照片了,
添加完照片后就可以运行标定了,选择图片中的参数然后运行。
然后就可以适当的删除一点角度较大,误差较大的图像。最后导出标定后的参数。
然后就需要找到需要用到的标定参数,也是我在这过程中遇到最大的坑了。
打开参数变量的表。
选择CameraParameters1,这个是左摄像头的参数。
在这当中,我们需要使用的变量有stereoParams.CameraParameters1.RadialDistortion(纵向畸变K)、stereoParams.CameraParameters1.TangentialDistortion(横向畸变P)、stereoParams.CameraParameters1.K(内参矩阵)。
通过纵向畸变K和横向畸变P可以得到畸变系数:畸变向量中前5个畸变系数的依次是:[k1, k2, p1, p2, k3],这里不要将内参矩阵误以为是畸变系数中的k。
然后右摄像头也是同理得到内参矩阵和畸变系数。
然后回到第一页,选择PoseCamera2,这里有旋转矩阵和平移矩阵。
这里最大的坑就是,旋转矩阵和平移矩阵不需要转置了,可能是MATLAB的版本不同的问题,MATLAB双目标定的工具已经帮你转置完成了,因为网上有很多的教程都是以前的,然后参数不同呀、版本不同等问题,所以会叫你将矩阵转置后才使用,但是这里不需要,直接使用就可以了。
SGBM
得到上面的数据后就可以运行SGBM了,拷贝下面代码,然后修改标定的参数,和上面一样,需要修改摄像头数据编号和照片尺寸大小和怎么切割你的图像。然后运行就可以了。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import time
import random
import math
# -----------------------------------双目相机的基本参数---------------------------------------------------------
# left_camera_matrix 左相机的内参矩阵
# right_camera_matrix 右相机的内参矩阵
#
# left_distortion 左相机的畸变系数 格式(K1,K2,P1,P2,0)
# right_distortion 右相机的畸变系数
# -------------------------------------------------------------------------------------------------------------
# 左镜头的内参,如焦距
# left_camera_matrix = np.array([[502.341081272160,0.,0.],[0.0300554866661331,502.383632516866,0.],[318.945837875210,215.788834517675,1.]])
# right_camera_matrix = np.array([[499.320830799651,0.,0.],[-0.460483054921348,499.344892903859,0],[311.940494781887,211.023931616266,1.]])
left_camera_matrix = np.array([[502.341081272160,0.0300554866661331,318.945837875210],[0,502.383632516866,215.788834517675],[0,0,1]])
right_camera_matrix = np.array([[499.320830799651,-0.460483054921348,311.940494781887],[0,499.344892903859,211.023931616266],[0,0,1]])
# 畸变系数,K1、K2、K3为径向畸变,P1、P2为切向畸变
left_distortion = np.array([[0.0976799851165019,0.0887809202705125,-0.00240610404565200,0.000198315354211117,-0.404262922842844]])
right_distortion = np.array([[0.100135388660911,0.0776766407117976,-0.00425081681066429,-0.000667818422262915,-0.377296294577705]])
# 旋转矩阵
# R = np.array([[0.999994652629781,-0.000513772596382777,-0.00322966709776183],
# [0.000516606567401912,0.999999482250355,0.000876707862588203],
# [0.00322921499712801,-0.000878371641739908,0.999994400301202]])
R = np.array([[0.999994652629781,0.000516606567401912,0.00322921499712801],
[-0.000513772596382777,0.999999482250355,-0.000878371641739908],
[-0.00322966709776183,0.000876707862588203,0.999994400301202]])
# 平移矩阵
T = np.array([-57.7633107372652,0.0314095766542149,-0.530994776546538])
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)
print(Q)
# --------------------------鼠标回调函数---------------------------------------------------------
# event 鼠标事件
# param 输入参数
# -----------------------------------------------------------------------------------------------
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")
# 加载视频文件
capture = cv2.VideoCapture(0)
# set the video frame width and height
capture.set(3,1280)
capture.set(4,480)
WIN_NAME = 'Deep disp'
cv2.namedWindow(WIN_NAME, cv2.WINDOW_AUTOSIZE)
# 读取视频
fps = 0.0
ret, frame = capture.read()
while ret:
# 开始计时
t1 = time.time()
# 是否读取到了帧,读取到了则为True
ret, frame = capture.read()
# 切割为左右两张图片
# frame1 = frame[0:480, 0:640]
# frame2 = frame[0:480, 640:1280]
frame1 = frame[:, :640, :]
frame2 = frame[:, 640:, :]
# 将BGR格式转换成灰度图片,用于畸变矫正
imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
# 重映射,就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
# 依据MATLAB测量数据重建无畸变图片,输入图片要求为灰度图
img1_rectified = cv2.remap(imgL, left_map1, left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(imgR, right_map1, right_map2, cv2.INTER_LINEAR)
# 转换为opencv的BGR格式
imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)
imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)
# ------------------------------------SGBM算法----------------------------------------------------------
# blockSize 深度图成块,blocksize越低,其深度图就越零碎,0<blockSize<10
# img_channels BGR图像的颜色通道,img_channels=3,不可更改
# numDisparities SGBM感知的范围,越大生成的精度越好,速度越慢,需要被16整除,如numDisparities
# 取16、32、48、64等
# mode sgbm算法选择模式,以速度由快到慢为:STEREO_SGBM_MODE_SGBM_3WAY、
# STEREO_SGBM_MODE_HH4、STEREO_SGBM_MODE_SGBM、STEREO_SGBM_MODE_HH。精度反之
# ------------------------------------------------------------------------------------------------------
blockSize = 9
img_channels = 3
stereo = cv2.StereoSGBM_create(minDisparity=1,
numDisparities=80,
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)
# 生成深度图(颜色图)
dis_color = disparity
dis_color = cv2.normalize(dis_color, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
dis_color = cv2.applyColorMap(dis_color, 2)
# 计算三维坐标数据值
threeD = cv2.reprojectImageTo3D(disparity, Q, handleMissingValues=True)
# 计算出的threeD,需要乘以16,才等于现实中的距离
threeD = threeD * 16
#完成计时,计算帧率
fps = (fps + (1. / (time.time() - t1))) / 2
frame = cv2.putText(frame, "fps= %.2f" % (fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
cv2.imshow("depth", dis_color)
cv2.imshow("left", frame1)
cv2.imshow(WIN_NAME, disp) # 显示深度图的双目画面
# 鼠标回调事件
cv2.setMouseCallback("depth", onmouse_pick_points, threeD)
# 若键盘按下q则退出播放
if cv2.waitKey(1) & 0xff == ord('q'):
break
# 释放资源
capture.release()
# 关闭所有窗口
cv2.destroyAllWindows()
效果
点击有颜色的图像位置就会在窗口输出对应的深度数据。到这里就完成了双目测距的功能,有了深度图就可以做很多事情了。比如说使用yolo识别出物体,然后数据物体对应位置的深度数据就可以知道感兴趣的物体和你的距离,也有更多的应用等你探索。
精度数据:手工测了一组