-
Notifications
You must be signed in to change notification settings - Fork 0
/
realsense_d435i_camera.py
92 lines (75 loc) · 4.04 KB
/
realsense_d435i_camera.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
"""
Credits to Intel RealSense Developer Team for providing the tutorial & guides
.. Code Author: Aldy Helnawan <aldyhelnawan003@gmail.com>
.. <aldyhelnawan.18071@mhs.its.ac.id>
.. <github: aldyhelnawan>
"""
""" Import the Library"""
import pyrealsense2 as rs
import cv2
import numpy as np
from realsense_d435i_config import hres, vres, fps, ir_1, ir_2, fill_stereo_camera, color_mode, hole_fill_config
########################################################################################################################
class IntelRealSenseCamera:
# Initialize the camera
def __init__(self):
print("Loading Intel RealSense Camera...")
self.pipeline = rs.pipeline()
config = rs.config()
# Configure the pipeline to stream the depth stream
config.enable_stream(rs.stream.color, hres, vres, rs.format.rgb8, fps)
config.enable_stream(rs.stream.depth, hres, vres, rs.format.z16, fps)
if ir_1 == 1:
config.enable_stream(rs.stream.infrared, 1, hres, vres, rs.format.y8, fps)
if ir_2 == 1:
config.enable_stream(rs.stream.infrared, 2, hres, vres, rs.format.y8, fps)
# Start streaming from file
self.pipeline.start(config)
align_rgb_depth = rs.stream.color
self.align_depth = align_rgb_depth
align_rgb_infrared = rs.stream.color
self.align_infrared = align_rgb_infrared
# Get the Camera to Stream
def get_frame_stream(self):
# Wait for a coherent pair of frames: color, depth, and infrared
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
if (ir_1 == 1) & (ir_2 == 0):
infrared_frame = frames.get_infrared_frame(1)
if (ir_1 == 0) & (ir_2 == 1):
infrared_frame = frames.get_infrared_frame(2)
if not color_frame or not depth_frame or not infrared_frame:
# If there is no frame, probably camera not connected, return False
print("Error!. No Camera Detected!. Make Sure That The Intel Realsense Camera Is Correctly Connected")
return False, None, None, None, None
# Set the number inside () to define the visualized depth color:
colorizer = rs.colorizer(color_mode)
# Setup the depth hole filler
if fill_stereo_camera == "y":
# Apply filter to fill the Holes in the depth image
spatial = rs.spatial_filter()
spatial.set_option(rs.option.holes_fill, hole_fill_config)
filtered_depth = spatial.process(depth_frame)
filtered_infrared = spatial.process(infrared_frame)
# Fill the Holes in the Stereo Sensor (Depth Sensor and Infrared Sensor)
hole_filling = rs.hole_filling_filter()
filled_depth = hole_filling.process(filtered_depth)
filled_infrared = hole_filling.process(filtered_infrared)
# Create colormap to show the visual depth of the Objects
depth_color_frame = colorizer.colorize(filled_depth)
filled_infrared = filled_infrared
if fill_stereo_camera == "n":
depth_color_frame = colorizer.colorize(depth_frame)
filled_infrared = infrared_frame
# Convert depth_frame to numpy array to render image in opencv
color_image = np.asanyarray(color_frame.get_data())
depth_color_image = np.asanyarray(depth_color_frame.get_data())
infrared_image = np.asanyarray(filled_infrared.get_data())
# Special for color_image, convert bgr (opencv) to rgb (opencv)
# And convert the Infrared Sensor to a 3D Matrix as the numpy can't process the 2 Channel Matrix
color_rgb = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
infrared_3d = cv2.cvtColor(infrared_image, cv2.COLOR_GRAY2RGB)
return True, color_rgb, depth_color_image, infrared_3d
def release(self):
self.pipeline.stop()