使用深度相机D435i+YOLOv8实现物体三维坐标实时显示

一、获取相机内参

下列指令为获取相机内参指令，输入此指令前需要获得相机的深度帧和彩色帧数据。

如何使用vsCode打开intel D435i深度相机

# 获取相机内参
depth_intrinsics = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
# 命令行打印相机内参显示
print("Depth Intrinsics:", depth_intrinsics)
print("Color Intrinsics:", color_intrinsics)

显示结果为：

Depth Intrinsics: [ 640x480  p[322.02 236.768]  f[391.288 391.288]  Brown Conrady [0 0 0 0 0] ]
Color Intrinsics: [ 640x480  p[328.884 240.065]  f[606.697 606.756]  Inverse Brown Conrady [0 0 0 0 0] ]

其中：640x480是像素宽度x像素高度； p[322.02 236.768]为主点（镜头光轴与图像平面的交点）的坐标； f[391.288 391.288]为焦距参数；Brown Conrady [0 0 0 0 0]分别表示畸变模型与畸变系数

二、关于 result.boxes.xywh.tolist()的xywh的解释

关于boxes = result.boxes.xywh.tolist()，可以在result源码里面找到相关的定义如下，可以看出相应的x，y为中心点坐标，w为宽度，h为高度

三、自动根据相机内参和xywh计算相应的深度数据

首先通过下列指令获取绘制的边框信息，包括中心点坐标x，y和边框宽度w，边框高度h

然后通过中心点的坐标计算相应点的深度距离，并利用rs.rs2_deproject_pixel_to_point(相机深度内参，点坐标，点深度)将其转化为相机坐标系下的x，y，z；最后实现单位转化，显示为以mm为单位的数据。

四、代码与结果展示

详细代码文件如下：

# 此程序用于实现视频分帧识别物体,并为所识别的物品添加矩形框，显示置信度、标签等，更新于2024/6/24
# 更新程序，用于显示实时三维坐标2024/6/24
import cv2
import numpy as np
import pyrealsense2 as rs
from ultralytics import YOLO  # 将YOLOv8导入到该py文件中

# 加载官方或自定义模型
model = YOLO(r"E:\Deep learning\YOLOv8\yolov8n.pt")  # 加载一个官方的检测模型
# model = YOLO(r"E:\Deep learning\YOLOv8\yolov8s.pt")  # 加载一个官方的检测模型
# model = YOLO(r"E:\Deep learning\YOLOv8\yolov8n-seg.pt")  # 加载一个官方的分割模型
# model = YOLO(r"E:\Deep learning\YOLOv8\yolov8n-pose.pt")  # 加载一个官方的姿态模型


# 深度相机配置
pipeline = rs.pipeline()  # 定义流程pipeline，创建一个管道
config = rs.config()  # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  # 初始化摄像头深度流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pipe_profile = pipeline.start(config)  # 启用管段流
align = rs.align(rs.stream.color)  # 这个函数用于将深度图像与彩色图像对齐

def get_aligned_images():  # 定义一个获取图像帧的函数，返回深度和彩色数组
    frames = pipeline.wait_for_frames()  # 等待获取图像帧
    aligned_frames = align.process(frames)  # 获取对齐帧，将深度框与颜色框对齐
    depth_frame = aligned_frames.get_depth_frame()  # 获取深度帧
    color_frame = aligned_frames.get_color_frame()  # 获取对齐帧中的的color帧
    depth_image = np.asanyarray(depth_frame.get_data())  # 将深度帧转换为NumPy数组
    color_image = np.asanyarray(color_frame.get_data())  # 将彩色帧转化为numpy数组
   
    # 获取相机内参
    # 获取深度内参
    depth_intri = depth_frame.profile.as_video_stream_profile().intrinsics
    # 获取彩色内参
    color_intri = color_frame.profile.as_video_stream_profile().intrinsics
    # 命令行输出内参检查
    # print("Depth Intrinsics:",depth_intri)
    # print("Color Intrinsics:",color_intri)
    
    # cv2.applyColorMap（）将深度图像转化为彩色图像，以便更好的可视化分析
    depth_colormap = cv2.applyColorMap(
                cv2.convertScaleAbs(depth_image, alpha=0.07), cv2.COLORMAP_JET)
    # 返回深度内参、对齐深度帧、彩色图像
    return depth_intri, depth_frame, color_image

if __name__ == '__main__':
    try:
        while True:
            depth_intri, depth_frame, color_image = get_aligned_images()  # 获取深度帧和彩色帧
            source = [color_image]
            # 轨迹追踪，persist=true表示数据储存
            # results = model.track(source, persist=True)
            results = model.predict(source, save=False)
            # 预测完后打印目标框
            for result in results:
                # 获取边框列表，其中每个边界框由中心点坐标、宽度、高度组成
                boxes = result.boxes.xywh.tolist()
                # 逐个绘图
                im_array = result.plot()
                
                for i in range(len(boxes)):  # 遍历boxes列表
                    # 将中心点坐标位置转化为整型，并赋值给ux和uy
                    ux, uy = int(boxes[i][0]), int(boxes[i][1])
                    # 得到深度帧中的对应坐标处的距离
                    dis = depth_frame.get_distance(ux, uy)
                    # 将指定深度帧的像素坐标和距离值转化为相机坐标系下的坐标x，y，z
                    camera_xyz = rs.rs2_deproject_pixel_to_point(
                        depth_intri, (ux, uy), dis)
                    # 将x，y，z转化成3位小数的Numpy数组
                    camera_xyz = np.round(np.array(camera_xyz), 3)
                    # 将单位转化为mm
                    camera_xyz = camera_xyz * 1000
                    #camera_xyz = np.array(list(camera_xyz)) * 1000
                    # 转化为一个列表
                    camera_xyz = list(camera_xyz)
                    # 在im_array上绘制圆形，圆心坐标为ux，uy，半径为4个像素
                    # 颜色为（255，255，255），线条宽度为5个像素
                    cv2.circle(im_array, (ux, uy), 4, (255, 255, 255), 5)  # 标出中心点
                    # 在im_array上绘制文本框，文本框内容为camera_xyz
                    # 位置为(ux + 20, uy + 10)，0字体类型，0.5字体大小，255，255，255字体颜色
                    # 最后两个参数为线宽和线型
                    cv2.putText(im_array, str(camera_xyz), (ux + 20, uy + 10), 0, 0.5,
                                [225, 255, 255], thickness=1, lineType=cv2.LINE_AA)  # 标出坐标
            
            # 设置窗口，窗口大小根据图像自动调整
            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
            # 将图像images显示在窗口中，这个显示的是带有追踪结果的图像
            cv2.imshow('RealSense', im_array)
            key = cv2.waitKey(1)  # 等待用户输入
            # Press esc or 'q' to close the image window
            if key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                pipeline.stop()
                break
    finally:
        # Stop streaming
        pipeline.stop()

结果展示：

intel D435i相机实时测量显示物体三维坐标

五、参考学习博文

Realsense D435i 通过YOLOv5、YOLOv8输出目标三维坐标_yolov5+d435i-CSDN博客