python 直接读取深度图像、rgb图像、点云并可视化_python提取d435i录制的bag图像

以Intel的D435I相机为例，我们可以使用python代码，进行深度图像、RGB图像的读取和点云的可视化：

import pyrealsense2 as rs
import numpy as np
import cv2
 
import pcl
from pcl import pcl_visualization
 
 
 
cloud = pcl.PointCloud_PointXYZRGB()
def visual(visual_viewer, pt,color):
    length = len(pt)
    points = np.zeros((length, 4),dtype=np.float32)
    for i in range(length):
        points[i][0] = pt[i][0]
        points[i][1] = pt[i][1]
        points[i][2] = pt[i][2]
        points[i][3] = color[i][1]
 
    # pt = [list(i) for i in pt]
    # pt = np.array([*pt])
    # pt = np.hstack((pt, np.uint8(color)))
 
    cloud.from_array(points) #从array构建点云的方式
 
    
    visual_viewer.ShowColorCloud(cloud)
 
    v = True
    # while v:
    #     v = not (visual_viewer.WasStopped())
 
 
 
if __name__ == "__main__":
    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    # Start streaming
    pipeline.start(config)
    #深度图像向彩色对齐
    align_to_color=rs.align(rs.stream.color)
 
    pc = rs.pointcloud()
    points = rs.points()
 
    visual_viewer = pcl_visualization.CloudViewing()
 
    try:
        while True:
            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
            
            frames = align_to_color.process(frames)
 
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            if not depth_frame or not color_frame:
                continue
            # Convert images to numpy arrays
 
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())
 
            # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
 
            # Stack both images horizontally
            images = np.hstack((color_image, depth_colormap))
 
            # Get point data
            colorful=color_image.reshape(-1,3)
 
            pc.map_to(color_frame)
            points = pc.calculate(depth_frame)
 
            #获取顶点坐标
            vtx = np.asanyarray(points.get_vertices())
            visual(visual_viewer, vtx, colorful)
 
            # Show images
            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('RealSense', images)
            key = cv2.waitKey(1)
            # Press esc or 'q' to close the image window
            if key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                break
    finally:
        # Stop streaming
        pipeline.stop()

若以racord的*.bag文件进行读取和可视化，有两种方案：

方案一：采用intel提供的库 pyrealsense2，从文件读取数据即可：

pipeline = rs.pipeline()
    
# Create a config object
config = rs.config()
    
# Tell config that we will use a recorded device from file to be used by the pipeline through playback.
rs.config.enable_device_from_file(config, file_path, repeat_playback=False)
pipeline.start(config)

其他就和上面用相机读取的时候一致了。

方案二：采用rosbag读取*.bag数据，并用cv_bridge转换数据：

import os
import cv2
import numpy as np
import rosbag
from cv_bridge import CvBridge
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
 
import pcl
from pcl import pcl_visualization
import pyrealsense2 as rs
 
 
 
import numpy as np
 
 
# 相机参数
depth_camera_metrix = np.array([[920.523,0.0,641.936],   [0.0, 919.243,351.036],   [0,0,1]])
 
 
 
 
def depth2xyz(depth_map,depth_cam_matrix,flatten=False,depth_scale=1000):
    fx,fy = depth_cam_matrix[0,0],depth_cam_matrix[1,1]
    cx,cy = depth_cam_matrix[0,2],depth_cam_matrix[1,2]
    h,w=np.mgrid[0:depth_map.shape[0],0:depth_map.shape[1]]
    z=depth_map/depth_scale
    x=(w-cx)*z/fx
    y=(h-cy)*z/fy
    xyz=np.dstack((x,y,z)) if flatten==False else np.dstack((x,y,z)).reshape(-1,3)
    #xyz=cv2.rgbd.depthTo3d(depth_map,depth_cam_matrix)
    return xyz
 
 
def visual_pcl(visual, depth, color):
    xyz = depth2xyz(depth, depth_camera_metrix)
    # colorful = np.asanyarray(color.get_data())
    xyz = xyz.reshape(-1, 3)
    color=color.reshape(-1,3)
 
    # pc.map_to(color)
    # points = pc.calculate(depth)
    # vtx = np.asanyarray(points.get_vertices())
 
    cloud = pcl.PointCloud_PointXYZRGB()
    temp = np.zeros((len(xyz), 4), np.float32)
    length = len(xyz)
    for i in range(length):
        temp[i][0] = xyz[i][0] * 1000.0
        temp[i][1] = xyz[i][1] * 1000.0
        temp[i][2] = xyz[i][2] * 1000.0
        temp[i][3] = color[i][0]
    cloud.from_array(temp)
    visual.ShowColorCloud(cloud)
 
 
 
if __name__ == "__main__":
    file_path = "./stairs.bag"
    visual = pcl_visualization.CloudViewing()
    pc = rs.pointcloud()
    points = rs.points()
 
    bag = rosbag.Bag(file_path, "r")
    bag_data = bag.read_messages()
    info = bag.get_type_and_topic_info()
    print(info)
 
 
    bridge = CvBridge()
    depth = None; points_tp = None
    for topic, msg, t in bag_data:
        print(topic)
        if topic == "/cam_1/aligned_depth_to_color/image_raw/compressed":
            # # cv_img = bridge.imgmsg_to_cv2(msg, "16UC1")
            # bridge.encoding_to_dtype_with_channels("16UC1")
            depth = bridge.compressed_imgmsg_to_cv2(msg)
            print(depth.shape)
 
            # depth = pc2.read_points(msg)
            # points = np.asanyarray(depth)  # 如果bag中保存了该类格式，可用此种方法读取点
 
            cv2.imshow("Image window", depth)
            cv2.waitKey(3)
            print("here")
        elif topic == "cam_1/color/image_raw/compressed":
            cv_img = bridge.compressed_imgmsg_to_cv2(msg)
            colorful=cv_img.reshape(-1,3)
 
            # pc.map_to(cv_img)
            # points = pc.calculate(depth)
            # vtx = np.asanyarray(points.get_vertices())
 
            visual_pcl(visual, depth, colorful)
 
            cv2.imshow("Cam1", cv_img)
            cv2.waitKey(3)
 
    print("end")

示例图片：