intel realsense 相机 python对深度图预处理和对齐源码

import cv2
import numpy as np
import pyrealsense2 as rs

frames = []
i = 0
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.depth,640,480,rs.format.z16,30)
cfg.enable_stream(rs.stream.color,640,480,rs.format.bgr8,30)
profile = pipe.start(cfg)

align_to = rs.stream.color
align = rs.align(align_to)

try:
    while True:
        frame = pipe.wait_for_frames()
        print(frame)
        depth_frame = frame.get_depth_frame()
        print(depth_frame)
        color_frame = frame.get_color_frame()
        colorizer = rs.colorizer()
        d = np.asanyarray(colorizer.colorize(depth_frame).get_data())
        #print(d)
        c = np.asanyarray(colorizer.colorize(color_frame).get_data())
        cv2.imshow('color_frame',c)
        cv2.imshow('depth_frame',d)

        #colorizer = rs.colorizer()
        hole_filling = rs.hole_filling_filter()
        filled_depth = hole_filling.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(filled_depth).get_data())
        cv2.imshow('filled depth',colorized_depth)
        #print(colorized_depth)



        #对齐
        aligned_frames = align.process(frame)
        print(aligned_frames)
        aligned_depth_frame = aligned_frames.get_depth_frame()
        print(aligned_depth_frame)
        aligned_color_frame = aligned_frames.get_color_frame()
        d1 = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
        #print(d1)
        c1 = np.asanyarray(colorizer.colorize(aligned_color_frame).get_data())
        cv2.imshow('depth_frame1',d1)
        #cv2.imshow('color_frame1',c1)        




        print('capture success')
        if cv2.waitKey(10)&0xff == ord('q'):
            break

        #二、抽取Decimation
        '''colorizer = rs.colorizer()
        decimation = rs.decimation_filter()
        decimationed_depth = decimation.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(decimationed_depth).get_data())
        cv2.imshow('decimationed depth',colorized_depth)

        #可以设置参数类似于迭代次数
        decimation.set_option(rs.option.filter_magnitude,4)
        decimationed_depth = decimation.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(decimationed_depth).get_data())
        cv2.imshow('decimationed4 depth',colorized_depth)

        #三、空间滤波器（spatial filter）
        colorizer = rs.colorizer()
        spatial = rs.spatial_filter()
        filtered_depth = spatial.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(filtered_depth).get_data())
        cv2.imshow('filtered depth',colorized_depth)

        # 可以设置相关的参数
        spatial.set_option(rs.option.filter_magnitude,5)
        spatial.set_option(rs.option.filter_smooth_alpha,1)
        spatial.set_option(rs.option.filter_smooth_delta,50)
        filtered_depth = spatial.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(filtered_depth).get_data())
        cv2.imshow('filtered1 depth',colorized_depth)

        spatial.set_option(rs.option.holes_fill,1)
        filtered_depth = spatial.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(filtered_depth).get_data())
        cv2.imshow('fill hole',colorized_depth)

        #四、时间滤波器（Temporal Filter）
        colorizer = rs.colorizer()
        frames.append(depth_frame)
        i += 1
        if i == 10:
            i = 0
            temporal = rs.temporal_filter()
            print(len(frames))
            for x in range(10):
                temp_filtered = temporal.process(frames[x])
            frames = []
            colorized_depth = np.asanyarray(colorizer.colorize(temp_filtered).get_data())
            cv2.imshow('temporal depth',colorized_depth)
        

        #五、孔填充（Hole Filling）
        colorizer = rs.colorizer()
        hole_filling = rs.hole_filling_filter()
        filled_depth = hole_filling.process(depth_frame)
        colorized_depth = np.asanyarray(colorizer.colorize(filled_depth).get_data())
        cv2.imshow('filled depth',colorized_depth)
        
        #六、复合多个滤波器
        colorizer = rs.colorizer()        
        depth_to_disparity = rs.disparity_transform(True)
        disparity_to_depth = rs.disparity_transform(False)

        decimation = rs.decimation_filter()
        spatial = rs.spatial_filter()
        temporal = rs.temporal_filter()
        hole_filling = rs.hole_filling_filter()

        frames.append(depth_frame)
        i += 1
        if i == 5:
            i = 0
            for x in range(5):
                frame = frames[x]
                frame = decimation.process(frame)
                frame = depth_to_disparity.process(frame)
                frame = spatial.process(frame)
                frame = temporal.process(frame)
                frame = disparity_to_depth.process(frame)
                frame = hole_filling.process(frame)
            frames = []
            colorized_depth = np.asanyarray(colorizer.colorize(frame).get_data())
            cv2.imshow('more depth filter',colorized_depth)'''
        

        #六、复合多个滤波器
        colorizer = rs.colorizer()        
        depth_to_disparity = rs.disparity_transform(True)
        disparity_to_depth = rs.disparity_transform(False)

        decimation = rs.decimation_filter()
        spatial = rs.spatial_filter()
        temporal = rs.temporal_filter()
        hole_filling = rs.hole_filling_filter()


        frames.append(depth_frame)
        i += 1
        if i == 10:
            i = 0
            for x in range(10):
                frame = frames[x]
                frame = decimation.process(frame)
                frame = depth_to_disparity.process(frame)
                frame = spatial.process(frame)
                frame = temporal.process(frame)
                frame = disparity_to_depth.process(frame)
                frame = hole_filling.process(frame)
            frames = []
            colorized_depth = np.asanyarray(colorizer.colorize(frame).get_data())
            #cv2.imshow('more depth filter',colorized_depth)


finally:
    pipe.stop()