realsense d435i获取某像素点三维坐标（计算深度和点云两种方法）

这篇写的比较详细但也比较乱，另一篇思路更清晰和简介一点，可供参考：

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

---

00 说明

• 代码1
  • 使用aligned_depth_frame.get_distance()与rs.rs2_deproject_pixel_to_point()获得三维坐标
  • opencv显示画面
  • 并将内参保存到了脚本文件目录下的json格式
  • 获取内参和图像帧的部分封装成函数

---

• 代码2
  • 两种方法：方法一是rs.rs2_deproject_pixel_to_point()，方法二是“点云”
  • 其中，第二种点云方法，共有三种计算方法
    ① 先转为float，再通过计算i索引找到坐标
    ② 先计算i索引找到坐标，再转为float
    ③ 直接reshape，然后通过[y][x]查找
    上述三种计算方法，推荐使用reshape
  • 这里有两点需要注意：
    ① 计算索引i时，i=y*宽度像素640+x
    ② reshape后查找是[y][x]，而不是[x][y]
  • opencv显示画面，并将坐标信息标注在画面上（这里需要注意，字体颜色排序不是RGB，而是BGR）

---

• 代码3
  • opencv显示画面，并将坐标信息标注在画面上
  • 使用aligned_depth_frame.get_distance()与rs.rs2_deproject_pixel_to_point()获得三维坐标
  • 将代码分开成两个函数，方便后续调用

---

关于 运行环境：

这是在Ubuntu18下运行，并且系统已安装ROS，所以是python2.7。

然后需要安装pyrealsense2包，安装过程可参考：https://blog.csdn.net/gyxx1998/article/details/121461293

因为是python2，所以文件首行需要添加：# -*- coding: utf-8 -*-，才可以写中文注释；如果是python3则没有此问题。

文件首行需要添加：# encoding: utf-8print，中文的print输出才可以正常显示

---

关于 注释：

上述三类代码都加了很详细的注释，可以都看一下，方便理解。

其中：

代码2我注释的比较详细，也有两种坐标获取方法，整体观感比较乱；

代码3为最后使用的，则比较简洁。

---

关于 遗留问题：

• depth_scale不为1，是否需要修改校准

---

关于 参考：

• pyrealsense 的基本操作：https://www.cnblogs.com/penway/p/13416824.html
  对于使用的api介绍简介清晰
• stack overflow：https://stackoverflow.com/questions/63266753/how-to-align-already-captured-rgb-and-depth-images
  简洁清晰的代码（作用是根据深度，清除某些背景）
• github官方api：https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/distance_to_object.ipynb?language=en_US
  介绍了和openCV dnn模块结合起来的用法示例
• realsense的官方api：
  https://dev.intelrealsense.com/docs/projection-in-intel-realsense-sdk-20
• realsense获取深度值，点云，以及通过深度值ｚ获得xyz：https://zhangzhe.blog.csdn.net/article/details/103730429
  两种获得三维坐标的方法比较清晰，比较容易理清思路（获取深度&点云）

01 代码1

# -*- coding: utf-8 -*-
import pyrealsense2 as rs
import numpy as np
import cv2
import json

pipeline = rs.pipeline()  #定义流程pipeline
config = rs.config()   #定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  #配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)   #配置color流
profile = pipeline.start(config)  #流程开始
align_to = rs.stream.color  #与color流对齐
align = rs.align(align_to)

def get_aligned_images():
    frames = pipeline.wait_for_frames()  #等待获取图像帧
    aligned_frames = align.process(frames)  #获取对齐帧
    aligned_depth_frame = aligned_frames.get_depth_frame()  #获取对齐帧中的depth帧
    color_frame = aligned_frames.get_color_frame()   #获取对齐帧中的color帧

    ############### 相机参数的获取 #######################
    intr = color_frame.profile.as_video_stream_profile().intrinsics   #获取相机内参
    depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics  #获取深度参数（像素坐标系转相机坐标系会用到）
    camera_parameters = {'fx': intr.fx, 'fy': intr.fy,
                         'ppx': intr.ppx, 'ppy': intr.ppy,
                         'height': intr.height, 'width': intr.width,
                         'depth_scale': profile.get_device().first_depth_sensor().get_depth_scale()
                         }
    # 保存内参到本地
    with open('./intr7insics.json', 'w') as fp:
        json.dump(camera_parameters, fp)
    #######################################################
    
    depth_image = np.asanyarray(aligned_depth_frame.get_data())  #深度图（默认16位）
    depth_image_8bit = cv2.convertScaleAbs(depth_image, alpha=0.03)  #深度图（8位）
    depth_image_3d = np.dstack((depth_image_8bit,depth_image_8bit,depth_image_8bit))  #3通道深度图
    color_image = np.asanyarray(color_frame.get_data())  # RGB图
    
    #返回相机内参、深度参数、彩色图、深度图、齐帧中的depth帧
    return intr, depth_intrin, color_image, depth_image, aligned_depth_frame

if __name__ == "__main__":
    while 1:
        intr, depth_intrin, rgb, depth, aligned_depth_frame = get_aligned_images() #获取对齐的图像与相机内参
        # 定义需要得到真实三维信息的像素点（x, y)，本例程以中心点为例
        print("============")
        print(aligned_depth_frame)
        x = 320  
        y = 240
        dis = aligned_depth_frame.get_distance(x, y)  #（x, y)点的真实深度值
        print("dis: ", dis)
        camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], dis)  #（x, y)点在相机坐标系下的真实值，为一个三维向量。其中camera_coordinate[2]仍为dis，camera_coordinate[0]和camera_coordinate[1]为相机坐标系下的xy真实距离。
        print(camera_coordinate)
        
        cv2.imshow('RGB image',rgb)  #显示彩色图像

        key = cv2.waitKey(1)
        # Press esc or 'q' to close the image window
        if key & 0xFF == ord('q') or key == 27:
            pipeline.stop()
            break
    cv2.destroyAllWindows()

效果

02 代码2

# -*- coding: utf-8 -*-
# encoding: utf-8print
import pyrealsense2 as rs
import numpy as np
import cv2
 
''' 
开启点云
'''
# Declare pointcloud object, for calculating pointclouds and texture mappings
pc = rs.pointcloud()
# We want the points object to be persistent so we can display the last cloud when a frame drops
points = rs.points()
 
''' 
设置
'''
 # 定义流程pipeline，创建一个管道
pipeline = rs.pipeline()

# 定义配置config
config = rs.config()
# 颜色和深度流的不同分辨率
# 配置depth流
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
# config.enable_stream(rs.stream.depth,  848, 480, rs.format.z16, 90)
# config.enable_stream(rs.stream.depth,  1280, 720, rs.format.z16, 30)

# 配置color流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)
# config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)
# config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
 
# streaming流开始
pipe_profile = pipeline.start(config) 

# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
# 获取深度传感器的深度标尺（参见rs - align示例进行说明）
depth_sensor = pipe_profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)      # ('Depth Scale is: ', 0.0010000000474974513)
 

# 创建对齐对象与color流对齐
# align_to 是计划对齐深度帧的流类型
align_to = rs.stream.color
# rs.align 执行深度帧与其他帧的对齐
align = rs.align(align_to)
 
 # Streaming循环
while True:
    ''' 
    获取图像帧与相机参数
    '''
    # 等待获取图像帧，获取颜色和深度的框架集
    frames = pipeline.wait_for_frames()         # frames.get_depth_frame（）是640x360深度图像
    # 获取对齐帧，将深度框与颜色框对齐
    aligned_frames = align.process(frames)
    # 获取对齐帧中的的depth帧
    aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame是640x480深度图像
    # 获取对齐帧中的的color帧
    aligned_color_frame = aligned_frames.get_color_frame()

    # 将images转为numpy arrays
    # RGB图
    img_color = np.asanyarray(aligned_color_frame.get_data())
    # 深度图（默认16位）
    img_depth = np.asanyarray(aligned_depth_frame.get_data())
 
    # Intrinsics & Extrinsics
    # 获取深度参数（像素坐标系转相机坐标系会用到）
    depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics
    # 获取相机内参
    color_intrin = aligned_color_frame.profile.as_video_stream_profile().intrinsics
    # 获取两摄像头之间的外参
    depth_to_color_extrin = aligned_depth_frame.profile.get_extrinsics_to(aligned_color_frame.profile)

    # 设置测试随机点
    # Map depth to color
    depth_pixel = [320, 240]   # Random pixel
    x = depth_pixel[0]
    y = depth_pixel[1]


    ''' 
    方法一：获取三维坐标(rs2_deproject_pixel_to_point方法)
    '''
    # rs2_deproject_pixel_to_point方法，2d转3d，获得三维坐标
    # camera_coordinate = rs.rs2_deproject_pixel_to_point(intrin=depth_intrin, pixel=[x, y], depth=dis)
    # depth_intrin 从上一步获取
    # x 像素点的x
    # y 像素点的y
    # dis 上一步计算的真实距离（输入的dis与输出的距离是一样的，改变的只是x与y
    dis = aligned_depth_frame.get_distance(x, y)         # 深度单位是m
    print ('===============方法1：二维映射三维函数=============')
    print ('depth: ',dis)       # ('depth: ', 2.502000093460083)
    
    camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, dis)
    print ('camera_coordinate: ',camera_coordinate)     # ('camera_coordinate: ', [-0.022640999406576157, -0.03151676058769226, 2.5230000019073486])

    color_point = rs.rs2_transform_point_to_point(depth_to_color_extrin, camera_coordinate)
    color_pixel = rs.rs2_project_point_to_pixel(color_intrin, color_point)
    print ('color_point: ',color_point)     # ('color_point: ', [-0.022640999406576157, -0.03151676058769226, 2.5230000019073486])
    print ('color_pixel: ',color_pixel)     # ('color_pixel: ', [320.0, 240.0])
 

  

    ''' 
    方法二：获取三维坐标（点云的另一种计算方法）
    '''
    print ('===============方法2：点云=============')
    # pc = rs.pointcloud()
    # frames = pipeline.wait_for_frames()
    # depth = frames.get_depth_frame()
    # color = frames.get_color_frame()
    # img_color = np.asanyarray(color_frame.get_data())
    # img_depth = np.asanyarray(depth_frame.get_data())
    pc.map_to(aligned_color_frame)
    points = pc.calculate(aligned_depth_frame)
    vtx = np.asanyarray(points.get_vertices())
    tex = np.asanyarray(points.get_texture_coordinates())

    npy_vtx = np.zeros((len(vtx), 3), float)
    for i in range(len(vtx)):
        npy_vtx[i][0] = np.float(vtx[i][0])
        npy_vtx[i][1] = np.float(vtx[i][1])
        npy_vtx[i][2] = np.float(vtx[i][2])

    npy_tex = np.zeros((len(tex), 3), float)
    for i in range(len(tex)):
        npy_tex[i][0] = np.float(tex[i][0])
        npy_tex[i][1] = np.float(tex[i][1])

    print ('        ----------计算方法1：先转浮点，再i查找-----------')
    print('npy_vtx_shape: ', npy_vtx.shape)     # (307200, 3)
    print('npy_tex_shape:  ', npy_tex.shape)     # (307200, 3)

    i = y*640+x

    print('pointcloud_output_vtx: ', npy_vtx[i])     # array([-0.02245255, -0.03125443,  2.50200009])
    print('pointcloud_output_tex: ', npy_tex[i])     # array([ 0.5,  0.5,  0. ])

 
    
    ''' 
    方法三：获取三维坐标(点云方法)
    '''
    pc.map_to(aligned_color_frame)
    points = pc.calculate(aligned_depth_frame)
    
    vtx = np.asanyarray(points.get_vertices())

    print ('        ----------计算方法2：先i查找，再转浮点-----------')
    print ('vtx_before_reshape: ', vtx.shape)        # 307200
    i = y * 640 + x
    print ('test_output_point', [np.float(vtx[i][0]),np.float(vtx[i][1]),np.float(vtx[i][2])])      # ('test_output_point', [-0.022542288526892662, -0.031379349529743195, 2.51200008392334])


    print ('        ----------计算方法3：reshape后数组查找-----------')
    vtx = np.reshape(vtx,(480, 640, -1))   
    print ('vtx_after_reshape: ', vtx.shape)       # (480, 640, 1)

    # 注意顺序是 y，x；而不是 x，y
    # print ('output_point', vtx[y][x])       # ('output_point', array([(-0.022641, -0.03151676,  2.523)], dtype=[('f0', '
    print ('output_point', vtx[y][x][0])        # ('output_point', (-0.022641, -0.03151676,  2.523))

    tex = np.asanyarray(points.get_texture_coordinates())


    ''' 
    显示图像并显示三维坐标信息（以方法3结果为例）
    '''
    # 点的位置
    cv2.circle(img_color, (320,240), 8, [255,0,255], thickness=-1)
    # 深度从img_depth[x, y]中获得
    cv2.putText(img_color,"Dis:"+str(img_depth[320,240])+" m", (40,40), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,255])
    cv2.putText(img_color,"X:"+str(np.float(vtx[y][x][0][0]))+" m", (80,80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,255])
    cv2.putText(img_color,"Y:"+str(np.float(vtx[y][x][0][1]))+" m", (80,120), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,255])
    cv2.putText(img_color,"Z:"+str(np.float(vtx[y][x][0][2]))+" m", (80,160), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,255])
    # 显示画面
    cv2.imshow('RealSence',img_color)
    key = cv2.waitKey(1)
    if key & 0xFF == ord('q'):
        break


pipeline.stop()
 

 

效果

03 代码3

# -*- 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流开始

# 创建对齐对象与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)       # 深度单位是mm
        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)

效果