def __init__(self):
self.conversion_service = ConversionService.get_instance()
self.pipeline = rs.pipeline()
config = rs.config()
self.threshold_filter = rs.threshold_filter()
self.threshold_filter.set_option(rs.option.max_distance, 4)
config.enable_stream(rs.stream.depth, rs.format.z16, 30)
config.enable_stream(rs.stream.color, rs.format.bgr8, 30)
# Start streaming
self.pipeline.start(config)
self.profile = self.pipeline.get_active_profile()
self.sensor = self.profile.get_device().first_depth_sensor()
self.sensor.set_option(rs.option.visual_preset, value=1)
self.sensor.set_option(rs.option.motion_range, value=200)
# Set colour palette to white to black
self.colorizer = rs.colorizer(3)
self.colorizer.set_option(rs.option.min_distance, 0)
self.colorizer.set_option(rs.option.max_distance, 0.1)
self.colorizer.set_option(rs.option.histogram_equalization_enabled,
False)
def getDepthFrame(self):
# frames = self.pipeline.wait_for_frames()
if (len(self.depthFramesBuffer) > 0):
print(',')
depthFrame = self.depthFramesBuffer.__getitem__(0)
spatial = rs.spatial_filter()
spatial.set_option(rs.option.filter_magnitude, 2)
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, 2)
# hole_filling = rs.hole_filling_filter()
# hole_filling.set_option(rs.option.filter_magnitude, 2)
thresh = rs.threshold_filter(0.1, 2)
#
# depthImage = cv2.bilateralFilter(depthImage,9,75,75)
depthFrame = thresh.process(depthFrame)
depthFrame = spatial.process(depthFrame)
# depthFrame = hole_filling.process(depthFrame)
depthImage = np.asanyarray(depthFrame.get_data())
# depthImage = depthImage * 255
# depthImage = cv2.medianBlur(depthImage,1)
# depthImage = cv2.GaussianBlur(depthImage,(3,3),3)
del self.depthFramesBuffer[0]
else:
frames = self.pipeline.wait_for_frames()
self.colorFramesBuffer.append(frames.get_color_frame())
depthFrame = frames.get_depth_frame()
depthframe = rs.hole_filling_filter.process(depthFrame)
depthImage = np.asanyarray(depthFrame.get_data())
depthImage = cv2.GaussianBlur(depthImage, (11, 11), 5)
depthImage = cv2.medianBlur(depthImage, 51)
depthImage = cv2.bilateralFilter(depthImage, 9, 75, 75)
print(len(self.depthFramesBuffer))
# del self.framesBuffer[-1]
depthColorMap = cv2.applyColorMap(
cv2.convertScaleAbs(depthImage, alpha=0.003), cv2.COLORMAP_JET)
return depthColorMap
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 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 capture_point_cloud(number_of_frames: int = 1, camera_id: str = "", max_dist: float = 0.5, min_dist: float = 0.1) \
-> np.numarray:
"""
Start a pipeline with a depth stream configuration (and enable a given camera id, if given. else - takes depth
frames from all connected cameras). Then, wait for frameset, apply a threshold filter, convert to depth frame,
and pass it to a point cloud realsense2 object to process and get the points, which it returns as a numpy array.
:param number_of_frames: The number of frames to sample.
:param camera_id: The camera id to get the depth frame from.
:param max_dist: the maximum distance between the camera and the object.
:param min_dist: the minimum distance between the camera and the object.
:return: point cloud's points as a numpy array.
"""
# Initialize the numpy array that we will return
vertices: np.numarray = np.asanyarray([])
# Declare RealSense pipeline, encapsulating the actual device and sensor (depth sensor)
pipe: rs2.pipeline = rs2.pipeline()
config: rs2.config = rs2.config()
# Enable depth stream
config.enable_stream(rs2.stream.depth)
# If there was given a camera_id, enable it in the configs
if len(camera_id) == 12:
config.enable_device(camera_id)
# Start streaming with chosen configuration
pipe.start(config)
# Create a thresh filter with max distance of 0.7 meter
thresh_filter: rs2.filter = rs2.threshold_filter(max_dist=max_dist, min_dist=min_dist)
try:
if DEBUG:
print("Taking Picture")
# Wait for 30 frames to make it more accurate
for _ in range(30):
pipe.wait_for_frames()
# Wait for the next set of frames from the camera
frames: rs2.composite_frame = pipe.wait_for_frames()
if DEBUG:
print("Processing Picture")
# Apply the thresh filter on the frames we just got
filtered_frames: rs2.composite_frame = thresh_filter.process(frames).as_frameset()
# Convert the filtered frames to depth frame
filtered_depth_frame: rs2.depth_frame = filtered_frames.get_depth_frame()
# Declare realsense's pointcloud object, for calculating pointclouds
rs_pc: rs2.pointcloud = rs2.pointcloud()
# Get the point cloud's points of the filtered depth frame
points: rs2.points = rs_pc.calculate(filtered_depth_frame)
# Convert the points to a numpy array
vertices: np.numarray = np.asanyarray(points.get_vertices())
# Eliminate the origin points (there are a lot of (0,0,0) that just make the calculation time longer)
vertices: np.numarray = np.asanyarray([[x, y, z] for (x, y, z) in vertices if not x == y == z == 0])
finally:
# stop the pipeline
pipe.stop()
# return the point cloud's points
return vertices
# declare RealSense pipeline, encapsulating the actual device and sensors
pipe: rs2.pipeline = rs2.pipeline()
config: rs2.config = rs2.config()
# enable depth stream
config.enable_stream(rs2.stream.depth)
# start streaming with chosen configuration
pipe.start(config)
# we'll use the colorizer to generate texture for our PLY
# (alternatively, texture can be obtained from color or infrared stream)
colorizer: rs2.colorizer = rs2.colorizer()
# create a thresh filter with max distance of 0.7 meter
thresh_filter: rs2.filter = rs2.threshold_filter(max_dist=2.0, min_dist=0.0)
try:
# wait for 30 frames to make it more accurate
for _ in range(30):
pipe.wait_for_frames()
# wait for the next set of frames from the camera
frames: rs2.composite_frame = pipe.wait_for_frames()
print("Applying thresh filter")
# apply the thresh filter on the frame we just got
filtered_frames: rs2.depth_frame = thresh_filter.process(frames)
# colorize the filtered frames
colorized = colorizer.process(filtered_frames)
def start_node(task_queue, result_queue):
##################################################################
# Vibration Control Util
##################################################################
try:
board = Arduino('COM5')
except:
board = Arduino('COM3')
pins = [
board.get_pin('d:10:p'),
board.get_pin('d:9:p'),
board.get_pin('d:6:p'),
board.get_pin('d:5:p'),
board.get_pin('d:3:p')
]
def set_vib(l: list):
val, idx = min((val, idx) for (idx, val) in enumerate(l))
pins[idx].write(max(1 - l[idx] / 60, 0))
for i in range(len(l)):
if i != idx:
pins[i].write(0)
##################################################################
# Vision Setup
##################################################################
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)
pipeline.start(config)
print("Start camera pipelining ...")
align_to = rs.align(rs.stream.color)
profile_data = pipeline.wait_for_frames().get_color_frame()
# Calculate crop Size
cropSize = (0, 0)
if profile_data.width / float(profile_data.height) > WHRatio:
cropSize = (int(profile_data.height * WHRatio), profile_data.height)
else:
cropSize = (profile_data.width, int(profile_data.width / WHRatio))
crop = [(int((profile_data.width - cropSize[1]) / 2),
int((profile_data.height - cropSize[0]) / 2)), cropSize]
last_frame_number = 0
colorizer = rs.colorizer()
decimation = rs.decimation_filter()
threshold = rs.threshold_filter()
spatial = rs.spatial_filter()
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
try:
while (True):
frame = pipeline.wait_for_frames()
frame = align_to.process(frame)
color_frame = frame.get_color_frame()
depth_frame = frame.get_depth_frame()
depth_frame = decimation.process(depth_frame)
depth_frame = threshold.process(depth_frame)
depth_frame = depth_to_disparity.process(depth_frame)
depth_frame = spatial.process(depth_frame)
depth_frame = disparity_to_depth.process(depth_frame)
color_mat = np.asanyarray(color_frame.get_data())
depth_mat = np.asanyarray(depth_frame.get_data())
if depthmap_visualization:
depth_colormap = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
cv2.imshow("Depth Map", depth_colormap)
if colormap_visualization:
cv2.imshow("Color Map", color_mat)
intensity = obstacleEstimate(depth_mat)
if depthintensity_verbose:
print("Intensity:", intensity)
set_vib(intensity)
task = None
objDetectEnabled = False
try:
task = task_queue.get_nowait()
if task is None:
break
else:
objDetectEnabled = True
except queue.Empty:
pass
# Detection by keyboard for testing
key = cv2.waitKey(1)
if key == 32: objDetectEnabled = True
if objDetectEnabled:
if color_frame.get_frame_number() != last_frame_number:
last_frame_number = color_frame.get_frame_number()
# Record start time
if objdetection_verbose:
start_time = time.clock()
objects = objectDetect(color_mat, depth_mat, crop)
# Send result to the message queue
if task is not None:
res = list(task)
res.append(objects)
result_queue.put(tuple(res))
for obj in objects:
className, confidence, rect, m = obj["classname"], obj[
"confidence"], obj["rect"], obj["distance"]
conf = "%s(%f), %fm" % (className, confidence,
m / 1000)
if objdetection_verbose:
print("Detected: %s" % conf)
if objdetect_visualization:
cv2.rectangle(color_mat, (rect[0], rect[1]),
(rect[2], rect[3]), (0, 255, 0))
cv2.putText(color_mat, conf, (rect[0], rect[1]),
cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 0))
if objdetection_verbose:
print("Detection time: %fs" %
(time.clock() - start_time))
if objdetect_visualization:
cv2.imshow("Object Detection", color_mat)
except KeyboardInterrupt:
print("Shutdown realsense worker ...")
finally:
# Stop streaming
pipeline.stop()
set_vib([60, 60, 60, 60, 60])
def main():
if not os.path.exists(args.out_rdir):
os.mkdir(args.out_rdir)
print('Created out_rgb_dir')
if not os.path.exists(args.out_ddir):
os.mkdir(args.out_ddir)
print('Created out_depth_dir')
if not os.path.exists(args.annotations_dir):
os.mkdir(args.annotations_dir)
print('Created args_out_dir')
#Create pipeline for realsense2
pipe = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 360, rs.format.rgb8, 30)
profile = pipe.start(config)
# Declare filters
dc_ftr = rs.decimation_filter() # Decimation - reduces depth frame density
st_ftr = rs.spatial_filter(
) # Spatial - edge-preserving spatial smoothing
tp_ftr = rs.temporal_filter() # Temporal - reduces temporal noise
th_ftr = rs.threshold_filter()
align_to = rs.stream.color
align = rs.align(align_to)
rgbd = np.zeros((1, 4, 360, 640))
i = 0
print(
'################|Initialization seguence completed.|################')
try:
while (1):
# Get frameset of color and depth
frames = pipe.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame() # 640x480
color_frame = aligned_frames.get_color_frame()
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
# Post processing
filtered = tp_ftr.process(
st_ftr.process(dc_ftr.process(aligned_depth_frame)))
filtered = th_ftr.process(
filtered) #can be removed if person is outside frame
rgb = np.asanyarray(color_frame.get_data())
rgb = np.transpose(rgb, (2, 0, 1))
depth = np.asanyarray(aligned_depth_frame.get_data(),
dtype=np.uint16)
depth = depth
im.show(rgb, args.bb)
im.rgbsave(i, rgb, str('./' + args.out_rdir))
im.dsave(i, depth, str('./' + args.out_ddir))
np.savetxt('./' + args.annotations_dir + '/' + str(i) + '.csv',
args.bb,
delimiter=',')
i = i + 1
print('saved' + str(i) + 'image')
# time.sleep(1)
finally:
pipe.stop()
print('################| Error in pipeline |################')
profile = pipeline.start(config)
align_to = rs.stream.depth
align = rs.align(align_to)
min_depth = 0.3
max_depth = 4.0
# Create colorizer object
colorizer = rs.colorizer()
colorizer.set_option(rs.option.color_scheme, 9)
colorizer.set_option(rs.option.histogram_equalization_enabled, 0)
colorizer.set_option(rs.option.min_distance, min_depth)
colorizer.set_option(rs.option.max_distance, max_depth)
# Filter
thr_filter = rs.threshold_filter()
thr_filter.set_option(rs.option.min_distance, min_depth)
thr_filter.set_option(rs.option.max_distance, max_depth)
# Disparity
dp_filter = rs.disparity_transform()
#https://dev.intelrealsense.com/docs/depth-image-compression-by-colorization-for-intel-realsense-depth-cameras#section-6references
#https://github.com/TetsuriSonoda/rs-colorize/blob/master/rs-colorize/rs-colorize.cpp
start = timer()
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.moveWindow("RealSense", 0, 0)
depth_sensor = profile.get_device().first_depth_sensor()
obstacle_line_thickness_pixel = 10 # [1-DEPTH_HEIGHT]: Number of pixel rows to use to generate the obstacle distance message. For each column, the scan will return the minimum value for those pixels centered vertically in the image.
USE_PRESET_FILE = True
PRESET_FILE = "../cfg/d4xx-default.json"
RTSP_STREAMING_ENABLE = True
RTSP_PORT = "8554"
RTSP_MOUNT_POINT = "/d4xx"
# List of filters to be applied, in this order.
# https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md
filters = [[True, "Decimation Filter",
rs.decimation_filter()],
[True, "Threshold Filter",
rs.threshold_filter()],
[True, "Depth to Disparity",
rs.disparity_transform(True)],
[True, "Spatial Filter",
rs.spatial_filter()],
[True, "Temporal Filter",
rs.temporal_filter()],
[False, "Hole Filling Filter",
rs.hole_filling_filter()],
[True, "Disparity to Depth",
rs.disparity_transform(False)]]
#
# The filters can be tuned with opencv_depth_filtering.py script, and save the default values to here
# Individual filters have different options so one have to apply the values accordingly
#
def threshold(depth_frame, min_depth, max_depth):
threshold_filter = rs.threshold_filter(min_dist=min_depth,
max_dist=max_depth)
return threshold_filter.process(depth_frame)
class CameraD435(Camera):
CFG_MODE_1 = 1 # Enable RGB and depth streams and don't save frames
CFG_MODE_2 = 2 # Enable RGB and infrared stream and don't save frames
CFG_MODE_3 = 3 # Enable only infrared stream and don't save frames
CFG_MODE_4 = 4 # Custom
# rs2_stream are types of data provided by RealSense device
stream_type = {
'depth': rs.stream.depth,
'colour': rs.stream.color,
'infrared': rs.stream.infrared
}
# rs2_format identifies how binary data is encoded within a frame
stream_format = {
'depth': rs.format.z16,
'colour': rs.format.bgr8,
'infrared': rs.format.y8
}
filters = {
'decimation': rs.decimation_filter(),
'threshold': rs.threshold_filter(),
'depth_to_disparity': rs.disparity_transform(True),
'spatial': rs.spatial_filter(),
'temporal': rs.temporal_filter(),
'hole_filling': rs.hole_filling_filter(),
'disparity_to_depth': rs.disparity_transform(False)
}
DS5_product_ids = [
"0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00",
"0B01", "0B03", "0B07", "0B3A", "0B5C"
]
def __init__(self,
configuration_mode=1,
enable_rgb_stream=True,
enable_depth_stream=True,
enable_infrared_stream=True,
save_rgb_frames=False,
save_depth_frames=False,
save_infrared_frames=False):
self._init_mode(configuration_mode, enable_rgb_stream,
enable_depth_stream, enable_infrared_stream,
save_rgb_frames, save_depth_frames,
save_infrared_frames)
super().__init__(Camera.TYPE_D435)
self.logger.log_info("Starting camera in configuration mode %d" %
configuration_mode)
self._setup_save_dir()
self.colour_frame_process = None
self.depth_frame_process = None
self.ir1_frame_process = None
self.ir2_frame_process = None
def _init_mode(self, configuration_mode, enable_rgb_stream,
enable_depth_stream, enable_infrared_stream,
save_rgb_frames, save_depth_frames, save_infrared_frames):
if configuration_mode == self.CFG_MODE_1:
cfg = [True, True, False, False, False, False]
elif configuration_mode == self.CFG_MODE_2:
cfg = [True, False, True, False, False, False]
elif configuration_mode == self.CFG_MODE_3:
cfg = [False, False, True, False, False, False]
else:
cfg = [
enable_rgb_stream, enable_depth_stream, enable_infrared_stream,
save_rgb_frames, save_depth_frames, save_infrared_frames
]
self.enable_rgb_stream = cfg[0]
self.enable_depth_stream = cfg[1]
self.enable_infrared_stream = cfg[2]
self.save_rgb_frames = cfg[3]
self.save_depth_frames = cfg[4]
self.save_infrared_frames = cfg[5]
def _setup_parameters(self):
self.depth_range_m = [
self.cfg.d435['depth_min_m'], self.cfg.d435['depth_max_m']
]
if self.cfg.d435['filters']['threshold']:
self.filters['threshold'].set_option(rs.option.min_distance,
self.depth_range_m[0])
self.filters['threshold'].set_option(rs.option.max_distance,
self.depth_range_m[1])
self.camera_facing_angle_degree = self.cfg.d435[
'camera_facing_angle_degree']
self.device_id = None
self.obstacle_line_height_ratio = self.cfg.d435[
'obstacle_line_height_ratio']
self.obstacle_line_thickness_pixel = self.cfg.d435[
'obstacle_line_thickness_pixel']
self.depth_scale = 0
self.colorizer = rs.colorizer()
self.depth_hfov_deg = None
self.depth_vfov_deg = None
self.stream_cfg = {
'd_w': self.cfg.d435['depth_width'],
'd_h': self.cfg.d435['depth_height'],
'd_fps': self.cfg.d435['depth_fps'],
'c_w': self.cfg.d435['color_width'],
'c_h': self.cfg.d435['color_height'],
'c_fps': self.cfg.d435['color_fps'],
'i_w': self.cfg.d435['infrared_width'],
'i_h': self.cfg.d435['infrared_height'],
'i_fps': self.cfg.d435['infrared_fps']
}
self._initialize_compute_vars()
# body offset - see initial script
self.metadata = [
'enable_rgb_stream', 'enable_infrared_stream',
'enable_depth_stream'
]
def _initialize_compute_vars(self):
self.vehicle_pitch_rad = None
self.current_time_us = 0
self.last_obstacle_distance_sent_ms = 0 # value of current_time_us when obstacle_distance last sent
# Obstacle distances in front of the sensor, starting from the left in increment degrees to the right
# See here: https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE
self.min_depth_cm = int(self.depth_range_m[0] * 100) # In cm
self.max_depth_cm = int(self.depth_range_m[1] *
100) # In cm, should be a little conservative
self.distances_array_length = 72
self.angle_offset = None
self.increment_f = None
self.distances = np.ones((self.distances_array_length, ),
dtype=np.uint16) * (self.max_depth_cm + 1)
self.obstacle_distance_data = None
def _find_device_that_supports_advanced_mode(self):
ctx = rs.context()
devices = ctx.query_devices()
for dev in devices:
if dev.supports(rs.camera_info.product_id) and str(
dev.get_info(
rs.camera_info.product_id)) in self.DS5_product_ids:
name = rs.camera_info.name
if dev.supports(name):
self.logger.log_info(
"Found device that supports advanced mode: %s" %
dev.get_info(name))
self.device_id = dev.get_info(rs.camera_info.serial_number)
return dev
raise Exception("No device that supports advanced mode was found")
# Loop until we successfully enable advanced mode
def _realsense_enable_advanced_mode(self, advnc_mode):
while not advnc_mode.is_enabled():
self.logger.log_info("Trying to enable advanced mode...")
advnc_mode.toggle_advanced_mode(True)
# At this point the device will disconnect and re-connect.
self.logger.log_info("Sleeping for 5 seconds...")
time.sleep(5)
# The 'dev' object will become invalid and we need to initialize it again
dev = self._find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
self.logger.log_info(
"Advanced mode is %s"
"enabled" if advnc_mode.is_enabled() else "disabled")
# Load the settings stored in the JSON file
def _realsense_load_settings_file(self, advnc_mode, setting_file):
# Sanity checks
if os.path.isfile(setting_file):
self.logger.log_info("Setting file found: %s" % setting_file)
else:
self.logger.log_info("Cannot find setting file: %s" % setting_file)
sys.exit()
if advnc_mode.is_enabled():
self.logger.log_info("Advanced mode is enabled")
else:
self.logger.log_info("Device does not support advanced mode")
sys.exit()
# Input for load_json() is the content of the json file, not the file path
with open(setting_file, 'r') as file:
json_text = file.read().strip()
advnc_mode.load_json(json_text)
def _realsense_configure_setting(self, setting_file):
device = self._find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(device)
self._realsense_enable_advanced_mode(advnc_mode)
self._realsense_load_settings_file(advnc_mode, setting_file)
def _enable_stream(self, stype, config):
if stype not in ['depth', 'colour', 'infrared']:
raise Exception("Stream type invalid")
ix = stype[0]
if stype == 'infrared':
config.enable_stream(self.stream_type[stype], 1,
self.stream_cfg['%s_w' % ix],
self.stream_cfg['%s_h' % ix],
self.stream_format[stype],
self.stream_cfg['%s_fps' % ix])
config.enable_stream(self.stream_type[stype], 2,
self.stream_cfg['%s_w' % ix],
self.stream_cfg['%s_h' % ix],
self.stream_format[stype],
self.stream_cfg['%s_fps' % ix])
else:
config.enable_stream(self.stream_type[stype],
self.stream_cfg['%s_w' % ix],
self.stream_cfg['%s_h' % ix],
self.stream_format[stype],
self.stream_cfg['%s_fps' % ix])
return config
def _open_pipe(self):
self.pipe = rs.pipeline()
config = rs.config()
# Configure image stream(s)
if self.device_id:
# connect to a specific device ID
config.enable_device(self.device_id)
if self.enable_rgb_stream:
config = self._enable_stream('colour', config)
if self.enable_depth_stream:
config = self._enable_stream('depth', config)
if self.enable_infrared_stream:
config = self._enable_stream('infrared', config)
# Start streaming with requested config
profile = self.pipe.start(config)
if self.enable_depth_stream:
# 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()
self.logger.log_info("Depth scale is: %s" % self.depth_scale)
self._set_obstacle_distance_params()
# Setting parameters for the OBSTACLE_DISTANCE message based on actual camera's intrinsics and user-defined params
def _set_obstacle_distance_params(self):
# Obtain the intrinsics from the camera itself
profiles = self.pipe.get_active_profile()
depth_intrinsics = profiles.get_stream(
self.stream_type['depth']).as_video_stream_profile().intrinsics
self.logger.log_info("Depth camera intrinsics: %s" % depth_intrinsics)
# For forward facing camera with a horizontal wide view:
# HFOV=2*atan[w/(2.fx)],
# VFOV=2*atan[h/(2.fy)],
# DFOV=2*atan(Diag/2*f),
# Diag=sqrt(w^2 + h^2)
self.depth_hfov_deg = m.degrees(2 * m.atan(self.stream_cfg['d_w'] /
(2 * depth_intrinsics.fx)))
self.depth_vfov_deg = m.degrees(2 * m.atan(self.stream_cfg['d_h'] /
(2 * depth_intrinsics.fy)))
self.logger.log_info("Depth camera HFOV: %0.2f degrees" %
self.depth_hfov_deg)
self.logger.log_info("Depth camera VFOV: %0.2f degrees" %
self.depth_vfov_deg)
self.angle_offset = self.camera_facing_angle_degree - (
self.depth_hfov_deg / 2)
self.increment_f = self.depth_hfov_deg / self.distances_array_length
self.logger.log_info("OBSTACLE_DISTANCE angle_offset: %0.3f" %
self.angle_offset)
self.logger.log_info("OBSTACLE_DISTANCE increment_f: %0.3f" %
self.increment_f)
self.logger.log_info(
"OBSTACLE_DISTANCE coverage: from %0.3f to %0.3f degrees" %
(self.angle_offset, self.angle_offset +
self.increment_f * self.distances_array_length))
# Sanity check for depth configuration
if self.obstacle_line_height_ratio < 0 or self.obstacle_line_height_ratio > 1:
self.logger.log_info(
"Please make sure the horizontal position is within [0-1]: %s"
% self.obstacle_line_height_ratio)
sys.exit()
if self.obstacle_line_thickness_pixel < 1 or self.obstacle_line_thickness_pixel > self.stream_cfg[
'd_h']:
self.logger.log_info(
"Please make sure the thickness is within [0-depth_height]: %s"
% self.obstacle_line_thickness_pixel)
sys.exit()
# Find the height of the horizontal line to calculate the obstacle distances
# - Basis: depth camera's vertical FOV, user's input
# - Compensation: vehicle's current pitch angle
def _find_obstacle_line_height(self):
# Basic position
obstacle_line_height = self.stream_cfg[
'd_h'] * self.obstacle_line_height_ratio
attitude = self.rb_r.get_key('ATTITUDE')
if attitude is not None:
self.vehicle_pitch_rad = attitude['pitch']
# Compensate for the vehicle's pitch angle if data is available
if self.vehicle_pitch_rad is not None and self.depth_vfov_deg is not None:
delta_height = m.sin(self.vehicle_pitch_rad / 2) / m.sin(
m.radians(self.depth_vfov_deg) / 2) * self.stream_cfg['d_h']
obstacle_line_height += delta_height
# Sanity check
if obstacle_line_height < 0:
obstacle_line_height = 0
elif obstacle_line_height > self.stream_cfg['d_h']:
obstacle_line_height = self.stream_cfg['d_h']
return obstacle_line_height
# Calculate the distances array by dividing the FOV (horizontal) into $distances_array_length rays,
# then pick out the depth value at the pixel corresponding to each ray. Based on the definition of
# the MAVLink messages, the invalid distance value (below MIN/above MAX) will be replaced with MAX+1.
#
# [0] [35] [71] <- Output: distances[72]
# | | | <- step = width / 72
# --------------- <- horizontal line, or height/2
# \ | /
# \ | /
# \ | /
# \ | /
# \ | /
# \ | /
# Camera <- Input: depth_mat, obtained from depth image
#
# Note that we assume the input depth_mat is already processed by at least hole-filling filter.
# Otherwise, the output array might not be stable from frame to frame.
# @njit # Uncomment to optimize for performance. This uses numba which requires llmvlite (see instruction at the top)
def _distances_from_depth_image(self, obstacle_line_height, depth_mat):
min_depth_m = self.depth_range_m[0]
max_depth_m = self.depth_range_m[1]
# Parameters for depth image
depth_img_width = depth_mat.shape[1]
depth_img_height = depth_mat.shape[0]
# Parameters for obstacle distance message
step = depth_img_width / self.distances_array_length
for i in range(self.distances_array_length):
# Each range (left to right) is found from a set of rows within a column
# [ ] -> ignored
# [x] -> center + obstacle_line_thickness_pixel / 2
# [x] -> center = obstacle_line_height (moving up and down according to the vehicle's pitch angle)
# [x] -> center - obstacle_line_thickness_pixel / 2
# [ ] -> ignored
# ^ One of [distances_array_length] number of columns, from left to right in the image
center_pixel = obstacle_line_height
upper_pixel = center_pixel + self.obstacle_line_thickness_pixel / 2
lower_pixel = center_pixel - self.obstacle_line_thickness_pixel / 2
# Sanity checks
if upper_pixel > depth_img_height:
upper_pixel = depth_img_height
elif upper_pixel < 1:
upper_pixel = 1
if lower_pixel > depth_img_height:
lower_pixel = depth_img_height - 1
elif lower_pixel < 0:
lower_pixel = 0
# Converting depth from uint16_t unit to metric unit. depth_scale is usually 1mm following ROS convention.
# dist_m = depth_mat[int(obstacle_line_height), int(i * step)] * depth_scale
min_point_in_scan = np.min(
depth_mat[int(lower_pixel):int(upper_pixel),
int(i * step)])
dist_m = min_point_in_scan * self.depth_scale
# Default value, unless overwritten:
# A value of max_distance + 1 (cm) means no obstacle is present.
# A value of UINT16_MAX (65535) for unknown/not used.
self.distances[i] = 65535
# Note that dist_m is in meter, while distances[] is in cm.
if dist_m > min_depth_m and dist_m < max_depth_m:
self.distances[i] = dist_m * 100
if self.current_time_us == self.last_obstacle_distance_sent_ms:
# no new frame
return
self.last_obstacle_distance_sent_ms = self.current_time_us
if self.angle_offset is None or self.increment_f is None:
self.logger.log_warn(
"Please call set_obstacle_distance_params before continuing")
else:
self.obstacle_distance_data = [
self.
current_time_us, # us Timestamp (UNIX time or time since system boot)
0, # sensor_type, defined here: https://mavlink.io/en/messages/common.html#MAV_DISTANCE_SENSOR
self.distances.tolist(), # distances, uint16_t[72], cm
0, # increment, uint8_t, deg
self.min_depth_cm, # min_distance, uint16_t, cm
self.max_depth_cm, # max_distance, uint16_t, cm
self.increment_f, # increment_f, float, deg
self.angle_offset, # angle_offset, float, deg
12 # MAV_FRAME, vehicle-front aligned: https://mavlink.io/en/messages/common.html#MAV_FRAME_BODY_FRD
]
self.logger.log_debug("Captured obstacle distance data: %s" %
str(self.obstacle_distance_data))
def _setup_threads(self):
super()._setup_threads()
if self.enable_depth_stream:
self.threads.append(Thread(target=self._save_obstacle_distance))
self.threads.append(Thread(target=self._check_save_data_flags))
self.threads.append(Thread(target=self._save_rgb_frames))
self.threads.append(Thread(target=self._save_depth_frames))
self.threads.append(Thread(target=self._save_infrared_frames))
# self.threads.append(Thread(target=self._save_rgb_metadata))
# self.threads.append(Thread(target=self._save_depth_metadata))
# self.threads.append(Thread(target=self._save_ir_metadata))
def _save_obstacle_distance(self):
while not self.exit_threads:
self.rb_i.add_key(self.obstacle_distance_data,
self.camera_type,
'obstacle_distance',
expiry=self.cfg.d435['save_redis_expiry'])
def _save_distance_sensor(self): # (average or singles ?)
return
def _check_save_data_flags(self):
# Initialise keys
self.rb_i.add_key(int(self.save_rgb_frames), self.camera_type,
'save_rgb_frames')
self.rb_i.add_key(int(self.save_depth_frames), self.camera_type,
'save_depth_frames')
self.rb_i.add_key(int(self.save_infrared_frames), self.camera_type,
'save_infrared_frames')
while not self.exit_threads:
self.save_rgb_frames = bool(
self.rb_i.get_key(self.camera_type, 'save_rgb_frames'))
self.save_depth_frames = bool(
self.rb_i.get_key(self.camera_type, 'save_depth_frames'))
self.save_infrared_frames = bool(
self.rb_i.get_key(self.camera_type, 'save_infrared_frames'))
time.sleep(1)
def _save_frame(self,
csvwriter,
frame,
last_frame_time,
ref=None,
*prepend):
try:
profile = frame.get_profile()
stype = profile.stream_name()
except:
return None
if ref is None:
ref = stype.lower()
if frame:
timestamp = frame.get_timestamp()
if timestamp == last_frame_time:
return timestamp
try:
if stype == 'stream.depth':
img = np.asanyarray(frame.as_frame().get_data())
img = cv2.convertScaleAbs(img, alpha=0.03)
else:
img = np.asanyarray(frame.get_data())
filename = "%s%s.%s.png" % (self.save_data_dir, str(timestamp),
ref)
cv2.imwrite(filename, img)
self.logger.log_debug("Saved %s frame: %s" % (ref, filename))
self._save_metadata(csvwriter, frame, ref, *prepend)
except csv.Error:
self.logger.log_warn("Could not write %s metadata" % ref)
self.logger.log_debug(traceback.format_exc())
except:
self.logger.log_warn("Could not save %s frame" % ref)
self.logger.log_debug(traceback.format_exc())
return timestamp
return None
def _generate_csv_header(self, ir_stream=False):
gps_header = [
'gps_1_lat', 'gps_1_lon', 'gps_1_fix_type', 'gps_2_lat',
'gps_2_lon', 'gps_2_fix_type'
]
ir_header = ['ir_stream']
if ir_stream:
return ir_header + self.frame_metadata_values + gps_header
return self.frame_metadata_values + gps_header
def _save_metadata(self, csvwriter, frame, ref=None, *prepend):
metadata = []
if prepend:
metadata = prepend + metadata
for value in self.frame_metadata_values:
if frame.supports_frame_metadata(value):
data = frame.get_frame_metadata(value)
else:
data = ""
metadata.append(data)
csvwriter.writerow(metadata)
def _save_rgb_frames(self):
last_frame_time = None
filename = self.save_data_dir + 'rgb_metadata.csv'
file_exists = os.path.exists(filename)
with open(filename, 'a+', newline='') as csvfile:
csvwriter = csv.writer(csvfile, delimiter=',')
if not file_exists:
header = self._generate_csv_header()
csvwriter.writerow(header)
while not self.exit_threads:
if self.colour_frame_process and self.save_rgb_frames:
last_frame_time = self._save_frame(
csvwriter, self.colour_frame_process, last_frame_time)
def _save_depth_frames(self):
last_frame_time = None
filename = self.save_data_dir + 'depth_metadata.csv'
file_exists = os.path.exists(filename)
with open(filename, 'a+', newline='') as csvfile:
csvwriter = csv.writer(csvfile, delimiter=',')
if not file_exists:
header = self._generate_csv_header()
csvwriter.writerow(header)
while not self.exit_threads:
if self.depth_frame_process and self.save_depth_frames:
last_frame_time = self._save_frame(
csvwriter, self.depth_frame_process, last_frame_time)
def _save_infrared_frames(self):
last_frame_time_1 = None
last_frame_time_2 = None
filename = self.save_data_dir + 'infrared_metadata.csv'
file_exists = os.path.exists(filename)
with open(filename, 'a+', newline='') as csvfile:
csvwriter = csv.writer(csvfile, delimiter=',')
if not file_exists:
header = self._generate_csv_header(ir_stream=True)
csvwriter.writerow(header)
while not self.exit_threads:
if self.ir1_frame_process and self.save_infrared_frames:
last_frame_time_1 = self._save_frame(
csvwriter,
self.ir1_frame_process,
last_frame_time_1,
1,
ref="infrared1")
if self.ir2_frame_process and self.save_infrared_frames:
last_frame_time_2 = self._save_frame(
csvwriter,
self.ir2_frame_process,
last_frame_time_2,
2,
ref="infrared2")
def start(self):
if self.cfg.d435['use_preset_file']:
dir_path = os.environ['MYCELIUM_CFG_ROOT']
preset_file = os.path.join(dir_path, self.cfg.d435['preset_file'])
self._realsense_configure_setting(preset_file)
for t in self.threads:
t.start()
self._open_pipe()
while not self.exit_threads:
self._process_frames()
def _process_frames(self):
frames = self.pipe.wait_for_frames()
color_frame = None
depth_frame = None
ir1_frame = None
ir2_frame = None
if self.enable_depth_stream:
depth_frame = frames.get_depth_frame()
if self.enable_rgb_stream:
color_frame = frames.get_color_frame()
if self.enable_infrared_stream:
ir1_frame = frames.get_infrared_frame(1)
ir2_frame = frames.get_infrared_frame(2)
if color_frame:
self.colour_frame_process = color_frame
if ir1_frame:
self.ir1_frame_process = ir1_frame
if ir2_frame:
self.ir2_frame_process = ir2_frame
if depth_frame:
# Store the timestamp for MAVLink messages
self.current_time_us = int(round(time.time() * 1000000))
# Apply the filters
filtered_frame = depth_frame
cfg_filters = self.cfg.d435['filters']
for k, v in cfg_filters.items():
if v:
filtered_frame = self.filters[k].process(filtered_frame)
self.depth_frame_process = filtered_frame
# Extract depth in matrix form
depth_data = filtered_frame.as_frame().get_data()
depth_mat = np.asanyarray(depth_data)
# Create obstacle distance data from depth image
obstacle_line_height = self._find_obstacle_line_height()
self._distances_from_depth_image(obstacle_line_height, depth_mat)
def main():
# Declare pointcloud object, for calculating pointclouds and texture mappings
pc = rs.pointcloud()
# We want the points object to be persistent so we can display the last cloud when a frame drops
points = rs.points()
pipe = rs.pipeline()
cfg = rs.config()
rs.config.enable_device_from_file(cfg,
'./20191211_160154.bag',
repeat_playback=False)
cfg.enable_stream(rs.stream.color, 424, 240, rs.format.rgb8,
6) # color camera
cfg.enable_stream(rs.stream.depth, 424, 240, rs.format.z16,
6) # depth camera
# pipe.start(cfg)
profile = pipe.start(cfg)
playback = profile.get_device().as_playback()
playback.set_real_time(False)
# define Filters
thres_filter = rs.threshold_filter()
depth_to_disparity = rs.disparity_transform(True)
spat_filter = rs.spatial_filter()
temp_filter = rs.temporal_filter()
# hole_fill_filter = rs.hole_filling_filter()
disparity_to_depth = rs.disparity_transform(False)
i = 0
while True:
try:
frames = pipe.wait_for_frames()
except:
break
print(i)
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
# process depth image
if True:
depth_frame = thres_filter.process(depth_frame)
depth_frame = depth_to_disparity.process(depth_frame)
depth_frame = spat_filter.process(depth_frame)
depth_frame = temp_filter.process(depth_frame)
# depth_frame = hole_fill_filter.process(depth_frame)
depth_frame = disparity_to_depth.process(depth_frame)
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
points = pc.calculate(depth_frame)
cloud = points_to_pcl(points)
cloud_normals, cluster_indices = region_growing_segmentation(cloud)
idx = choose_flat_plane(cluster_indices, cloud_normals)
chosen_plane_indices = cluster_indices[idx]
arr = cloud.to_array()
plane_points = arr[chosen_plane_indices]
plane_coef = fit_plane(plane_points)
plane_filtered_indices = find_close_points(cloud, plane_coef)
processed = mark_pixels(depth_colormap, 424, plane_filtered_indices)
# Dilation
img_processed = image_post_process(processed)
cv2.imwrite("./images/proc_" + str(i) + ".png", img_processed)
i += 1
obstacle_line_height_ratio = 0.18 # [0-1]: 0-Top, 1-Bottom. The height of the horizontal line to find distance to obstacle.
obstacle_line_thickness_pixel = 10 # [1-DEPTH_HEIGHT]: Number of pixel rows to use to generate the obstacle distance message. For each column, the scan will return the minimum value for those pixels centered vertically in the image.
USE_PRESET_FILE = True
PRESET_FILE = "../cfg/d4xx-default.json"
RTSP_STREAMING_ENABLE = True
RTSP_PORT = "8554"
RTSP_MOUNT_POINT = "/d4xx"
# List of filters to be applied, in this order.
# https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md
filters = [
[True, "Decimation Filter", rs.decimation_filter()],
[True, "Threshold Filter", rs.threshold_filter()],
[True, "Depth to Disparity", rs.disparity_transform(True)],
[True, "Spatial Filter", rs.spatial_filter()],
[True, "Temporal Filter", rs.temporal_filter()],
[False, "Hole Filling Filter", rs.hole_filling_filter()],
[True, "Disparity to Depth", rs.disparity_transform(False)]
]
#
# The filters can be tuned with opencv_depth_filtering.py script, and save the default values to here
# Individual filters have different options so one have to apply the values accordingly
#
# decimation_magnitude = 8
# filters[0][2].set_option(rs.option.filter_magnitude, decimation_magnitude)
def threshold(self, frame, minDistance, maxDistance):
#apply threshold filter to a depth image from the min distance to max distance to be viewed
threshold_filter = rs.threshold_filter(minDistance, maxDistance)
return threshold_filter.process(frame)
def print_hi(name):
# Configure depth and color streams
pipeline = rs.pipeline()
colorizer = rs.colorizer()
threshold_distance = 0.4
tr1 = rs.threshold_filter(min_dist=0.15, max_dist=threshold_distance)
config = rs.config()
# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
depth_sensor = pipeline_profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
if device_product_line == 'L500':
config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
else:
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
clipping_distance_in_meters = 0.9
clipping_distance = clipping_distance_in_meters / depth_scale
align_to = rs.stream.color
align = rs.align(align_to)
pipeline.start(config)
#thresholds for detect skins
lower = np.array([0, 48, 80], dtype="uint8")
upper = np.array([20, 255, 255], dtype="uint8")
try:
while True:
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame(
) # aligned_depth_frame is a 640x480 depth image
color_frame = aligned_frames.get_color_frame()
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Remove background - Set pixels further than clipping_distance to grey
grey_color = 153
depth_image_3d = np.dstack(
(depth_image, depth_image,
depth_image)) # depth image is 1 channel, color is 3 channels
bg_removed = np.where(
(depth_image_3d > clipping_distance) | (depth_image_3d <= 0),
grey_color, color_image)
# Render images
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
images = np.hstack((bg_removed, depth_colormap))
# cv2.namedWindow('Background Removed', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Color+depth', images)
# images[270:, :] = [0, 0, 0]
# images[0:240,:] = [0,0,0]
# images[:, 0:50] = [0, 0, 0]
# images[:, 630:] = [0, 0, 0]
img_hsv = cv2.cvtColor(bg_removed, cv2.COLOR_BGR2HSV)
## Gen lower mask (0-5) and upper mask (175-180) of RED
lower_red = np.array([94, 80, 2], dtype="uint8")
upper_red = np.array([126, 255, 255], dtype="uint8")
mask = cv2.inRange(img_hsv, lower_red, upper_red)
bluepen = cv2.bitwise_and(bg_removed, bg_removed, mask=mask)
cv2.imshow('BluePen', bluepen)
bgModel = cv2.createBackgroundSubtractorMOG2(0, 50)
fgmask = bgModel.apply(bluepen)
kernel = np.ones((3, 3), np.uint8)
fgmask = cv2.erode(fgmask, kernel, iterations=1)
img = cv2.bitwise_and(bluepen, bluepen, mask=fgmask)
# Skin detect and thresholding
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower = np.array([94, 80, 2], dtype="uint8")
upper = np.array([126, 255, 255], dtype="uint8")
skinMask = cv2.inRange(hsv, lower, upper)
kernel = np.ones((3, 3), np.uint8)
skinMask = cv2.erode(skinMask, kernel, iterations=2)
skinMask = cv2.dilate(skinMask, kernel, iterations=1)
cv2.imshow('Threshold Hands', skinMask)
contours, hierarchy = cv2.findContours(skinMask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
length = len(contours)
maxArea = -1
drawing = np.zeros(img.shape, np.uint8)
if length > 0:
for i in xrange(length):
temp = contours[i]
area = cv2.contourArea(temp)
if area > maxArea:
maxArea = area
ci = i
res = contours[ci]
hull = cv2.convexHull(res)
# drawing = np.zeros(img.shape, np.uint8)
cx, cy = centroid(res)
print(aligned_depth_frame.get_distance(cx, cy))
cv2.drawContours(drawing, [res], 0, (0, 255, 0), 2)
else:
drawing = np.zeros(img.shape, np.uint8)
cv2.imshow('DRAWING', drawing)
# bluepen_gray = cv2.cvtColor(bluepen, cv2.COLOR_BGR2HSV)
# contours, hierarchy = cv2.findContours(bluepen_gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# length = len(contours)
# drawing = np.zeros(bluepen_gray.shape, np.uint8)
# maxArea = -1
# if length > 0:
# for i in xrange(length):
# temp = contours[i]
# area = cv2.contourArea(temp)
# if area > maxArea:
# maxArea = area
# ci = i
# res1 = contours[ci]
#
# hull = cv2.convexHull(res1)
#
# cv2.drawContours(drawing, [res1], 0, (0, 255, 0), 2)
# cv2.drawContours(drawing, [hull], 0, (0, 0, 255), 3)
# cv2.imshow('',drawing)
# length = len(contours)
# print(length)
# drawing = np.zeros(res.shape, np.uint8)
# maxArea = -1
# if length > 0:
# for i in xrange(length):
# temp = contours[i]
# area = cv2.contourArea(temp)
# if area > maxArea:
# maxArea = area
# ci = i
# res = contours[ci]
#
# hull = cv2.convexHull(res)
#
#
# cv2.drawContours(drawing, [res], 0, (0, 255, 0), 2)
# cv2.drawContours(drawing, [hull], 0, (0, 0, 255), 3)
# cv2.imshow('Only contour for calibration', drawing)
# cv2.imshow('BluePen', res)
# converted = cv2.cvtColor(images, cv2.COLOR_BGR2HSV)
# skinMask = cv2.inRange(converted, lower, upper)
# kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
# skinMask = cv2.erode(skinMask, kernel, iterations=2)
# skinMask = cv2.dilate(skinMask, kernel, iterations=1)
#
# skinMask = cv2.GaussianBlur(skinMask, (5, 5), 0)
# skin = cv2.bitwise_and(images, images, mask=skinMask)
cv2.waitKey(1)
# img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# ## Gen lower mask (0-5) and upper mask (175-180) of RED
# mask1 = cv2.inRange(img_hsv, (0, 50, 20), (5, 255, 255))
# mask2 = cv2.inRange(img_hsv, (175, 50, 20), (180, 255, 255))
#
# ## Merge the mask and crop the red regions
# mask = cv2.bitwise_or(mask1, mask2)
# croped = cv2.bitwise_and(img, img, mask=mask)
finally:
# Stop streaming
pipeline.stop()
# threshold_filter = rs.threshold_filter()
# threshold_filter.set_option(rs.option.max_distance, 1)
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades +
'haarcascade_frontalface_default.xml')
colorizer = rs.colorizer(3)
align_to = rs.stream.color
align = rs.align(align_to)
spatialFilter= rs.spatial_filter(.25, 50, 5, 1)
disparity = rs.disparity_transform(True)
kernel = np.ones((3,3),np.uint8)
try:
x = 0
while x < 10:
start = timer()
#x = (.5 * start) - .49
#print(x)
thresholdFilter = rs.threshold_filter(2 + x, 5 + x)
try:
frames = pipeline.wait_for_frames()
frames = align.process(frames)
timeStamp = frames.get_timestamp() / 1000
except:
break
#get data
motion.get_data(frames, timeStamp)
depthFrame = frames.get_depth_frame()
#apply rs filters
depthFrame1 = thresholdFilter.process(depthFrame)
depthFrame1 = disparity.process(depthFrame1)
depthFrame1 = spatialFilter.process(depthFrame1)
#####################################################
## Export to PLY ##
#####################################################
# First import the library
import pyrealsense2 as rs
import open3d as o3d
from open3d.open3d.geometry import PointCloud
from open3d.open3d.geometry import OrientedBoundingBox
import numpy as np
import time
pc = rs.pointcloud()
pipe = rs.pipeline()
cfg = rs.config()
thre = rs.threshold_filter(0.1, 2)
align = rs.align(rs.stream.color)
profile = pipe.start(cfg)
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
for i in range(10):
pipe.wait_for_frames()
frames = pipe.wait_for_frames()
aligned_frames = align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
filtered = thre.process(depth_frame)
def create_filter(self):
self.th_filter = rs.threshold_filter(max_dist=self.max_dist)
self.sp_filter = rs.spatial_filter()
self.sp_filter.set_option(rs.option.filter_magnitude, 3.0)
self.sp_filter.set_option(rs.option.holes_fill, 2.0)
self.tmp_filter = rs.temporal_filter()
import multiprocessing
from statistics import mean
import simpleaudio as sa
import wave
# Declare RealSense pipeline, encapsulating the actual device and sensors.
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)
pipe = rs.pipeline()
pipe.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
threshold = rs.threshold_filter()
threshold.set_option(rs.option.max_distance, 10)
#_________________________________________________________________________
# Stereo Audio:
# get timesteps for each sample, T is note duration in seconds
sample_rate = 44100
#T = 0.2272753623188406
T = 0.227275362
valor = sample_rate * T
t = np.linspace(0, T, int(valor), False)
def stereo_beep(left, right):
global t
def ai():
# Configure depth and color streams
test_img = cv2.imread('testImag.jpg')
pipeline = rs.pipeline()
config = rs.config()
colorizer = rs.colorizer()
config.enable_stream(rs.stream.depth, 480, 270, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 424, 240, rs.format.bgr8, 15)
model_file = 'mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite'
labels = read_label_file('coco_labels.txt')
interpreter = make_interpreter(model_file)
interpreter.allocate_tensors()
## align = rs.align(rs.stream.color)
dist_thres_cm = 180 # cm
max_depth_m = 8
min_depth_m = 0.1
confThreshold = 0.65
confidence = 0
off_ul = 15
off_vl = 10
off_ur = 25
off_vr = 15
corn_1 = (72, 44)
corn_2 = (168, 92)
cornColor_1 = (corn_1[0] - off_ul, corn_1[1] - off_vl
) # compensate # FOV cameras
cornColor_2 = (corn_2[0] + off_ur, corn_2[1] + off_vr)
filters = [[True, "Decimation Filter",
rs.decimation_filter()],
[True, "Threshold Filter",
rs.threshold_filter()],
[True, "Depth to Disparity",
rs.disparity_transform(True)],
[True, "Spatial Filter",
rs.spatial_filter()],
[True, "Temporal Filter",
rs.temporal_filter()],
[True, "Hole Filling Filter",
rs.hole_filling_filter(True)],
[True, "Disparity to Depth",
rs.disparity_transform(False)]]
if filters[1][0] is True:
filters[1][2].set_option(rs.option.min_distance, min_depth_m)
filters[1][2].set_option(rs.option.max_distance, max_depth_m)
# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
progress("INFO: Depth scale is: %.4f" % depth_scale)
progress("INFO: video recording")
out = cv2.VideoWriter('avcnet.avi', cv2.VideoWriter_fourcc(*'XVID'), 15,
(240, 136))
# default parameters
orient = 0
tim_old = 0.1
state = 'fly'
global velocity_y
velocity_y = 0
fly_1 = 400
k = 0.66 # k=(tau/T)/(1+tau/T) tau time constant LPF, T period
# k=0 # no filtering
fps = FPS().start()
# Start streaming
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
## frames = align.process(frames)
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Apply the filters
filtered_frame = depth_frame
for i in range(len(filters)):
if filters[i][0] is True:
filtered_frame = filters[i][2].process(filtered_frame)
# Extract depth in matrix form
depth_data = filtered_frame.as_frame().get_data()
depth_mat = np.asanyarray(depth_data) # shape 136,240
# Convert images to numpy arrays
# output_image = np.asanyarray(colorizer.colorize(filtered_frame).get_data()) #shape: 136,240,3
color_image = np.asanyarray(color_frame.get_data())
# calculate distance
distances = distances_depth_image(depth_mat, min_depth_m,
max_depth_m, depth_scale, corn_1,
corn_2)
# Stack both images horizontally
# output_color = cv2.resize(color_image, (240, 136))
if color_frame:
imag = cv2.resize(color_image, (300, 300))
common.set_input(interpreter, imag)
interpreter.invoke()
scale = (1, 1)
objects = detect.get_objects(interpreter, confThreshold, scale)
data_out = []
if objects:
for obj in objects:
inference = [] # clear inference
box = obj.bbox
inference.extend((obj.id, obj.score, box))
# print('inference:',inference)
data_out.append(
inference) # list of all detected objects
# print('data_out:',data_out)
objID = data_out[0][
0] # object with largest confidence selected
confidence = data_out[0][1]
labeltxt = labels[objID]
box = data_out[0][2]
if confidence > confThreshold:
draw_rect(imag,
box,
confidence,
labeltxt,
use_normalized_coordinates=False)
output_color = cv2.resize(imag, (240, 136))
# cv2.rectangle(output_image, corn_1, corn_2, (0, 255, 0), thickness=2)
cv2.rectangle(output_color,
cornColor_1,
cornColor_2, (0, 255, 0),
thickness=2)
#===========================================================================
# classify the input image
fly = np.min(distances) # distance in cm
left = (distances[0:24] <
dist_thres_cm).sum() # object is on the left side
right = (distances[24:48] <
dist_thres_cm).sum() # object is on the right side
stop = ((distances[0:48] < dist_thres_cm).sum() == 48) * 48
# build the label
my_dict = {'left': left, 'right': right, 'stop': stop}
maxPair = max(my_dict.items(), key=itemgetter(1))
fly_f = k * fly_1 + (1 - k) * fly
fly_1 = fly_f
proba = int(fly_f)
if state == 'avoid':
if fly_f >= dist_thres_cm + 10:
state = 'fly'
print(state, int(fly_f), file=f)
else:
label = 'forward'
# proba=fly_f
if fly_f <= dist_thres_cm:
label = maxPair[0]
# proba=int(fly_f)
print(my_dict, int(fly_f), file=f)
state = 'avoid'
label_1 = "{} {} {}".format(label, proba, state)
# draw the label on the image
cv2.putText(output_color, label_1, (10, 25),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# Write the frame into the file 'output.avi'
out.write(output_color)
if vehicle.channels['8'] > 1400:
if state == "fly":
event.clear()
if state == "avoid":
event.set()
if label == 'left':
velocity_y = 0.8
if label == 'right':
velocity_y = -0.8
if label == 'stop':
velocity_y = 0
if vehicle.channels['8'] < 1400:
event.clear()
# show the output frame
cv2.imshow("Frame", output_color)
key = cv2.waitKey(10) & 0xFF
# update the FPS counter
fps.update()
# if the `Esc` key was pressed, break from the loop
if key == 27:
break
finally:
# Stop streaming
pipeline.stop()
# do a bit of cleanup
# stop the timer and save FPS information
fps.stop()
progress("INFO: elapsed time: {:.2f}".format(fps.elapsed()))
progress("INFO: approx. FPS: {:.2f}".format(fps.fps()))
print("[INFO] elapsed time: {:.2f}".format(fps.elapsed()), file=f)
print("[INFO] approx. FPS: {:.2f}".format(fps.fps()), file=f)
f.close()
progress('INFO:end')
time.sleep(3)
cv2.destroyAllWindows()
out.release()
