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

最新推荐文章于 2024-06-24 21:35:34 发布
最新推荐文章于 2024-06-24 21:35:34 发布
阅读量8.6k 收藏 208
点赞数 18
文章标签： ros ubuntu realsense
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/gyxx1998/article/details/121635086
版权

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中是应该乘还是除，还是不需要处理呢？
冰激凌啊
关注
专栏目录
RealSense D435 输出世界坐标系的csv文件
linf233的博客
07-27 8056
第一篇博客，最近在实习，一直在做机器视觉的研究。今天简单的介绍一个RealsenseD435的应用。 RealsenseD435是一款深度摄像头，可以快读的输出深度图像。首先了解一下什么事世界坐标系，相机坐标系，和图像坐标系，这里给大家推荐一篇博客。https://blog.csdn.net/waeceo/article/details/50580607 在Viewer里我们可以很轻松地得到相...
ROS/RealSenseD435i：已知二维图像中一点获取其深度图像中的三维坐标（python实现）
Vegetable__的博客
12-17 6393
一、思路 深度相机会发布一个将深度图像与彩色图像对齐的topic：/camera/aligned_depth_to_color/image_raw（深度流向彩色流对齐），利用该topic可以获取对应的三维坐标，该坐标还需要经过深度相机的内参矩阵变换。 二、变换原理 三、程度实现 3.1获取相机的内参矩阵 import pyrealsense2 as rs import numpy as np import cv2 import json pipeline = rs.pipeline() # 定义流程p
ROS:二维坐标映射到三维坐标上(彩色与深度图像匹配)（基于深度相机D415）
07-31
对应博文：https://blog.csdn.net/qq_23670601/article/details/97970892 - 最近在用D435做物体识别抓取的时候发现二维坐标到三维坐标没有一个直接能用的从二维像素点坐标转换到三维坐标的ROS节点，自己之前写过的kinect V2的坐标映射的通用性太差，所以这次写了一个节点，利用深度相机ROS节点发布的深度与彩色对齐的图像话题和图像信息内外参话题，将二维坐标映射到三维坐标系。 - 我挂上来的这个节点是将鼠标指向的二维坐标转换到三维坐标系下，并发布话题可视化到了rviz中，因为我自己的物体识别发布的像素坐标话题是一个自己写的消息文件，这次就不放出来了，需要用的把我这里的鼠标反馈的坐标改成你自己的坐标就行了。
realsense d435i获取某像素点三维坐标（计算深度和点云两种方法）
gyxx1998的博客
11-30 2万+
说明： 代码1 使用aligned_depth_frame.get_distance()与rs.rs2_deproject_pixel_to_point()获得三维坐标 opencv显示画面 并将内参保存到了脚本文件目录下的json格式 代码2 opencv显示画面，并将坐标信息标注在画面上 这里用的方法奇奇怪怪，我有点看不懂，既使用了rs.rs2_deproject_pixel_to_point()，又使用的点云，这也是我最后选择修改成自己代码的原因 代码3 opencv显示画面，并将坐标
Realsense D435i 通过YOLOv5、YOLOv8输出目标三维坐标
Zeng999212的博客
10-19 9747
使用realsense d435i 深度相机，通过yolov5、YOLOv8获取目标的三维坐标
相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标
weixin_30436891的博客
01-22 1324
关键词：相机位姿估计，单目尺寸测量，环境探知 用途：基于相机的环境测量，SLAM，单目尺寸测量 文章类型：原理说明、Demo展示 @Author：VShawn @Date:2016-11-28 @Lab: CvLab202@CSU 目录 《相机位姿估计0：基本原理之如何解PNP问题》 《相机位姿估计1：根据四个特征点估计相机姿态》 《相机位姿估计1_1：OpenCV:...
使用深度相机D435i+YOLOv8实现物体三维坐标实时显示
最新发布
doudou2weiwei的博客
06-24 3142
下列指令为获取相机内参指令，输入此指令前需要获得相机的深度帧和彩色帧数据。显示结果为：其中：640x480是像素宽度x像素高度；p[322.02 236.768]为主点（镜头光轴与图像平面的交点）的坐标；f[391.288 391.288]为焦距参数；Brown Conrady [0 0 0 0 0]分别表示畸变模型与畸变系数。
Yolov5与Intel Realsense D435i三维距离测量源码及使用指南
资源摘要信息: "本资源是一套基于Yolov5目标检测模型和Intel Realsense D435i深度相机的三维距离测量系统，它允许用户识别和测量场景中多个物体之间的空间距离。该系统可以运行在支持Yolov5的环境中，并且需要安装...
EPNP根据像素坐标求解实际三维世界坐标python实现
06-29
EPNP算法利用这种投影关系，通过四个或更多的二维像素点对应于三维空间中的四个点，来求解这些三维点的实际坐标。关键在于，它采用了一种优化策略，即最小化投影误差，以求得最接近观测值的三维坐标。 在Python中，...
Matlab: ply三维点云转二维深度图(简化)
07-02
在本文中，我们将深入探讨如何使用Matlab将PLY格式的三维点云数据转换为二维深度图。PLY（Polygon File Format）是一种常见的3D模型文件格式，通常用于存储三维几何信息，包括点云数据。而深度图则是一个表示场景中...
D435i手眼标定参数输出
08-07
基于D435I相机的手眼标定，包括内参获取，外参估计等
Intel Realsense D435 如何通过图像识别出的比例点（x, y）获得实际点相对于摄像机原点的三维坐标（x, y, z）?
Dontla的博客
10-12 4329
点云
Intel Realsense D435 通过识别目标的像素坐标和深度值（使用内参intrinsics）获取目标点的真实坐标
Dontla的博客
10-21 1万+
Intel Realsense D435 通过识别目标的像素坐标和深度值（使用内参intrinsics）获取目标点的真实坐标基本获取内参`intrinsics`代码实操代码1（在`tensorflow-yolov3`中获取内参）实操代码2（在`tensorflow-yolov3` `draw_bbox()`函数中实现坐标转换操作） 基本获取内参intrinsics代码 import pyreals...
Ubuntu: 查看图像像素坐标
雪剑封心
01-14 2105
介绍UBuntu下查看图像像素坐标的软件mtPoint 安装命令: sudo apt-get install mtpaint
如何由图像中某点的二维坐标获得其在空间中的三维坐标
热门推荐
qq_33221533的博客
07-22 3万+
摄像机标定------标定原理 一：摄像机成像原理         摄像机成像模型一般有三种：透镜投影模型（小孔摄像机模型）、正交投影模型和透视投影模型 光学中最基本的高斯成像公式：1/u + 1/v = 1/f       (式一) 原理图如下： 二：认识三个坐标系                                                      ...
Ubutntu下使用realsense d435i（三）：使用yolo v5测量目标物中心点三维坐标
gyxx1998的博客
12-13 1万+
使用realsense和yolo识别物体，并活动物体的中心点坐标
D435i学习
qq_51185963的博客
05-09 1237
启动之前要在/vins-fusion/src/VINS-Fusion/config/realsense_d435i/下，三个配置文件的left、right、realsense_stereo_imu_config.yaml修改为自己的参数。end VIBA 1、start VIBA 2、end VIBA 2全部结束了之后，便可以开始建图。启动ORB_SLAM3后，要将相机平移一个“8”，在两三圈之后，等待start VIBA 1、启动vins配置的rviz。
Realsense D435i 深度相机获取距离角度坐标，C2D，D2C，相机标定，坐标系转换，ROS2
2203_76027118的博客
12-08 1610
懒的写注释和解析了，可读性和可移植性也不算很高，有什么不理解的地方可以和我交流，看见我会回复的。是C2D得到大概的目标点在深度像素坐标系下的坐标，然后对这一圈的深度进行处理。
利用python将一个二维坐标点转成相机三维坐标点
05-24
要将二维坐标点转换为相机三维坐标点，我们需要知道相机的内部参数和外部参数。 内部参数包括焦距、主点、图像尺寸等信息，可以通过相机标定获得。外部参数包括相机的位置和朝向，可以通过计算机视觉中的相机位姿估计方法获得。 假设已知相机的内部参数以及相机在世界坐标系下的位姿，我们可以通过以下步骤将二维坐标点转换为相机三维坐标点： 1. 将二维坐标点归一化，即将像素坐标 $(u,v)$ 转换为归一化坐标 $(x,y)$，其中 $x=(u-c_x)/f_x$，$y=(v-c_y)/f_y$，$c_x$ 和 $c_y$ 分别为主点的横纵坐标，$f_x$ 和 $f_y$ 分别为相机焦距在横纵方向上的分量。 2. 将归一化坐标 $(x,y)$ 转换为相机坐标系下的坐标 $(X_c,Y_c,Z_c)$，其中 $Z_c$ 为相机到目标物体的距离。 $$ \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} Z_c $$ 3. 将相机坐标系下的坐标 $(X_c,Y_c,Z_c)$ 转换为世界坐标系下的坐标 $(X_w,Y_w,Z_w)$，其中 $(X_w,Y_w,Z_w)$ 为目标物体在世界坐标系下的坐标。 $$ \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} = \begin{bmatrix} R_{11} & R_{12} & R_{13} & T_x \\ R_{21} & R_{22} & R_{23} & T_y \\ R_{31} & R_{32} & R_{33} & T_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix} $$ 其中 $R$ 为相机的旋转矩阵，$T$ 为相机的平移向量。 因此，将二维坐标点 $(u,v)$ 转换为相机三维坐标点 $(X_w,Y_w,Z_w)$ 的完整代码如下： ```python import numpy as np # 相机内部参数 fx = 100 # 焦距 fy = 100 cx = 320 # 主点 cy = 240 # 相机外部参数 R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # 旋转矩阵 T = np.array([0, 0, 0]) # 平移向量 # 二维坐标点 u = 200 v = 150 # 归一化坐标 x = (u - cx) / fx y = (v - cy) / fy # 相机坐标系下的坐标 Z_c = 1 # 假设相机到目标物体的距离为1 X_c = x * Z_c Y_c = y * Z_c # 世界坐标系下的坐标 P_c = np.array([X_c, Y_c, Z_c, 1]).reshape(4, 1) P_w = np.dot(np.hstack((R, T.reshape(3, 1))), P_c) X_w, Y_w, Z_w = P_w[0, 0], P_w[1, 0], P_w[2, 0] print('二维坐标点 ({}, {}) 转换为相机三维坐标点 ({}, {}, {})'.format(u, v, X_w, Y_w, Z_w)) ``` 需要注意的是，上述代码中默认相机的旋转矩阵为单位矩阵，即相机的朝向与世界坐标系重合。如果实际情况中相机的朝向不同，需要根据实际情况修改旋转矩阵 $R$ 的值。
评论 14
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值