-
Notifications
You must be signed in to change notification settings - Fork 3
/
d405_helpers.py
141 lines (116 loc) · 4.63 KB
/
d405_helpers.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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
import pyrealsense2 as rs
import numpy as np
import cv2
from d405_helpers_without_pyrealsense import *
exposure_keywords = ['low', 'medium', 'auto']
exposure_range = [0, 500000]
def exposure_argument_is_valid(value):
if value in exposure_keywords:
return True
is_string = isinstance(value, str)
is_int = isinstance(value, int)
int_value = exposure_range[0] - 10
if is_string:
if not value.isdigit():
return False
else:
int_value = int(value)
elif is_int:
int_value = value
if (int_value >= exposure_range[0]) or (int_value <= exposure_range[1]):
return True
return False
def check_exposure_value(value):
if not exposure_argument_is_valid(value):
raise ValueError(f'The provided exposure setting, {value}, is not a valide keyword, {exposure_keywords}, or is outside of the allowed numeric range, {exposure_range}.')
def prepare_exposure_value(value):
check_exposure_value(value)
if value in exposure_keywords:
return value
is_int = isinstance(value, int)
if is_int:
return value
is_string = isinstance(value, str)
if is_string:
return int(value)
return None
def start_d405(exposure):
camera_info = [{'name': device.get_info(rs.camera_info.name),
'serial_number': device.get_info(rs.camera_info.serial_number)}
for device
in rs.context().devices]
exposure = prepare_exposure_value(exposure)
print('All cameras that were found:')
print(camera_info)
print()
d405_info = None
for info in camera_info:
if info['name'].endswith('D405'):
d405_info = info
if d405_info is None:
print('D405 camera not found')
print('Exiting')
exit()
else:
print('D405 found:')
print(d405_info)
print()
pipeline = rs.pipeline()
config = rs.config()
config.enable_device(d405_info['serial_number'])
# 1280 x 720, 5 fps
# 848 x 480, 10 fps
# 640 x 480, 30 fps
#width, height, fps = 1280, 720, 5
#width, height, fps = 848, 480, 10
#width, height, fps = 640, 480, 30
width, height, fps = 640, 480, 15
config.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps)
config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps)
profile = pipeline.start(config)
if exposure == 'auto':
# Use autoexposre
stereo_sensor = pipeline.get_active_profile().get_device().query_sensors()[0]
stereo_sensor.set_option(rs.option.enable_auto_exposure, True)
else:
default_exposure = 33000
if exposure == 'low':
exposure_value = int(default_exposure/3.0)
elif exposure == 'medium':
exposure_value = 30000
else:
exposure_value = int(exposure)
stereo_sensor = pipeline.get_active_profile().get_device().query_sensors()[0]
stereo_sensor.set_option(rs.option.exposure, exposure_value)
return pipeline, profile
def get_camera_info(frame):
intrinsics = rs.video_stream_profile(frame.profile).get_intrinsics()
# from Intel's documentation
# https://intelrealsense.github.io/librealsense/python_docs/_generated/pyrealsense2.intrinsics.html#pyrealsense2.intrinsics
# "
# coeffs Distortion coefficients
# fx Focal length of the image plane, as a multiple of pixel width
# fy Focal length of the image plane, as a multiple of pixel height
# height Height of the image in pixels
# model Distortion model of the image
# ppx Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge
# ppy Vertical coordinate of the principal point of the image, as a pixel offset from the top edge
# width Width of the image in pixels
# "
# out = {
# 'dist_model' : intrinsics.model,
# 'dist_coeff' : intrinsics.coeffs,
# 'fx' : intrinsics.fx,
# 'fy' : intrinsics.fy,
# 'height' : intrinsics.height,
# 'width' : intrinsics.width,
# 'ppx' : intrinsics.ppx,
# 'ppy' : intrinsics.ppy
# }
camera_matrix = np.array([[intrinsics.fx, 0.0, intrinsics.ppx],
[0.0 , intrinsics.fy, intrinsics.ppy],
[0.0 , 0.0 , 1.0]])
distortion_model = intrinsics.model
distortion_coefficients = np.array(intrinsics.coeffs)
camera_info = {'camera_matrix': camera_matrix, 'distortion_coefficients': distortion_coefficients, 'distortion_model': distortion_model}
return camera_info