背景:Ubuntu18.04 ROS Melodic 已安装配置好RealSense相关程序,链接D435i相机后,得到如下Rostopic:
/camera/color/image_raw # 彩色图像信息
/camera/depth/image_rect_raw # 深度图像信息
于是写一个python程序:
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import cv2
import time
import os
from imageio import imsave
def depth2Gray(im_depth):
"""
将深度图转至三通道8位灰度图
(h, w, 3)
"""
# 16位转8位
x_max = np.max(im_depth)
x_min = np.min(im_depth)
if x_max == x_min:
print('图像渲染出错 ...')
raise EOFError
k = 255 / (x_max - x_min)
b = 255 - k * x_max
ret = (im_depth * k + b).astype(np.uint8)
return ret
def depth2RGB(im_depth):
"""
将深度图转至三通道8位彩色图
先将值为0的点去除,然后转换为彩图,然后将值为0的点设为红色
(h, w, 3)
im_depth: 单位 mm或m
"""
im_depth = depth2Gray(im_depth)
im_color = cv2.applyColorMap(im_depth, cv2.COLORMAP_JET)
return im_color
def inpaint(img, missing_value=0):
"""
Inpaint missing values in depth image.
:param missing_value: Value to fill in teh depth image.
"""
img = cv2.copyMakeBorder(img, 1, 1, 1, 1, cv2.BORDER_DEFAULT)
mask = (img == missing_value).astype(np.uint8)
scale = np.abs(img).max()
img = img.astype(np.float32) / scale # Has to be float32, 64 not supported.
img = cv2.inpaint(img, mask, 1, cv2.INPAINT_NS)
# Back to original size and value range.
img = img[1:-1, 1:-1]
img = img * scale
return img
def color_callback(color_msg):
global color_image
color_image = bridge.imgmsg_to_cv2(color_msg, desired_encoding="bgr8")
def depth_callback(depth_msg):
global depth_image
depth_image = bridge.imgmsg_to_cv2(depth_msg, desired_encoding="passthrough")
def run():
rospy.init_node('realsense_capture_node')
bridge = CvBridge()
color_sub = rospy.Subscriber('/camera/color/image_raw', Image, color_callback)
depth_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, depth_callback)
# 创建保存图像的文件夹
save_path = os.path.join(os.getcwd(), "data", time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime()))
os.makedirs(save_path)
# 创建实时图像显示窗口
cv2.namedWindow("live", cv2.WINDOW_AUTOSIZE)
# cv2.namedWindow("save", cv2.WINDOW_AUTOSIZE)
saved_count = 0
try:
while not rospy.is_shutdown():
# 在回调函数中接收图像数据,这样我们就可以在主循环中使用它们
if color_image is not None and depth_image is not None:
color_image_copy = color_image.copy()
depth_image_copy = depth_image.copy()
# 可视化深度图像
depth_image_copy = inpaint(depth_image_copy)
depth_image_color = depth2RGB(depth_image_copy)
# 调整彩色图像的大小以匹配深度图像的分辨率
color_image_copy_resized = cv2.resize(color_image_copy, (depth_image_color.shape[1], depth_image_color.shape[0]))
cv2.imshow("live", np.hstack((color_image_copy_resized, depth_image_color)))
key = cv2.waitKey(30)
# 按 's' 键保存图像
if key & 0xFF == ord('s'):
# 调整彩色图像的大小以匹配深度图像的分辨率
color_image_copy_resized = cv2.resize(color_image_copy, (depth_image_copy.shape[1], depth_image_copy.shape[0]))
cv2.imwrite(os.path.join(save_path, "{:04d}r.png".format(saved_count)), color_image_copy_resized)
imsave(os.path.join(save_path, "{:04d}d.tiff".format(saved_count)), depth_image_copy)
saved_count += 1
cv2.imshow("live", np.hstack((color_image_copy_resized, depth_image_color)))# 原本是save
# 按 'q' 键或按下 Esc 键退出程序
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
except rospy.ROSInterruptException:
pass
if __name__ == '__main__':
color_image = None
depth_image = None
bridge = CvBridge()
run()
运行程序后,会出现一个窗口,里面有两个画面,按下s键就会保存一套照片,按下q键即退出