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 NMS
augment=False,
half=False,
hide_labels=False,
hide_conf=False,
line_thickness=3
):
######## zed 初始化 #########
zed = sl.Camera()
# 设置相机的分辨率1080和采集帧率30fps
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD1080 # Use HD1080 video mode
init_params.camera_fps = 30 # fps可选：15、30、60、100
err = 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.names
colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))]
#toFP16
if 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=3
img = img[:, :, ::-1].transpose(2, 0, 1) #BGR to RGB, to 3x416x416
img = np.ascontiguousarray(img) #channel = imgsz
img = torch.from_numpy(img).to(device)
#uint8 to fp16/32
img = img.half() if half else img.float()
#0 - 255 to 0.0 - 1.0
img /= 255.0 #512
if img.ndimension() == 3:
img = img.unsqueeze(0)
# print("shape2",img.shape[2]) #320
t1 = 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): #检测框坐标，置信度，类别id
xywh = (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 math
x1, 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:
break
cv2.destroyAllWindows()
if __name__ == "__main__":
detect()
5.可视化展示
由此可以看出深度测距和点云测距存在一定误差，一般使用点云测距会更精准些。
