准备工作包括安装驱动以及pyrealsense2,具体可参考上一篇:
这个在上一篇已经介绍了代码:
详细的注释可以参考上一篇,这一篇只放一下代码,并将关键代码做简要介绍。
realsense官方的api介绍可以参考:
realsense提供的api中,通过二维像素点获得三维坐标的共有 两种方式:
上述两种方法得到的结果完全相同。
将图像用opencv显示出来,计算的三维坐标结果也用cv2.putText()
显示在图像上。
其中需要注意的是,颜色顺序不是RGB,而是BGR,所以想改字体颜色的时候需要注意一下。
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)
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)
是关键代码
camera_coordinate = rs.rs2_deproject_pixel_to_point(intrin=depth_intrin, pixel=[x, y], depth=dis)
是关键代码
dis = aligned_depth_frame.get_distance(x, y) # 获取该像素点对应的深度
camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, dis) # 获取对应像素点的三维坐标
# -*- 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)
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
vtx = np.asanyarray(points.get_vertices())
reshape
中参数的顺序vtx = np.reshape(vtx,(480, 640, -1))
camera_coordinate = vtx[y][x][0]
depth_scale
不为1,放到实际的depth中是应该乘还是除,还是不需要处理呢?