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

最新推荐文章于 2025-06-05 09:09:13 发布

原创

最新推荐文章于 2025-06-05 09:09:13 发布 · 3w 阅读

374 ·

CC 4.0 BY-SA版权

文章标签：

#python #ubuntu #realsense

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

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