通过realsense d435进行三维点云重建

河北一帆

已于 2024-07-14 16:06:31 修改

阅读量1.3k

点赞数 2

文章标签：机器人 python

于 2024-01-28 17:52:23 首次发布

本文链接：https://blog.csdn.net/qq_41816368/article/details/135898192

版权

本文介绍了如何使用Python和OpenCV在RealSense相机中获取深度数据，通过get_non_zeros函数筛选有效像素，然后利用rebuid_3d函数进行坐标转换。同时展示了如何将深度图转换为点云并发布到ROS系统中。

摘要由CSDN通过智能技术生成

import numpy as np


def get_non_zeros(array_2d):
	non_zero_mask = array_2d != 0
	row_indices, col_indices = np.nonzero(array_2d)
	non_zero_elements = array_2d[array_2d != 0]
	pixels = np.vstack((row_indices, col_indices, non_zero_elements)).T
	# 仅选取小于10米的点
	pixels = pixels[pixels[:, 2] <= 1000]
	
	return pixels, non_zero_mask


def rebuid_3d(pixels, fx, fy, cx, cy):
	# change unit to m
	pixels[:, 2] = pixels[:, 2] / 1000.0
	pixels[:, 0] *= pixels[:, 2]
	pixels[:, 1] *= pixels[:, 2]
	
	K = np.array([[fx, 0, cx],
	              [0, fy, cy],
	              [0, 0,  1]])
	
	pixels = pixels.T
	
	coord = np.linalg.inv(K) @ pixels
	
	# slam coord to rviz coord
	x_angle = 90
	Rx = np.array([[1, 0, 0],
	               [0, np.cos(x_angle*np.pi/180), -np.sin(x_angle*np.pi/180)],
	               [0, np.sin(x_angle*np.pi/180), np.cos(x_angle*np.pi/180)]])
	
	y_angle = -90
	Ry = np.array([[np.cos(y_angle*np.pi/180), 0, -np.sin(y_angle*np.pi/180)],
	               [0, 1, 0],
	               [np.sin(y_angle*np.pi/180), 0, np.cos(y_angle*np.pi/180)]])
	
	z_angle = 90
	Rz = np.array([[np.cos(z_angle*np.pi/180), -np.sin(z_angle*np.pi/180), 0],
	               [np.sin(z_angle*np.pi/180), np.cos(z_angle*np.pi/180), 0],
	               [0, 0, 1]])
	
	# change to rviz coord
	coord = Ry @ coord
	coord = coord.T
	coord[:, 1] *= -1
	
	return coord

import pyrealsense2 as rs
import numpy as np
import cv2
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import matplotlib.pyplot as plt
from utils import get_non_zeros, rebuid_3d


pipeline = rs.pipeline()
config = rs.config()

# config.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, 6)
# config.enable_stream(rs.stream.infrared, 2, 640, 480, rs.format.y8, 6)

config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

pipeline.start(config)

# ros topic publisher
rospy.init_node('realsense_node')
image_pub = rospy.Publisher('/camera/image_raw', Image, queue_size=10)
pointcloud_pub = rospy.Publisher('/camera/pointcloud', PointCloud2, queue_size=10)
bridge = CvBridge()

try:
	while True:

		# Wait for a coherent pair of frames: depth and color
		frames = pipeline.wait_for_frames()
		
		color_frame = frames.get_color_frame()
		intrinsics = color_frame.get_profile().as_video_stream_profile().get_intrinsics()
		fx = intrinsics.fx
		fy = intrinsics.fy
		cx = intrinsics.width / 2
		cy = intrinsics.height / 2
	
		depth_frame = frames.get_depth_frame()
		
		# ir1_frame = frames.get_infrared_frame(1) # Left IR Camera, it allows 0, 1 or no input
		# ir2_frame = frames.get_infrared_frame(2) # Right IR camera

		# Convert images to numpy arrays
		depth_image = np.asanyarray(depth_frame.get_data())     # unit-mm
		color_image = np.asanyarray(color_frame.get_data())
		
		pixels, _ = get_non_zeros(depth_image)
		pixels = pixels.astype(float)
		coord = rebuid_3d(pixels, fx, fy, cx, cy)
		
		header = rospy.Header()
		header.stamp = rospy.Time.now()
		header.frame_id = 'map'  # 设置帧ID
		cloud_msg = point_cloud2.create_cloud_xyz32(header, coord)
		pointcloud_pub.publish(cloud_msg)
	
		image_msg = bridge.cv2_to_imgmsg(color_image, encoding='bgr8')
		image_msg.header.stamp = rospy.Time.now()
		image_pub.publish(image_msg)

		depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
		images = np.hstack((color_image, depth_colormap))

		cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
		cv2.imshow('RealSense', images)
		cv2.waitKey(1)

finally:
	pipeline.stop()

测试图片发布

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2


def image_callback(msg):
    try:
        # 将ROS Image消息转换为OpenCV格式
        bridge = CvBridge()
        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
        # 显示图像
        cv2.imshow("Received Image", cv_image)
        cv2.waitKey(1)

    except Exception as e:
        rospy.logerr(e)


def image_subscriber():
    rospy.init_node('image_subscriber', anonymous=True)
    rospy.Subscriber("/camera/image_raw", Image, image_callback)
    rospy.spin()


if __name__ == '__main__':
    try:
        # 启动图像订阅者节点
        image_subscriber()
    except rospy.ROSInterruptException:
        pass

import pyrealsense2 as rs
import cv2
import numpy as np
import time
import os
import sys
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
from load_model import load_model, inference
from sensor_msgs.msg import Image
from cv_bridge import CvBridge


if __name__ == '__main__':
	rospy.set_param('/use_sim_time', False)
	rospy.init_node('realsense_node')
	pointcloud_pub = rospy.Publisher('/pointcloud', PointCloud2, queue_size=10)

	image_left_pub = rospy.Publisher('/camera/image/bgr', Image, queue_size=10)
	image_right_pub = rospy.Publisher('/camera/image/depth', Image, queue_size=10)
	bridge = CvBridge()

	orb = cv2.ORB_create()

	dam = load_model()

	# 确定图像的输入分辨率与帧率
	resolution_width = 640  # pixels
	resolution_height = 480  # pixels
	frame_rate = 15  # fps

	# 注册数据流，并对其图像
	align = rs.align(rs.stream.color)
	rs_config = rs.config()
	rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
	rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)
	# rs_config.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, frame_rate)
	# rs_config.enable_stream(rs.stream.infrared, 2, 640, 480, rs.format.y8, frame_rate)
	# check相机是不是进来了
	connect_device = []
	for d in rs.context().devices:
		print('Found device: ',
			  d.get_info(rs.camera_info.name), ' ',
			  d.get_info(rs.camera_info.serial_number))
		if d.get_info(rs.camera_info.name).lower() != 'platform camera':
			connect_device.append(d.get_info(rs.camera_info.serial_number))

	# if len(connect_device) < 2:
	# 	print('Registrition needs two camera connected.But got one.')
	# 	exit()

	# 确认相机并获取相机的内部参数
	pipeline1 = rs.pipeline()
	rs_config.enable_device(connect_device[0])
	# pipeline_profile1 = pipeline1.start(rs_config)
	pipeline1.start(rs_config)

	# pipeline2 = rs.pipeline()
	# rs_config.enable_device(connect_device[1])
	# # pipeline_profile2 = pipeline2.start(rs_config)
	# pipeline2.start(rs_config)

	while True:
		# 等待数据进来
		frames1 = pipeline1.wait_for_frames()
		# frames2 = pipeline2.wait_for_frames()

		# 将进来的RGBD数据对齐
		aligned_frames1 = align.process(frames1)
		# aligned_frames2 = align.process(frames2)

		# 将对其的RGB—D图取出来
		color_frame1 = aligned_frames1.get_color_frame()
		depth_frame1 = aligned_frames1.get_depth_frame()
		# color_frame2 = aligned_frames2.get_color_frame()
		# depth_frame2 = aligned_frames2.get_depth_frame()

		intrinsics = frames1.get_profile().as_video_stream_profile().get_intrinsics()
		fx = intrinsics.fx
		fy = intrinsics.fy
		cx = intrinsics.width / 2
		cy = intrinsics.height / 2
		print(fx, fy, cx, cy)

		# 数组化数据便于处理
		ir_frame_left1 = frames1.get_infrared_frame(1)
		ir_frame_right1 = frames1.get_infrared_frame(2)
		# ir_frame_left2 = frames2.get_infrared_frame(1)
		# ir_frame_right2 = frames2.get_infrared_frame(2)

		color_image1 = np.asanyarray(color_frame1.get_data())
		depth_image1 = np.asanyarray(depth_frame1.get_data())
		# ir_left_image1 = np.asanyarray(ir_frame_left1.get_data())
		# ir_right_image1 = np.asanyarray(ir_frame_right1.get_data())

		# color_image2 = np.asanyarray(color_frame2.get_data())
		# depth_image2 = np.asanyarray(depth_frame2.get_data())
		# ir_left_image2 = np.asanyarray(ir_frame_left2.get_data())
		# ir_right_image2 = np.asanyarray(ir_frame_right2.get_data())

		# depth_colormap1 = cv2.applyColorMap(cv2.convertScaleAbs(depth_image1, alpha=0.03), cv2.COLORMAP_JET)
		# images1 = np.hstack((color_image1, depth_colormap1))

		# depth_colormap2 = cv2.applyColorMap(cv2.convertScaleAbs(depth_image2, alpha=0.03), cv2.COLORMAP_JET)
		# images2 = np.hstack((color_image2, depth_colormap2))


		# 显示结果
		# cv2.imshow('Stereo Matching Result', color_image1)
		# cv2.waitKey(1)

		image_left_msg = bridge.cv2_to_imgmsg(color_image1, encoding="bgr8")
		image_left_msg.header.stamp = rospy.Time.now()  # 更新时间戳
		image_left_pub.publish(image_left_msg)

		image_right_msg = bridge.cv2_to_imgmsg(depth_image1, encoding="16UC1")		# mono8 for stereo
		image_right_msg.header.stamp = rospy.Time.now()  # 更新时间戳
		image_right_pub.publish(image_right_msg)

		timestamp = rospy.Time.now()
		save_path1 = '/home/robotics/gazebo_calib/src/env_pkg/data/realsense_calib/left/' + str(timestamp) + '.png'
		save_path2 = '/home/robotics/gazebo_calib/src/env_pkg/data/realsense_calib/right/' + str(timestamp) + '.png'

		# cv2.imwrite(save_path1, ir_left_image1)
		# cv2.imwrite(save_path2, ir_right_image1)

		print("images published {}".format(rospy.Time.now()))