00 准备工作

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

Ubutntu下使用realsense d435i（一）：准备工作以及安装pyrealsense2

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

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

realsense d435i获取某像素点三维坐标（计算深度和点云两种方法）
https://blog.csdn.net/gyxx1998/article/details/121611001
详细的注释可以参考上一篇，这一篇只放一下代码，并将关键代码做简要介绍。
realsense官方的api介绍可以参考：

• https://dev.intelrealsense.com/docs/projection-in-intel-realsense-sdk-20

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中是应该乘还是除，还是不需要处理呢？