realsense_camera.py

import pyrealsense2 as rs
import numpy as np
import cv2

class RealsenseCamera:
    def __init__(self):
        # Configure depth and color streams
        print("Loading Intel Realsense Camera")
        try:
            self.pipeline = rs.pipeline()
        except Exception as e:
            print("Error while starting the pipeline:" , str(e))
            #rs.utils.stop_all_pipelines()  # Stop any existing pipelines

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

        # Start streaming
        self.pipeline.start(config)
        align_to = rs.stream.color
        self.align = rs.align(align_to)


    def get_frame_stream(self):
        # Wait for a coherent pair of frames: depth and color
        frames = self.pipeline.wait_for_frames()
        aligned_frames = self.align.process(frames)
        depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()
        if not depth_frame or not color_frame:
            # If there is no frame, probably camera not connected, return False
            print("Error, impossible to get the frame, make sure that the Intel Realsense camera is correctly connected")
            return False, None, None
        
        # Apply filter to fill the Holes in the depth image
        spatial = rs.spatial_filter()
        spatial.set_option(rs.option.holes_fill, 3)
        filtered_depth = spatial.process(depth_frame)

        hole_filling = rs.hole_filling_filter()
        filled_depth = hole_filling.process(filtered_depth)

        
        
        # Create colormap to show the depth of the Objects
        colorizer = rs.colorizer()
        depth_colormap = np.asanyarray(colorizer.colorize(filled_depth).get_data())
        
        
        # Convert images to numpy arrays
        distance = depth_frame.get_distance(int(50),int(50))
        
        #print("distance", distance)
        depth_image = np.asanyarray(filled_depth.get_data())
        color_image = np.asanyarray(color_frame.get_data())
        
        #cv2.imshow("Colormap", depth_colormap)
        #cv2.imshow("depth img", depth_image)

        return True, color_image, depth_image
        
    
    def release(self, color_image, depth_image):
        self.pipeline.stop()
        print(depth_image)
        
        # 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.10), 2)
        
        # Stack both images horizontally
        
        images = np.hstack((color_image, depth_colormap))

# Create an instance of RealsenseCamera
camera = RealsenseCamera()

# Continuous frame display loop
while True:
    success, color_image, depth_image = camera.get_frame_stream()
    if not success:
        break
    cv2.imshow("Color Image", color_image)
    cv2.imshow("Depth Image", depth_image)
    
    key = cv2.waitKey(1)
    if key ==27:
        break
# Release the camera and OpenCV windows
camera.release(color_image, depth_image)



#cv2.waitKey(1)
cv2.destroyAllWindows()