Ubutntu下使用realsense d435i（二）：获取某二维像素点的三维坐标

2024年2月14日 3次阅读 来源: 冰激凌啊

00 准备工作

准备工作包括安装驱动以及pyrealsense2，具体可参考上一篇：

01 通过二维像素点获得物体三维坐标

这个在上一篇已经介绍了代码：

详细的注释可以参考上一篇，这一篇只放一下代码，并将关键代码做简要介绍。

realsense官方的api介绍可以参考：

realsense提供的api中，通过二维像素点获得三维坐标的共有 两种方式：

  • 获取该像素点的深度 –> 根据深度z，获得x和y
  • 通过点云计算x，y，z坐标

上述两种方法得到的结果完全相同。

将图像用opencv显示出来，计算的三维坐标结果也用cv2.putText()显示在图像上。

其中需要注意的是，颜色顺序不是RGB，而是BGR，所以想改字体颜色的时候需要注意一下。

1.1 通过深度计算（rs.rs2_deproject_pixel_to_point）

# -*- coding: utf-8 -*-
import pyrealsense2 as rs
import numpy as np
import cv2
 
 
''' 设置 '''
pipeline = rs.pipeline()    # 定义流程pipeline，创建一个管道
config = rs.config()    # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)      # 配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)     # 配置color流

pipe_profile = pipeline.start(config)       # streaming流开始

# 创建对齐对象与color流对齐
align_to = rs.stream.color      # align_to 是计划对齐深度帧的流类型
align = rs.align(align_to)      # rs.align 执行深度帧与其他帧的对齐
 

''' 获取对齐图像帧与相机参数 '''
def get_aligned_images():
    
    frames = pipeline.wait_for_frames()     # 等待获取图像帧，获取颜色和深度的框架集 
    aligned_frames = align.process(frames)      # 获取对齐帧，将深度框与颜色框对齐 

    aligned_depth_frame = aligned_frames.get_depth_frame()      # 获取对齐帧中的的depth帧 
    aligned_color_frame = aligned_frames.get_color_frame()      # 获取对齐帧中的的color帧

    #### 获取相机参数 ####
    depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics     # 获取深度参数（像素坐标系转相机坐标系会用到）
    color_intrin = aligned_color_frame.profile.as_video_stream_profile().intrinsics     # 获取相机内参


    #### 将images转为numpy arrays #### 
    img_color = np.asanyarray(aligned_color_frame.get_data())       # RGB图 
    img_depth = np.asanyarray(aligned_depth_frame.get_data())       # 深度图（默认16位）

    return color_intrin, depth_intrin, img_color, img_depth, aligned_depth_frame


''' 获取随机点三维坐标 '''
def get_3d_camera_coordinate(depth_pixel, aligned_depth_frame, depth_intrin):
    x = depth_pixel[0]
    y = depth_pixel[1]
    dis = aligned_depth_frame.get_distance(x, y)        # 获取该像素点对应的深度
    # print ('depth: ',dis) # 深度单位是m
    camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, dis)
    # print ('camera_coordinate: ',camera_coordinate)
    return dis, camera_coordinate



if __name__=="__main__":
    while True:
        ''' 获取对齐图像帧与相机参数 '''
        color_intrin, depth_intrin, img_color, img_depth, aligned_depth_frame = get_aligned_images()        # 获取对齐图像与相机参数


        ''' 获取随机点三维坐标 '''
        depth_pixel = [320, 240]        # 设置随机点，以相机中心点为例
        dis, camera_coordinate = get_3d_camera_coordinate(depth_pixel, aligned_depth_frame, depth_intrin)
        print ('depth: ',dis)       # 深度单位是m
        print ('camera_coordinate: ',camera_coordinate)


        ''' 显示图像与标注 '''
        #### 在图中标记随机点及其坐标 ####
        cv2.circle(img_color, (320,240), 8, [255,0,255], thickness=-1)
        cv2.putText(img_color,"Dis:"+str(dis)+" m", (40,40), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[0,0,255])
        cv2.putText(img_color,"X:"+str(camera_coordinate[0])+" m", (80,80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        cv2.putText(img_color,"Y:"+str(camera_coordinate[1])+" m", (80,120), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        cv2.putText(img_color,"Z:"+str(camera_coordinate[2])+" m", (80,160), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        
        #### 显示画面 ####
        cv2.imshow('RealSence',img_color)
        key = cv2.waitKey(1)

1.1.1 代码解析

  • config.enable_stream()函数可以设置不同的分辨率
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)      # 配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)     # 配置color流


config.enable_stream(rs.stream.depth,  848, 480, rs.format.z16, 90)
config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)


config.enable_stream(rs.stream.depth,  1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
  • aligned_depth_frame.get_distance(x, y)是关键代码
    • x和y为二维图像，像素点的坐标
    • 返回值为对应像素点的深度
  • camera_coordinate = rs.rs2_deproject_pixel_to_point(intrin=depth_intrin, pixel=[x, y], depth=dis)是关键代码
    • 输入 depth_intrin ：从上一步获取
    • 输入 x ：像素点的x
    • 输入 y ：像素点的y
    • 输入 dis ：上一步计算的真实距离（输入的dis与输出的距离z是一样的，改变的只是x与y）
    • 输出 camera_coordinate ：二维点在实际中对象的三维坐标x，y，z
dis = aligned_depth_frame.get_distance(x, y)        # 获取该像素点对应的深度

camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, dis) # 获取对应像素点的三维坐标

1.2 通过点云计算

# -*- coding: utf-8 -*-
import pyrealsense2 as rs
import numpy as np
import cv2
 
 
''' 设置 '''
pipeline = rs.pipeline()    # 定义流程pipeline，创建一个管道
config = rs.config()    # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)      # 配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)     # 配置color流

# config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 90)
# config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)

# config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)

pipe_profile = pipeline.start(config)       # streaming流开始

pc = rs.pointcloud()        # 声明点云对象
points = rs.points()
 

''' 获取图像帧 '''
def get_images():
    
    frames = pipeline.wait_for_frames()     # 等待获取图像帧，获取颜色和深度的框架集 

    depth_frame = frames.get_depth_frame()      # 获取depth帧 
    color_frame = frames.get_color_frame()      # 获取color帧

    ###### 将images转为numpy arrays ##### 
    img_color = np.asanyarray(color_frame.get_data())       # RGB图 
    img_depth = np.asanyarray(depth_frame.get_data())       # 深度图（默认16位）

    return  img_color, img_depth, depth_frame, color_frame


''' 获取随机点三维坐标(点云方法) '''
def get_3d_camera_coordinate(depth_pixel, color_frame, depth_frame):
    x = depth_pixel[0]
    y = depth_pixel[1]

    ###### 计算点云 #####
    pc.map_to(color_frame)
    points = pc.calculate(depth_frame)
    vtx = np.asanyarray(points.get_vertices())
    # print ('vtx_before_reshape: ', vtx.shape) # 307200
    vtx = np.reshape(vtx,(480, 640, -1))   
    # print ('vtx_after_reshape: ', vtx.shape) # (480, 640, 1)

    camera_coordinate = vtx[y][x][0]
    # print ('camera_coordinate: ',camera_coordinate)
    dis = camera_coordinate[2]
    return dis, camera_coordinate



if __name__=="__main__":
    while True:
        ''' 获取图像帧 '''
        img_color, img_depth, depth_frame, color_frame = get_images()        # 获取图像


        ''' 获取随机点三维坐标 '''
        depth_pixel = [320, 240]        # 设置随机点，以相机中心点为例
        dis, camera_coordinate = get_3d_camera_coordinate(depth_pixel, color_frame, depth_frame)
        print ('depth: ',dis)       # 深度单位是m
        print ('camera_coordinate: ',camera_coordinate)


        ''' 显示图像与标注 '''
        #### 在图中标记随机点及其坐标 ####
        cv2.circle(img_color, (320,240), 8, [255,0,255], thickness=-1)
        cv2.putText(img_color,"Dis:"+str(dis)+" m", (40,40), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[0,0,255])
        cv2.putText(img_color,"X:"+str(camera_coordinate[0])+" m", (80,80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        cv2.putText(img_color,"Y:"+str(camera_coordinate[1])+" m", (80,120), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        cv2.putText(img_color,"Z:"+str(camera_coordinate[2])+" m", (80,160), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
        
        #### 显示画面 ####
        cv2.imshow('RealSence',img_color)
        key = cv2.waitKey(1)

1.2.1 代码解析

  • 点云计算的关键代码如下：
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
vtx = np.asanyarray(points.get_vertices())
  • 坐标计算的关键代码如下
    • 注意reshape中参数的顺序
    • 注意是先y，在x
vtx = np.reshape(vtx,(480, 640, -1)) 
camera_coordinate = vtx[y][x][0]

02 待解决

  • depth_scale不为1，放到实际的depth中是应该乘还是除，还是不需要处理呢？
    原文作者：冰激凌啊
    原文地址: https://blog.csdn.net/gyxx1998/article/details/121635086
    本文转自网络文章，转载此文章仅为分享知识，如有侵权，请联系博主进行删除。
点赞