深度学习（十一）---zed 调用yolov5 进行识别目标并实时测距

1. 前言

zed 相机测距有2种方式：一种是根据点云数据进行测试，二是根据zed获取深度值进行测距。上篇文章调用yolov5模型进行实时图像推理及网页端部署我们讲述了zed调用yolov5进行目标识别，我们在此基础上进一步实现目标测距功能。

2.深度图和点云图的区别：

（1）深度图像也叫距离影像，是指将从图像采集器到场景中各点的距离（深度）值作为像素值的图像。深度D等于像素在该视图相机坐标系下Z坐标。获取方法有：激光雷达深度成像法、计算机立体视觉成像、坐标测量机法、莫尔条纹法、结构光法。

（2）点云：当一束激光照射到物体表面时，所反射的激光会携带方位、距离等信息。通过深度相机得到的物体外观表面的点数据集合是点云。使用深度相机得到的点云信息为三维坐标（X，Y，Z）。点云格式有*.las ;*.pcd; *.txt等。

（3）深度数据流所提供的图像帧中，每一个像素点代表的是在深度感应器的视野中，该特定的（x, y）坐标处物体到离摄像头平面最近的物体到该平面的距离（以毫米为单位）。

（4）深度图像经过坐标转换可以计算为点云数据；有规则及必要信息的点云数据可以反算为深度图像。

（5）视差图是立体匹配算法的产出，而深度图则是立体匹配到点云生成的中间桥梁。

简而言之，点云可以看作是三维的深度图。深度图只包含每个像素的距离或Z信息，而点云包含物体表面的3D点（X，Y， Z）的集合，可以包含颜色信息。

3.测距原理

（1）基于点云三维测距

基于点云的三维测距，首先获取目标物体(x,y)的点云值,然后利用欧几里得公式进行求解

point_cloud = sl.Mat()
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU)
s, point_cloud_value = point_cloud.get_value((x2+x1)/2, (y2+y1)/2)
或者： 
point_cloud_value = point_cloud.get_value((x2+x1)/2, (y2+y1)/2)[1]
# print("point_cloud_value",point_cloud_value) -->输出：-->(SUCCESS, array([....]))
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] + point_cloud_value[2] * point_cloud_value[2])

（2）基于深度值测距

使用深度值进行测距，可以直接得出距离值

depth = sl.Mat()
zed.retrieve_measure(depth, sl.MEASURE.DEPTH, sl.MEM.CPU)
s,depth_value = depth.get_value((x2+x1)/2, (y2+y1)/2)  -->输出：(SUCCESS, ...)
或者：
depth_value = depth.get_value((x2+x1)/2, (y2+y1)/2)[1]

4.完整代码实现

import time
import cv2
import numpy as np
import torch
from models.experimental import attempt_load
from utils.datasets import letterbox
from utils.general import check_img_size, non_max_suppression,scale_coords, xyxy2xywh,set_logging,plot_one_box
# from utils.plots import colors, plot_one_box
from utils.torch_utils import select_device,time_synchronized
from numpy import random
import pyzed.sl as sl@torch.no_grad()
def detect(weights='weights/yolov5s.pt', imgsz=512,           cap = 0,             conf_thres=0.25,    iou_thres=0.45,      max_det=1000,        device='',           crop=True,           classes=None,        agnostic_nms=False,  #class-agnostic NMSaugment=False,       half=False,         hide_labels=False,   hide_conf=False,     line_thickness=3    ):######## zed 初始化 #########zed = sl.Camera()# 设置相机的分辨率1080和采集帧率30fpsinit_params = sl.InitParameters()init_params.camera_resolution = sl.RESOLUTION.HD1080  # Use HD1080 video modeinit_params.camera_fps = 30  # fps可选：15、30、60、100err = zed.open(init_params)  # 根据自定义参数打开相机if err != sl.ERROR_CODE.SUCCESS:exit(1)runtime_parameters = sl.RuntimeParameters()  # 设置相机获取参数runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD  # 创建sl.Mat对象来存储图像（容器），Mat类可以处理1到4个通道的多种矩阵格式（定义储存图象的类型）image = sl.Mat()  # 图像point_cloud = sl.Mat() #点云depth = sl.Mat()  #深度# # 获取分辨率# resolution = zed.get_camera_information().camera_resolution# w, h = resolution.width , resolution.height# x,y = int(w/2),int(h/2)  # 中心点########### 图像预处理 ##########device = select_device(device)print("device", device)half &= device.type != 'cpu'  model = attempt_load(weights, map_location=device)  stride = int(model.stride.max())  imgsz = check_img_size(imgsz, s=stride)  names = model.module.names if hasattr(model, 'module') else model.namescolors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))]  #toFP16if half:model.half()  if device.type != 'cpu':model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters()))) while True:### zed捕获图像 #####if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:  #相机成功获取图象# 获取图像timestamp = zed.get_timestamp(sl.TIME_REFERENCE.CURRENT)  #获取图像被捕获时的时间戳(ms)zed.retrieve_image(image, sl.VIEW.LEFT)  # image：容器，sl.VIEW.LEFT：内容 ，左镜头view = image.get_data()  # 转换成图像数组，便于后续的显示或者储存# 显示彩色图# img0 = cv2.resize(view,(384,384))im = view[:, :, 0:3]#### 图像推理，目标识别 ############labels = []t0 = time.time()img = letterbox(im, imgsz)[0]  #channel=3img = img[:, :, ::-1].transpose(2, 0, 1)  #BGR to RGB, to 3x416x416img = np.ascontiguousarray(img)  #channel = imgszimg = torch.from_numpy(img).to(device)#uint8 to fp16/32img = img.half() if half else img.float()  #0 - 255 to 0.0 - 1.0img /= 255.0   #512if img.ndimension() == 3:img = img.unsqueeze(0)# print("shape2",img.shape[2])   #320t1 = time_synchronized()pred = model(img, augment=False)[0]pred = non_max_suppression(pred, conf_thres, iou_thres, classes, agnostic_nms)t2 = time_synchronized()for i, det in enumerate(pred):  #s, im0 = '', im.copy()s += '%gx%g ' % img.shape[2:]  gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  if det is not None and len(det):det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()for c in det[:, -1].unique():n = (det[:, -1] == c).sum()s += f"{n} {names[int(c)]}{'s' * (n > 1)}, "  for *xyxy, conf, cls in reversed(det):   #检测框坐标，置信度，类别idxywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  line = (cls, *xywh, conf)  c = int(cls)  #names[c] 获取类别名称label = None if hide_labels else (names[c] if hide_conf else f'{names[c]} {conf:.2f}')if crop:    plot_one_box(xyxy, im0, label=label, color=colors[int(c)], line_thickness=line_thickness)labels.append([names[c],conf,xyxy])###### 点云测距 #########import mathx1, y1 = int(xyxy[0]), int(xyxy[1])x2, y2 = int(xyxy[2]), int(xyxy[3])zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU)s, point_cloud_value = point_cloud.get_value((x2+x1)/2, (y2+y1)/2)# print("point_cloud_value",point_cloud_value)distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] +point_cloud_value[2] * point_cloud_value[2])cv2.circle(im0, (int((x2+x1)/2), int((y2+y1)/2)), 5, [0, 0, 255], 5) #画出目标中心点               cv2.putText(im0, "Distance: " + str(round(distance / 1000, 2)) + 'm', (int(xyxy[0]), int(xyxy[3] + 25)),cv2.FONT_HERSHEY_COMPLEX, 1, colors[int(c)], 2) #标出点云距离mm-->m##### 求解深度距离 ##########zed.retrieve_measure(depth, sl.MEASURE.DEPTH, sl.MEM.CPU)s1,depth_value = depth.get_value((x2+x1)/2, (y2+y1)/2)cv2.putText(im0, "depth_value: " + str(round(depth_value /1000, 2)) + 'm', (int(xyxy[0]), int(xyxy[3] + 75)),cv2.FONT_HERSHEY_COMPLEX, 1, colors[int(c)], 2)  #标出深度值# print("depth_value",depth_value)print(f'time: ({time.time() - t0:.3f}s)')cv2.namedWindow('666',cv2.WINDOW_NORMAL)cv2.resizeWindow('666',800,600)cv2.imshow("666",im0)key = cv2.waitKey(20)  if key == 27:breakcv2.destroyAllWindows()if __name__ == "__main__":detect()