def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5,
spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20):
"""
RS에서 획득한 깊이 프레임 전처리 필터링 하는 함수
Return:
filtered_frame : rs.frame()
"""
# Post processing possible only on the depth_frame
assert (depth_frame.is_depth_frame())
# Available filters and control options for the filters
decimation_filter = rs.decimation_filter()
spatial_filter = rs.spatial_filter()
temporal_filter = rs.temporal_filter()
filter_magnitude = rs.option.filter_magnitude
filter_smooth_alpha = rs.option.filter_smooth_alpha
filter_smooth_delta = rs.option.filter_smooth_delta
# Apply the control parameters for the filter
decimation_filter.set_option(filter_magnitude, decimation_magnitude)
spatial_filter.set_option(filter_magnitude, spatial_magnitude)
spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha)
spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta)
temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha)
temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta)
# Apply the filters
filtered_frame = decimation_filter.process(depth_frame)
filtered_frame = spatial_filter.process(filtered_frame)
filtered_frame = temporal_filter.process(filtered_frame)
return filtered_frame
def __init__(self, filters=[]):
"""
Connect to RealSense and initialize filters
:param filters: [String, ...], default=[]: '' TODO list filters
"""
self.pipe = rs.pipeline()
cfg = rs.config()
profile = self.pipe.start(cfg)
# camera parameters
self.depth_scale = profile.get_device().first_depth_sensor(
).get_depth_scale()
# filters to apply to depth images
self.filters = filters
if 'align' in self.filters:
self.align = rs.align(rs.stream.color)
if 'decimation' in self.filters:
self.decimation = rs.decimation_filter()
self.decimation.set_option(rs.option.filter_magnitude, 4)
if 'spatial' in self.filters:
self.spatial = rs.spatial_filter()
# self.spatial.set_option(rs.option.holes_fill, 3)
self.spatial.set_option(rs.option.filter_magnitude, 5)
self.spatial.set_option(rs.option.filter_smooth_alpha, 1)
self.spatial.set_option(rs.option.filter_smooth_delta, 50)
if 'temporal' in self.filters:
# TODO
self.temporal = rs.temporal_filter()
self.temporal_iters = 3
if 'hole_filling' in self.filters:
self.hole_filling = rs.hole_filling_filter()
if 'colorize' in self.filters:
self.colorizer = rs.colorizer()
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20):
"""
Filter the depth frame acquired using the Intel RealSense device
Parameters
-----------
depth_frame : rs.frame()
The depth frame to be post-processed
decimation_magnitude : double
The magnitude of the decimation filter
spatial_magnitude : double
The magnitude of the spatial filter
spatial_smooth_alpha : double
The alpha value for spatial filter based smoothening
spatial_smooth_delta : double
The delta value for spatial filter based smoothening
temporal_smooth_alpha : double
The alpha value for temporal filter based smoothening
temporal_smooth_delta : double
The delta value for temporal filter based smoothening
Return:
----------
filtered_frame : rs.frame()
The post-processed depth frame
"""
# Post processing possible only on the depth_frame
assert (depth_frame.is_depth_frame())
# Available filters and control options for the filters
decimation_filter = rs.decimation_filter()
spatial_filter = rs.spatial_filter()
temporal_filter = rs.temporal_filter()
filter_magnitude = rs.option.filter_magnitude
filter_smooth_alpha = rs.option.filter_smooth_alpha
filter_smooth_delta = rs.option.filter_smooth_delta
# Apply the control parameters for the filter
decimation_filter.set_option(filter_magnitude, decimation_magnitude)
spatial_filter.set_option(filter_magnitude, spatial_magnitude)
spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha)
spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta)
temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha)
temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta)
# Apply the filters
filtered_frame = decimation_filter.process(depth_frame)
filtered_frame = spatial_filter.process(filtered_frame)
filtered_frame = temporal_filter.process(filtered_frame)
return filtered_frame
def get_depth_filter_list(decimate=True,
d2d=True,
spatial=True,
temporal=True):
filters = []
if decimate:
dec_filt = rs.decimation_filter()
dec_filt.set_option(rs.option.filter_magnitude, 2)
filters.append(dec_filt)
if d2d:
depth2disparity = rs.disparity_transform()
filters.append(depth2disparity)
if spatial:
spat = rs.spatial_filter()
spat.set_option(rs.option.holes_fill, FILL_ALL_ZERO_PIXELS)
filters.append(spat)
if temporal:
temp = rs.temporal_filter()
filters.append(temp)
if d2d:
disparity2depth = rs.disparity_transform(False)
filters.append(disparity2depth)
return filters
def __init__(self):
ctx = rs.context()
self.devices = ctx.query_devices()
self.configs = list()
self.filters = list()
for device in self.devices:
config = rs.config()
config.enable_device(device.get_info(rs.camera_info.serial_number))
config.enable_stream(rs.stream.depth, IMG_WIDTH, IMG_HEIGHT,
rs.format.z16, 30)
config.enable_stream(rs.stream.color, IMG_WIDTH, IMG_HEIGHT,
rs.format.bgr8, 30)
self.configs.append(config)
align = rs.align(rs.stream.color)
spatial = rs.spatial_filter()
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)
spatial.set_option(ts.option.holes_fill, 3)
temporal = rs.temporal_filter()
hole_filling = rs.hole_filling_filter()
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
decimate = rs.decimation_filter()
self.filters.append({
'align': align,
'spatial': spatial,
'temporal': temporal,
'hole': hole_filling,
'disparity': depth_to_disparity,
'depth': disparity_to_depth,
'decimate': decimate
})
def _filter_depth_frame(depth_frame):
"""
滤波器，用于获取坐标前的深度图像处理
:param depth_frame: 深度帧
:return: 滤波后的深度帧
"""
dec = rs.decimation_filter()
dec.set_option(rs.option.filter_magnitude, 1)
depth_frame_pro = dec.process(depth_frame)
depth2disparity = rs.disparity_transform()
depth_frame_pro = depth2disparity.process(depth_frame_pro)
spat = rs.spatial_filter()
# 启用空洞填充,5为填充所有零像素
spat.set_option(rs.option.holes_fill, 5)
depth_frame_pro = spat.process(depth_frame_pro)
temp = rs.temporal_filter()
depth_frame_pro = temp.process(depth_frame_pro)
disparity2depth = rs.disparity_transform(False)
depth_frame_pro = disparity2depth.process(depth_frame_pro)
# depth_image_pro = np.asanyarray(depth_frame_pro.get_data())
# depth_colormap_pro = cv2.applyColorMap(cv2.convertScaleAbs(depth_image_pro, alpha=0.03), cv2.COLORMAP_JET)
return depth_frame_pro
def __init__(self):
# Create a pipeline
self.pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 60)
config.enable_stream(rs.stream.color, 640, 360, rs.format.bgr8, 60)
# Start streaming
self.profile = self.pipeline.start(config)
s = self.profile.get_device().query_sensors()[1]
s.set_option(rs.option.exposure, 80)
depth_sensor = self.profile.get_device().first_depth_sensor()
self.depth_scale = depth_sensor.get_depth_scale()
self.spat_filter = rs.spatial_filter(
) # Spatial - edge-preserving spatial smoothing
self.temp_filter = rs.temporal_filter(
) # Temporal - reduces temporal noise
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
self.align = rs.align(align_to)
def __init__(self):
# Initialize RealSense intrinsics
self.diagonal = np.linalg.norm((self.W, self.H))
self.pipeline = rs.pipeline()
self.aligner = rs.align(rs.stream.color)
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
self.config.enable_stream(rs.stream.color, self.W, self.H,
rs.format.bgr8, 30)
self.temporal_filter = rs.temporal_filter()
self.hole_filling_filter = rs.hole_filling_filter()
# Start up camera
profile = self.pipeline.start(self.config)
# Set camera options
sensor = profile.get_device().first_depth_sensor()
sensor.set_option(rs.option.enable_auto_exposure, 1)
# sensor.set_option(rs.option.exposure, 5000)
# Acquire an initial set of frames used for calibration
# Flush 10 frames to get the Intel temporal filter warmed up
for i in range(30):
self.__acquire_raw_aligned()
# Save a snapshot of the background for later subtraction, blur it for denoising purposes
self.__depth_background = ndimage.gaussian_filter(
self.__depth_raw_aligned, 20)
def tempfilter(self,frame):
self.temporal = rs.temporal_filter()
if len(self.framequeue) > 10:
self.framequeue.popleft()
self.framequeue.append(frame)
for fr_x in self.framequeue:
self.depth_frame = self.temporal.process(fr_x)
def post_process_depth_frame(depth_frame,
decimation_magnitude=1.0,
spatial_magnitude=2.0,
spatial_smooth_alpha=0.5,
spatial_smooth_delta=20,
temporal_smooth_alpha=0.4,
temporal_smooth_delta=20):
assert (depth_frame.is_depth_frame())
# Available filters and control options for the filters
decimation_filter = rs.decimation_filter()
spatial_filter = rs.spatial_filter()
temporal_filter = rs.temporal_filter()
filter_magnitude = rs.option.filter_magnitude
filter_smooth_alpha = rs.option.filter_smooth_alpha
filter_smooth_delta = rs.option.filter_smooth_delta
# Apply the control parameters for the filter
decimation_filter.set_option(filter_magnitude, decimation_magnitude)
spatial_filter.set_option(filter_magnitude, spatial_magnitude)
spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha)
spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta)
temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha)
temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta)
# Apply the filters
filtered_frame = decimation_filter.process(depth_frame)
filtered_frame = spatial_filter.process(filtered_frame)
filtered_frame = temporal_filter.process(filtered_frame)
return filtered_frame
def computeBackground(options, pipeline, align):
count = 0
minVal = np.full(
(constants.FRAME_DECIMATED_HEIGHT, constants.FRAME_DECIMATED_WIDTH),
constants.MAX_FLOAT32,
dtype=np.float32)
decimation = rs.decimation_filter()
decimation.set_option(rs.option.filter_magnitude, 4)
temporal = rs.temporal_filter()
while count < 60:
frames = pipeline.wait_for_frames()
alignedFrames = align.process(frames)
depth = alignedFrames.get_depth_frame()
# depth = frames.get_depth_frame()
filtered_depth = decimation.process(depth)
# filtered_depth = temporal.process(filtered_depth)
# filtered_depth = depth
d = np.asanyarray(filtered_depth.get_data()).astype(np.float32)
print d.shape, d.dtype
zeros = d.size - np.count_nonzero(d)
print('Input:zeros:' + str(zeros) + ' total:' + str(d.size))
d[d == 0.] = constants.MAX_FLOAT32
minVal = np.minimum(minVal, d)
print('Minval: zeros:' +
str(minVal[minVal == constants.MAX_FLOAT32].size) + ' total:' +
str(minVal.size))
count = count + 1
return inpaint(options, minVal)
def __init__(self, w=640, h=480, depth=True, frame_rate=30):
'''
Initializing the Python RealSense Control Flow:
w: Int (default = 640, can also be 1280)
h: Int (default = 480, can also be 720)
depth: Bool (default = True)
frame_rate: Int (default = 30)
RGB and Depth formats are: bgr8, z16
Note: In this class, variables should not be directly changed.
'''
self.width = w
self.height = h
self.depth_on = depth
self._pipeline = rs.pipeline()
self._config = rs.config()
self._config.enable_stream(rs.stream.color, w, h, rs.format.bgr8,
frame_rate)
self._intrinsic = None
if depth:
self.align = rs.align(rs.stream.color)
self._preset = 0
# Presets (for D415):
# 0: Custom
# 1: Default
# 2: Hand
# 3: High Accuracy
# 4: High Density
# 5: Medium Density
# depth interpolation
self.interpolation = cv2.INTER_NEAREST # use nearest neighbor
# self.interpolation = cv2.INTER_LINEAR # linear
# self.interpolation = cv2.INTER_CUBIC # cubic
# beautify depth image for viewing
self._config.enable_stream(rs.stream.depth, w, h, rs.format.z16,
frame_rate)
self.colorizer = rs.colorizer()
# initialize filters
self.decimation = rs.decimation_filter()
self.decimation.set_option(rs.option.filter_magnitude, 4)
self.depth_to_disparity = rs.disparity_transform(True)
self.spatial = rs.spatial_filter()
self.spatial.set_option(rs.option.filter_magnitude, 5)
self.spatial.set_option(rs.option.filter_smooth_alpha, 0.5)
self.spatial.set_option(rs.option.filter_smooth_delta, 20)
self.temporal = rs.temporal_filter()
self.disparity_to_depth = rs.disparity_transform(False)
print(
"Initialized RealSense Camera\nw: {}, h: {}, depth: {}, frame_rate: {}"
.format(w, h, depth, frame_rate))
def filtering(self, frame):
'''Filter setting'''
# Decimation #
decimation = rs.decimation_filter()
decimation.set_option(rs.option.filter_magnitude, 1)
# Spatial #
spatial = rs.spatial_filter()
# spatial.set_option(rs.option.filter_magnitude, 5)
spatial.set_option(rs.option.filter_smooth_alpha, 0.6)
spatial.set_option(rs.option.filter_smooth_delta, 8)
# spatial.set_option(rs.option.holes_fill, 3)
# Temporal #
temporal = rs.temporal_filter()
temporal.set_option(rs.option.filter_smooth_alpha, 0.5)
temporal.set_option(rs.option.filter_smooth_delta, 20)
# Hole #
hole_filling = rs.hole_filling_filter()
##
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
'''Appling filter'''
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)
return frame
def mainSegment(options = None):
pipeline, depthScale, align = initCamera()
print '--------------------------------------------'
print options
counter = 0
if constants.PROFILE_ON:
pr = cProfile.Profile() #profile
pr.enable() #profile
try:
warmUp(pipeline)
minVal = computeBackground(options, pipeline, align)
net = bp.newNet()
t0 = time.time()
counterOld = counter
decimation = rs.decimation_filter()
decimation.set_option(rs.option.filter_magnitude,
constants.DECIMATION_FACTOR)
temporal = rs.temporal_filter(0.5, 20, 5)#valid 1 of last 2
while True:
counter = counter + 1
frames = pipeline.wait_for_frames()
# alignedFrames = align.process(frames)
# depth = alignedFrames.get_depth_frame()
depth = frames.get_depth_frame()
filtered_depth = decimation.process(depth)
filtered_depth = temporal.process(filtered_depth)
# intrinsics already reflects decimation
intrinsics = filtered_depth.profile.as_video_stream_profile().intrinsics
# filtered_depth = depth
# if counter % 2 == 1:
#help the temporal filter since we cannot process full rate
# continue
d = np.asanyarray(filtered_depth.get_data()).astype(np.float32)
alpha = subtraction(options, minVal, d)
if (options and options.get('display')):
cv2.imshow('Contour', alpha)
if constants.PROFILE_ON:
result = pr.runcall(bp.process, options, net, d, alpha,
depthScale, intrinsics) #profile
else:
result = bp.process(options, net, d, alpha, depthScale,
intrinsics)
if (options and options.get('display')):
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if counter % 10 == 0:
t1 = time.time()
print '{:.3f} images/sec'.format((counter - counterOld)/(t1-t0))
t0 = t1
counterOld = counter
yield json.dumps(result)
finally:
if constants.PROFILE_ON:
pr.disable()
pr.print_stats()#profile
pipeline.stop()
def __init__(self):
self.filters = [
rs.decimation_filter(RESCALE),
rs.disparity_transform(True),
rs.hole_filling_filter(1),
rs.spatial_filter(0.5, 8, 2, 2),
rs.temporal_filter(0.5, 20, 1),
rs.disparity_transform(False)
]
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5,
spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20):
"""
Filter the depth frame acquired using the Intel RealSense device
Parameters:
-----------
depth_frame : rs.frame()
The depth frame to be post-processed
decimation_magnitude : double
The magnitude of the decimation filter
spatial_magnitude : double
The magnitude of the spatial filter
spatial_smooth_alpha : double
The alpha value for spatial filter based smoothening
spatial_smooth_delta : double
The delta value for spatial filter based smoothening
temporal_smooth_alpha : double
The alpha value for temporal filter based smoothening
temporal_smooth_delta : double
The delta value for temporal filter based smoothening
Return:
----------
filtered_frame : rs.frame()
The post-processed depth frame
"""
# Post processing possible only on the depth_frame
assert (depth_frame.is_depth_frame())
# Available filters and control options for the filters
decimation_filter = rs.decimation_filter()
spatial_filter = rs.spatial_filter()
temporal_filter = rs.temporal_filter()
filter_magnitude = rs.option.filter_magnitude
filter_smooth_alpha = rs.option.filter_smooth_alpha
filter_smooth_delta = rs.option.filter_smooth_delta
# Apply the control parameters for the filter
decimation_filter.set_option(filter_magnitude, decimation_magnitude)
spatial_filter.set_option(filter_magnitude, spatial_magnitude)
spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha)
spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta)
temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha)
temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta)
# Apply the filters
filtered_frame = decimation_filter.process(depth_frame)
filtered_frame = spatial_filter.process(filtered_frame)
filtered_frame = temporal_filter.process(filtered_frame)
return filtered_frame
def start_pipe(self, align=True, usb3=True):
if not self.pipelineStarted:
if align:
print('Etablissement de la connection caméra')
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
self.pipeline = rs.pipeline()
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
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
self.profile = self.pipeline.start(config)
align_to = rs.stream.color
self.align = rs.align(align_to)
time.sleep(1)
# self.pipeline = rs.pipeline()
# config = rs.config()
#
# if usb3:
# config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
#
# else:
# self.profile = config.resolve(self.pipeline) # does not start streaming
#
# self.profile = self.pipeline.start(config)
# self.pipelineStarted = True
# # Align the two streams
# align_to = rs.stream.color
# self.align = rs.align(align_to)
self.pipelineStarted = True
# Get depth scale
depth_sensor = self.profile.get_device().first_depth_sensor()
self.depth_scale = depth_sensor.get_depth_scale()
# Création des filtres
self.hole_filling = rs.hole_filling_filter()
self.temporal_filter = rs.temporal_filter()
self.spatial_filter = rs.spatial_filter()
self.depth_to_disparity = rs.disparity_transform(False)
# Get Intrinsic parameters
self.get_intrinsic()
print('Caméra Ouverte')
def init_depth_cam(self):
self.pipeline_profile = self.pipeline.start(self.config)
self.depth_sensor = self.pipeline_profile.get_device().first_depth_sensor()
self.depth_sensor.set_option(rs.option.emitter_enabled, 1)
self.depth_sensor.set_option(rs.option.laser_power, 250)
self.depth_sensor.set_option(rs.option.depth_units, 0.0001) #changed 0.0001
self.temp_filter=rs.temporal_filter()
self.temp_filter.set_option(rs.option.filter_smooth_alpha,0.8)
self.temp_filter.set_option(rs.option.filter_smooth_delta,10)
self.temp_filter.set_option(rs.option.holes_fill,1.0)
self.spatial_filter=rs.spatial_filter()
self.spatial_filter.set_option(rs.option.holes_fill,3)
device = self.pipeline_profile.get_device()
def init_video_capture(self):
try:
# Create a pipeline
self.pipeline = rs.pipeline()
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, DEPTH_RES_X, DEPTH_RES_Y,
rs.format.z16, DEPTH_FPS)
config.enable_stream(rs.stream.color, RGB_RES_X, RGB_RES_Y,
rs.format.bgr8, RGB_FPS)
# config.enable_stream(rs.stream.infrared, 1, DEPTH_RES_X, DEPTH_RES_Y, rs.format.y8, DEPTH_FPS)
# config.enable_stream(rs.stream.infrared, 2, DEPTH_RES_X, DEPTH_RES_Y, rs.format.y8, DEPTH_FPS)
# Start streaming
profile = self.pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
self.depth_scale = depth_sensor.get_depth_scale()
# print("[RealSense D435]: Depth scale", self.depth_scale)
# TODO: Allow settings to be changed on initializing the function
depth_sensor.set_option(rs.option.laser_power, 360) # 0 - 360
depth_sensor.set_option(
rs.option.depth_units,
0.001) # Number of meters represented by a single depth unit
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
self.align = rs.align(align_to)
self.hole_filling_filter = rs.hole_filling_filter()
self.decimation_filter = rs.decimation_filter()
self.temporal_filter = rs.temporal_filter()
except Exception as e:
print('[RealsenseD435Camera]: ERROR:', e, file=sys.stderr)
print(
'[RealsenseD435Camera]: Could not initialize camera. If the resource is busy, check if any other script '
'is currently accessing the camera. If this is not the case, replug the camera and try again.',
file=sys.stderr)
sys.exit(0)
self.init_colorizer()
self.started = False
self.read_lock = threading.Lock()
def __init__(self, debugFlag=False, debugPath=''):
self.debugFlag = debugFlag
# Decimation - reduces depth frame density
self.decimateFilter = rs.decimation_filter()
self.thresholdFilter = rs.threshold_filter(min_dist = 1.8, max_dist = 3)
# Converts from depth representation to disparity representation and vice - versa in depth frames
self.depth_to_disparity = rs.disparity_transform(True)
# Spatial - edge-preserving spatial smoothing
self.spatial_filter = rs.spatial_filter()
# Temporal - reduces temporal noise
self.temporalFilter = rs.temporal_filter()
self.disparity_to_depth = rs.disparity_transform(False)
def filtering(self):
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
spatial = rs.spatial_filter()
temporal = rs.temporal_filter()
hole_filling = rs.hole_filling_filter()
for frame in self.depth_frams:
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)
self.aligned_depth_frame = frame.get_data()
self.colorized_depth = np.asanyarray(
self.colorizer.colorize(frame).get_data())
def prepare_filters(self):
# prepare post-processing filters
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2 ** 3)
spatial = rs.spatial_filter()
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)
spatial.set_option(rs.option.holes_fill, 3)
colorizer = rs.colorizer()
self.filters = [rs.disparity_transform(),
rs.decimation_filter(),
rs.spatial_filter(),
rs.temporal_filter(),
rs.disparity_transform(False)]
def post_processing(frame,
enable_spatial=True,
enable_temporal=True,
enable_hole=True,
spatial_params=[(rs.option.filter_magnitude, 5),
(rs.option.filter_smooth_alpha, 1),
(rs.option.filter_smooth_delta, 50),
(rs.option.holes_fill, 3)],
temporal_params=[],
hole_params=[]):
"""Filters to cleanup depth maps.
"""
# Filters and settings
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
# Depth to disparity before spatial and temporal filters
frame = depth_to_disparity.process(frame)
# Spatial filter
if enable_spatial:
# Settings
spatial = rs.spatial_filter()
for spatial_param in spatial_params:
spatial.set_option(spatial_param[0], spatial_param[1])
# Apply on frame
frame = spatial.process(frame)
# Temporal filter
if enable_temporal:
temporal = rs.temporal_filter()
for temporal_param in temporal_params:
temporal.set_option(temporal_param[0], temporal_param[1])
frame = temporal.process(frame)
# Back to depth
frame = disparity_to_depth.process(frame)
# Hole filling
if enable_hole:
hole_filling = rs.hole_filling_filter()
for hole_param in hole_params:
hole_filling.set_option(hole_param[0], hole_param[1])
frame = hole_filling.process(frame)
return frame
def __init__(self):
# ---------------------------------------------------------
# real sense
# ---------------------------------------------------------
# # ストリーム(Depth/Color)の設定
print("start Depth camera")
width: int = 640
height: int = 480
config = rs.config()
config.enable_stream(rs.stream.color, width, height, rs.format.bgr8,
30)
config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30)
#
self.colorizer = rs.colorizer()
#temporal filter
self.temporal = rs.temporal_filter()
#hole filling filter
self.hole_filling = rs.hole_filling_filter()
# ストリーミング開始
self.pipeline = rs.pipeline()
profile = self.pipeline.start(config)
# Skip 5 first frames to give the Auto-Exposure time to adjust
for x in range(5):
self.pipeline.wait_for_frames()
#get device information
depth_sensor = profile.get_device().first_depth_sensor()
#print("depth sensor:",depth_sensor)
self.depth_scale = depth_sensor.get_depth_scale()
#print("depth scale:",depth_scale)
clipping_distance_in_meters = 1.0 # meter
clipping_distance = clipping_distance_in_meters / self.depth_scale
print("clipping_distance:", clipping_distance)
# Alignオブジェクト生成
align_to = rs.stream.color
self.align = rs.align(align_to)
self.frameNo = 0
self.timenow = ""
def apply_filters(self: 'Camera',
frames: Iterable[rs2.depth_frame]) -> rs2.frame:
"""
Apply a set of filters on the frames that were captured.
:param frames: The frames to filter
:return: The final frame after adding filters
"""
frame: rs2.frame = frames[0]
temp_filter = rs2.temporal_filter()
for frame in frames:
frame = temp_filter.process(frame)
filters = utils.get_filters(self.filters)
for fil in filters:
frame = fil.process(frame)
return frame
def create_filters(self):
spatial = rs.spatial_filter()
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)
spatial.set_option(rs.option.holes_fill, 3)
temporal = rs.temporal_filter()
hole_filling = rs.hole_filling_filter()
disparity_to_depth = rs.disparity_transform(False)
depth_to_disparity = rs.disparity_transform(True)
filters = {
"S": spatial,
"T": temporal,
"H": hole_filling,
"DZ": disparity_to_depth,
"ZD": depth_to_disparity
}
return filters
def __init__(self, serial=None, resolution="high", calib_file=None):
self._is_start = False
self._frame_id = 0
self._auto_exp_skip = 30
self.calib_file = calib_file
msg = "`resolution` can only be [`low` or `high`]"
assert resolution in ['low', 'high'], msg
# query any connected realsense
if serial is None:
serials = utils.rs_discover_cams()
if serials:
self._serial = serials[0]
else:
raise ValueError(
"Could not find connected camera. Ensure USB 3.0 is used.")
else:
self._serial = serial
if resolution == "low":
self._height = constants.D415.HEIGHT_L.value
self._width = constants.D415.WIDTH_L.value
self._fps = constants.D415.FPS_L.value
else:
self._height = constants.D415.HEIGHT_H.value
self._width = constants.D415.WIDTH_H.value
self._fps = constants.D415.FPS_H.value
self._resolution = (self._width, self._height)
# pipeline and config
self._pipe = rs.pipeline()
self._cfg = rs.config()
# post-processing
self._spatial_filter = rs.spatial_filter()
self._hole_filling = rs.hole_filling_filter()
self._temporal_filter = rs.temporal_filter()
# misc
self._colorizer = rs.colorizer()
self._align = rs.align(rs.stream.color)
def post_process(image): spatial_filter = rs.spatial_filter() temporal_filter = rs.temporal_filter() hole_filling_filter = rs.hole_filling_filter() filter_magnitude = rs.option.filter_magnitude filter_smooth_alpha = rs.option.filter_smooth_alpha filter_smooth_delta = rs.option.filter_smooth_delta spatial_filter.set_option(filter_magnitude, 2) spatial_filter.set_option(filter_smooth_alpha, 0.5) spatial_filter.set_option(filter_smooth_delta, 20) temporal_filter.set_option(filter_smooth_alpha, 0.4) temporal_filter.set_option(filter_smooth_delta, 20) # Apply the filters filtered_frame = spatial_filter.process(image) filtered_frame = temporal_filter.process(filtered_frame) #filtered_frame = hole_filling_filter.process(filtered_frame) return filtered_frame
def get_filter(filter_name: str, *args) -> rs2.filter_interface:
"""
Basically a factory for filters (Maybe change to Dict 'cause OOP)
:param filter_name: The filter name
:param args: The arguments for the filter
:return: A filter which corresponds to the filter name with it's arguments
"""
if filter_name == 'decimation_filter':
return rs2.decimation_filter(*args)
elif filter_name == 'threshold_filter':
return rs2.threshold_filter(*args)
elif filter_name == 'disparity_transform':
return rs2.disparity_transform(*args)
elif filter_name == 'spatial_filter':
return rs2.spatial_filter(*args)
elif filter_name == 'temporal_filter':
return rs2.temporal_filter(*args)
elif filter_name == 'hole_filling_filter':
return rs2.hole_filling_filter(*args)
else:
raise Exception(f'The filter \'{filter_name}\' does not exist!')
def __init__(self,
image_w=640,
image_h=480,
image_d=3,
compress_depth=True):
self.combined_array = None
self.compress_depth = compress_depth
self.running = True
self.image_w = image_w
self.image_h = image_h
#self.depth, self.color = None, None
#Set stream speed and resolution
self.fps = 30
#start post processing
self.colorizer = colorizer()
self.holefiller = hole_filling_filter(1)
self.spatial = spatial_filter()
self.temporal = temporal_filter()
#start the Realsense pipeline
self.pipe = pipeline()
#load the configuration
cfg = config()
cfg.enable_stream(stream.color,
stream_index=-1,
width=848,
height=480,
framerate=30)
cfg.enable_stream(stream.depth,
stream_index=-1,
width=848,
height=480,
framerate=30)
#start the pipeline
profile = self.pipe.start(cfg)
#set units to 100um
depth_sensor = profile.get_device().first_depth_sensor()
depth_sensor.set_option(option.depth_units, 0.0001)
def create_pipeline(config: dict):
"""Sets up the pipeline to extract depth and rgb frames
Arguments:
config {dict} -- A dictionary mapping for configuration. see default.yaml
Returns:
tuple -- pipeline, process modules, filters, t265 device (optional)
"""
# Create pipeline and config for D4XX,L5XX
pipeline = rs.pipeline()
rs_config = rs.config()
# IF t265 is enabled, need to handle seperately
t265_dev = None
t265_sensor = None
t265_pipeline = rs.pipeline()
t265_config = rs.config()
if config['playback']['enabled']:
# Load recorded bag file
rs.config.enable_device_from_file(
rs_config, config['playback']['file'],
config['playback'].get('repeat', False))
# This code is only activated if the user points to a T265 Recorded Bag File
if config['tracking']['enabled']:
rs.config.enable_device_from_file(
t265_config, config['tracking']['playback']['file'],
config['playback'].get('repeat', False))
t265_config.enable_stream(rs.stream.pose)
t265_pipeline.start(t265_config)
profile_temp = t265_pipeline.get_active_profile()
t265_dev = profile_temp.get_device()
t265_playback = t265_dev.as_playback()
t265_playback.set_real_time(False)
else:
# Ensure device is connected
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:
logging.error("No connected Intel Realsense Device!")
sys.exit(1)
if config['advanced']:
logging.info(
"Attempting to enter advanced mode and upload JSON settings file"
)
load_setting_file(ctx, devices, config['advanced'])
if config['tracking']['enabled']:
# Cycle through connected devices and print them
for dev in devices:
dev_name = dev.get_info(rs.camera_info.name)
print("Found {}".format(dev_name))
if "Intel RealSense D4" in dev_name:
pass
elif "Intel RealSense T265" in dev_name:
t265_dev = dev
elif "Intel RealSense L515" in dev_name:
pass
if config['tracking']['enabled']:
if len(devices) != 2:
logging.error("Need 2 connected Intel Realsense Devices!")
sys.exit(1)
if t265_dev is None:
logging.error("Need Intel Realsense T265 Device!")
sys.exit(1)
if t265_dev:
# Unable to open as a pipeline, must use sensors
t265_sensor = t265_dev.query_sensors()[0]
profiles = t265_sensor.get_stream_profiles()
pose_profile = [
profile for profile in profiles
if profile.stream_name() == 'Pose'
][0]
t265_sensor.open(pose_profile)
t265_sensor.start(callback_pose)
logging.info("Started streaming Pose")
rs_config.enable_stream(rs.stream.depth, config['depth']['width'],
config['depth']['height'], rs.format.z16,
config['depth']['framerate'])
# other_stream, other_format = rs.stream.infrared, rs.format.y8
rs_config.enable_stream(rs.stream.color, config['color']['width'],
config['color']['height'], rs.format.rgb8,
config['color']['framerate'])
# Start streaming
pipeline.start(rs_config)
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
color_sensor = profile.get_device().first_color_sensor()
depth_scale = depth_sensor.get_depth_scale()
# depth_sensor.set_option(rs.option.global_time_enabled, 1.0)
# color_sensor.set_option(rs.option.global_time_enabled, 1.0)
if config['playback']['enabled']:
dev = profile.get_device()
playback = dev.as_playback()
playback.set_real_time(False)
# Processing blocks
filters = []
decimate = None
align = rs.align(rs.stream.color)
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
# Decimation
if config.get("filters").get("decimation"):
filt = config.get("filters").get("decimation")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
decimate = rs.decimation_filter(**filt)
# Spatial
if config.get("filters").get("spatial"):
filt = config.get("filters").get("spatial")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.spatial_filter(**filt)
filters.append(my_filter)
# Temporal
if config.get("filters").get("temporal"):
filt = config.get("filters").get("temporal")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.temporal_filter(**filt)
filters.append(my_filter)
process_modules = (align, depth_to_disparity, disparity_to_depth, decimate)
intrinsics = get_intrinsics(pipeline, rs.stream.color)
proj_mat = create_projection_matrix(intrinsics)
sensor_meta = dict(depth_scale=depth_scale)
config['sensor_meta'] = sensor_meta
# Note that sensor must be saved so that it is not garbage collected
t265_device = dict(pipeline=t265_pipeline, sensor=t265_sensor)
return pipeline, process_modules, filters, proj_mat, t265_device
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
# Processing blocks
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
colorizer = rs.colorizer()
filters = [rs.disparity_transform(),
rs.spatial_filter(),
rs.temporal_filter(),
rs.disparity_transform(False)]
# pyglet
window = pyglet.window.Window(
config=gl.Config(
double_buffer=True,
samples=8 # MSAA
),
resizable=True, vsync=True)
keys = pyglet.window.key.KeyStateHandler()
window.push_handlers(keys)
def convert_fmt(fmt):
