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
if args.record_rosbag or args.record_imgs:
depth_sensor.set_option(rs.option.visual_preset, Preset.HighAccuracy)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_scale = depth_sensor.get_depth_scale()
# We will not display the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 3 # 3 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# 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
align = rs.align(align_to)
# Streaming loop
frame_count = 0
try:
while True:
# Get frameset of color and depth
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()
color_frame = aligned_frames.get_color_frame()
def main(args):
if args.t: # read tracking datas
Pubs = JointPub()
tracking_datas = []
with open("/home/pku-hr6/yyf_ws/data/arm_running.txt", 'r') as rf:
lines = rf.readlines()
for line in lines:
datas = line.split(" ")
datas = [float(data) for data in datas]
print(datas)
tracking_datas.append(datas)
else:
rospy.init_node('data_saver')
data_nums = args.n
file_names = args.file
datas = []
save_files = os.path.join("./data", file_names + '.txt')
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
pc = rs.pointcloud()
# 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))
# device_product_line is SR300
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
pipeline.start(config)
# get scale of depth sensor
depth_sensor = pipeline_profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 0.5 # 0.5 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# creat align map
align_to = rs.stream.color
align = rs.align(align_to)
l_h = 170
l_s = 42
l_v = 41
u_h = 185
u_s = 255
u_v = 255
l_b = np.array([l_h, l_s, l_v])
u_b = np.array([u_h, u_s, u_v])
count = 0
with open(save_files, "w") as wf:
# while count < data_nums:
while len(datas) < data_nums:
# while True:
# Wait for a coherent pair of frames: depth and color
if args.t:
if count >= len(tracking_datas) - 1:
break
frames = pipeline.wait_for_frames()
aligned_frame = align.process(frames)
depth_frame = aligned_frame.get_depth_frame()
color_frame = aligned_frame.get_color_frame()
if not depth_frame or not color_frame:
continue
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# remove background
grey_color = 153
depth_image_3d = np.dstack(
(depth_image, depth_image,
depth_image)) # depth img is 1 channel, color is 3 channels
bg_rmvd = np.where(
(depth_image_3d > clipping_distance) | (depth_image_3d <= 0),
grey_color, color_image)
# get final img
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
hsv_map = cv2.cvtColor(bg_rmvd, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_map, l_b, u_b)
res = cv2.bitwise_and(bg_rmvd, bg_rmvd, mask=mask)
img = np.hstack((bg_rmvd, depth_colormap))
# get object from mask map and calculate position
mask_index = np.nonzero(mask)
valid_p = False
depth_point = np.array([])
if not mask_index[0].shape[0] == 0:
valid_p = True
x_index = int(np.median(mask_index[1]))
y_index = int(np.median(mask_index[0]))
x_min = x_index - 20
x_max = x_index + 20
y_min = y_index - 20
y_max = y_index + 20
# Intrinsics & Extrinsics
depth_intrin = depth_frame.profile.as_video_stream_profile(
).intrinsics
# print(depth_intrin)
# 640x480 p[314.696 243.657] f[615.932 615.932]
depth_pixel = [x_index, y_index]
dist2obj = depth_frame.get_distance(x_index, y_index)
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, depth_pixel, dist2obj)
depth_point = [float(dep) * 100
for dep in depth_point] # using cm
depth_point = [
depth_point[2], -depth_point[0], -depth_point[1]
] #[x,y,z](in base) = [z,-x,-y](in camera)
depth_point = [
depth_point[0] + 1.8, depth_point[1], depth_point[2] + 5.3
]
txt = "({:.2f},{:.2f},{:.2f})".format(depth_point[0],
depth_point[1],
depth_point[2])
# print(txt)
cv2.rectangle(res, (x_min, y_min), (x_max, y_max), (255, 0, 0),
2)
cv2.putText(res, txt, (x_index, y_index),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1)
cv2.imshow('Align Example', img)
cv2.imshow("mask", mask)
cv2.imshow("res", res)
# print(depth_point)
# msg = rospy.wait_for_message('/motor_states/pan_tilt_port',MotorStateList,timeout=10)
# # print(len(msg.motor_states))
# joints,valid_q = get_joint_angle(msg)
# if valid_p and valid_q:
# depth_point = np.array(depth_point)
# # print(joints)
# # print(depth_point)
# if args.only_q:
# data = joints
# else:
# data = np.append(depth_point, joints)
# print(data)
# datas.append(data)
# if args.t: # pubulish joint
# data = tracking_datas[count]
# Pubs.publish_jointsD(data)
# count = count + 1
if cv2.waitKey(100) & 0xFF == ord('q'): # quit
break
for data in datas:
for i in range(len(data)):
wf.write(str(round(data[i], 2)))
if i != len(data) - 1:
wf.write(" ")
else:
wf.write("\n")
def deviceInitial(self):
self.deviceDetect()
self.pipeline = rs.pipeline()
self.config = rs.config()
# Configure depth and color streams
if self.options.enableColor is True:
self.config.enable_stream(
rs.stream.color,
self.options.resColor[0],
self.options.resColor[1],
rs.format.bgr8,
30,
)
if self.options.enableInfrared is True:
self.config.enable_stream(
rs.stream.infrared,
self.options.resInfrared[0],
self.options.resInfrared[1],
rs.format.bgr8,
30,
)
if self.options.enableDepth is True:
self.config.enable_stream(
rs.stream.depth,
self.options.resDepth[0],
self.options.resDepth[1],
rs.format.z16,
30,
)
# Start streaming
cfg = self.pipeline.start(self.config)
# Alignment
if self.options.enableAlign is True:
if self.options.enableColor is True:
align_to = rs.stream.color
elif self.options.enableInfrared is True:
align_to = rs.stream.infrared
self.align = rs.align(align_to)
# Advanced settings
dev = cfg.get_device()
depth_sensor = dev.first_depth_sensor()
if self.options.enableDepth is True and self.options.enablePost is True:
depth_sensor.set_option(rs.option.visual_preset, 4)
if self.options.enableIntrin is True:
self.scale = depth_sensor.get_depth_scale()
# Get intrinsics
if self.options.enableColor is True:
stream = cfg.get_stream(rs.stream.color)
profile = stream.as_video_stream_profile()
elif self.options.enableInfrared is True:
stream = cfg.get_stream(rs.stream.infrared)
profile = stream.as_video_stream_profile()
self.intrin = profile.get_intrinsics()
def main():
default_model_dir = '../all_models'
default_model = 'mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite'
default_labels = 'coco_labels.txt'
parser = argparse.ArgumentParser()
parser.add_argument('--model',
help='.tflite model path',
default=os.path.join(default_model_dir, default_model))
parser.add_argument('--labels',
help='label file path',
default=os.path.join(default_model_dir,
default_labels))
parser.add_argument(
'--top_k',
type=int,
default=3,
help='number of categories with highest score to display')
parser.add_argument('--camera_idx',
type=str,
help='Index of which video source to use. ',
default=0)
parser.add_argument('--threshold',
type=float,
default=0.1,
help='classifier score threshold')
args = parser.parse_args()
print('Loading {} with {} labels.'.format(args.model, args.labels))
interpreter = common.make_interpreter(args.model)
interpreter.allocate_tensors()
labels = load_labels(args.labels)
# Configure depth and color streams
pipeline = rs.pipeline()
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
profile = pipeline.start(config)
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
color = np.asanyarray(color_frame.get_data())
colorizer = rs.colorizer()
colorized_depth = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
# Create alignment primitive with color as its target stream:
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Update color and depth frames:
aligned_depth_frame = frames.get_depth_frame()
colorized_depth = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
cv2_im = color
cv2_im_rgb = cv2.cvtColor(cv2_im, cv2.COLOR_BGR2RGB)
pil_im = Image.fromarray(cv2_im_rgb)
start = time.monotonic()
common.set_input(interpreter, pil_im)
interpreter.invoke()
objs = get_output(interpreter,
score_threshold=args.threshold,
top_k=args.top_k)
inference_time = time.monotonic() - start
inference_time = 'Inference time: %.2f ms (%.2f fps)' % (
inference_time * 1000, 1.0 / inference_time)
color = append_objs_to_img(color, objs, labels, inference_time)
# Calculate the distance
depth_scale = profile.get_device().first_depth_sensor(
).get_depth_scale()
color = calculate_distance(color, aligned_depth_frame, objs,
labels, depth_scale)
# Stack both images horizontally
#images = np.hstack((color, colorized_depth))
#cv2.imwrite("image.jpg", images)
cv2.imshow('frame', color)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
finally:
# Stop streaming
pipeline.stop()
def run_live(ingredient_detector):
data_pusher = DataPusher()
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.z16, 30)
config.enable_stream(rs.stream.color, REALSENSE_WIDTH, REALSENSE_HEIGHT, rs.format.bgr8, 30)
align_to = rs.stream.color
align = rs.align(align_to)
# Start streaming
pipeline.start(config)
colorizer = rs.colorizer()
hole_filler = rs.hole_filling_filter()
# Skip 5 first frames to give the Auto-Exposure time to adjust
for x in range(5):
pipeline.wait_for_frames()
while True:
frames = pipeline.wait_for_frames(timeout_ms=5000)
aligned_frames = align.process(frames)
color = aligned_frames.get_color_frame()
depth = aligned_frames.get_depth_frame()
# depth = hole_filler.process(depth)
# depth2 = depth.get_distance(100, 100)
# print(depth2)
if not depth or not color:
continue
depth_frame_data = np.asanyarray(depth.get_data())
color_frame_data = np.asanyarray(color.get_data())
depth_image = np.asanyarray(colorizer.colorize(depth).get_data(), dtype=np.uint8)
# depth_image = np.asanyarray(depth.get_data(), dtype=np.uint8)
color_image = np.asanyarray(color.get_data(), dtype=np.uint8)
color_image_with_overlay = np.copy(color_image)
# cv.imwrite("rgb.jpg", color_image)
# cv.imwrite("depth.jpg", depth_image)
process_colour_image(color_image_with_overlay, ingredient_detector)
circle_detector.draw_segmented_circle(color_image_with_overlay)
if cv.waitKey(1) & 0xFF == ord(' '):
# np.save("colour.txt", color_image)
# np.save("depth.txt", np.asanyarray(depth.get_data(), dtype=np.uint8))
# r = color_image[:, :, 0]
# g = color_image[:, :, 1]
# b = color_image[:, :, 2]
# np.savetxt("colour-r.txt", r)
# np.savetxt("colour-g.txt", g)
# np.savetxt("colour-b.txt", b)
# np.savetxt("depth.txt", np.asanyarray(depth.get_data(), dtype=np.uint8))
scan_request = ScanRequest(color_frame_data, depth_frame_data, 1, 1)
data_pusher.push_scan(scan_request)
check_for_key()
cv.imshow("Depth", depth_image)
cv.imshow("RGB", color_image_with_overlay)
sleep(0.2)
def run(self):
try:
if self.motor_control is not None:
ofile = open(self.prefix_filename + 'alphamap.csv', 'w')
writer = csv.writer(ofile)
print('Real sense thread is starting up')
advanced.set_adv()
pipeline = rs.pipeline()
cnt = rs.context()
devs = cnt.query_devices()
d = devs.front()
print(devs.size())
serial_no = d.get_info(rs.camera_info(1))
print(serial_no)
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8,
15)
#config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 10)
#config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 10)
time.sleep(1)
pipeline.start(config)
print('Camera is warming up')
time.sleep(6)
pointcloud = rs.pointcloud()
#Make an align object. Allows us to align depth to color
align_to = rs.stream.color
align = rs.align(align_to)
print('Done initializing real sense pipeline')
if self.motor_control is not None:
print('Initializing Arduino board')
self.motor_control.pipeline = pipeline
self.motor_control.setup()
print('Done initializing Arduino board')
count = 1
while True:
if self.motor_control is not None:
pos = self.motor_control.nextPos()
for (m_nr, p) in enumerate(pos):
print('Motor', m_nr, ': ', p)
self.motor_filename = str(count) + '_' + serial_no
writer.writerow([count] + pos)
frames = pipeline.wait_for_frames()
depth = frames.get_depth_frame()
color = frames.get_color_frame()
pointcloud.map_to(color)
points = pointcloud.calculate(depth)
width = color.get_width()
height = color.get_height()
external_format = GL_RGB
if color.get_profile().format() is rs.format.y8:
external_format = GL_LUMINANCE
external_type = GL_UNSIGNED_BYTE
pixels = np.asanyarray(color.get_data())
# 2018-01-14 Kenny: This is the old librealsense2 interface
#coordinates = np.asanyarray(points.get_vertices())
#uvs = np.asanyarray(points.get_texture_coordinates())
coords = np.asanyarray(points.get_vertices_EXT(),
dtype=np.float32)
texs = np.asanyarray(points.get_texture_coordinates_EXT(),
dtype=np.float32)
vertex_array = np.hstack((coords, texs))
filename = self.prefix_filename + 'frame' + str(
count) + self.postfix_filename
if self.motor_control is not None:
filename = self.prefix_filename + self.motor_filename + self.postfix_filename
if self.save_color:
imsave(filename + 'color.tif', pixels)
if self.save_texture:
texture = np.asanyarray(
points.get_texture_coordinates_EXT())
imsave(filename + 'texture.tif', texture)
if self.save_ply:
points.export_to_ply(filename + '.ply', color)
if self.save_depth:
is_aligned = False
while not is_aligned:
aligned_frames = align.process(frames)
try:
aligned_depth_frame = aligned_frames.get_depth_frame(
)
depth_image = np.asanyarray(
aligned_depth_frame.get_data())
is_aligned = True
except:
pass
imsave(filename + 'depth.tif', depth_image)
if self.render is not None:
self.render.copy_data(vertex_array, width, height,
external_format, external_type,
pixels)
print('frame', count, 'done')
count = count + 1
if not threading.main_thread().is_alive():
print('Main thread is dead, closing down sensor')
ofile.close()
pipeline.stop()
if self.bot is not None:
self.bot.end('')
return
except Exception as e:
print(e)
pipeline.stop()
ofile.close()
if self.bot is not None:
self.bot.end(e)
return
def kedalaman(self, frames):
self.align = rs.align(rs.stream.color)
frames = self.align.process(frames)
aligned_depth_frame = frames.get_depth_frame()
depth_real = np.asanyarray(aligned_depth_frame.get_data())
return depth_real
def Realsense(self):
#Realsense
global color_image
#이미지 가져옴
pipeline = rs.pipeline()
#설정 파일 생성
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)
#설정을 적용하여 이미지 취득 시작, 프로파일 얻음
profile = pipeline.start(config)
#깊이 센서를 얻음
depth_sensor = profile.get_device().first_depth_sensor()
#깊이 센서의 깊이 스케일 얻음
depth_scale = depth_sensor.get_depth_scale()
#print("Depth Scale is: ", depth_scale)
#1 meter, 클리핑할 영역을 1m로 설정
clipping_distance_in_meters = 1
clipping_distance = clipping_distance_in_meters / depth_scale #스케일에 따른 클리핑 거리
#depth 이미지를 맞추기 위한 이미지, 컬러 이미지
align_to = rs.stream.color
#depth 이미지와 맞추기 위해 align 생성
align = rs.align(align_to)
#try:
while True:
#color와 depth의 프레임셋을 기다림
frames = pipeline.wait_for_frames()
#frames.get_depth_frame() 은 640x360 depth 이미지이다.
#모든(depth 포함) 프레임을 컬러 프레임에 맞추어 반환
aligned_frames = align.process(frames)
#aligned depth 프레임은 640x480 의 depth 이미지이다.
aligned_depth_frame = aligned_frames.get_depth_frame()
#컬러 프레임을 얻음
color_frame = aligned_frames.get_color_frame()
#프레임이 없으면, 건너 뜀
if not aligned_depth_frame or not color_frame:
continue
#depth이미지를 배열로
depth_image = np.asanyarray(aligned_depth_frame.get_data())
#color 이미지를 배열로
color_image = np.asanyarray(color_frame.get_data())
#글자 표시
color = (0, 255, 255)
thickness = 2
location = (250, 30)
font = cv2.FONT_HERSHEY_COMPLEX
fontScale = 1.0
cv2.putText(color_image, 'q : Quit', location, font, fontScale,
color, thickness)
#이미지 윈도우 정의
cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)
#이미지를 넣어 윈도우에 보임
cv2.imshow('Align Example', color_image)
key = cv2.waitKey(1)
#if key & 0xFF == ord('s'):
# cv2.imwrite('./calibration_data/{0}.jpg'.format(i), color_image)
# cv2.destroyAllWindows()
#q를 누르면, 나간다.
if key & 0xFF == ord('q') or key == 27:
#윈도우 제거
cv2.destroyAllWindows()
break
def start_pipeline():
save_path = "./out"
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)
pipeline.start(config)
print('[INFO] Starting pipeline')
profile = pipeline.get_active_profile()
# profile = pipeline.start(config)
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
print("[INFO] Width: ", w, "Height: ", h)
fx, fy, = depth_intrinsics.fx, depth_intrinsics.fy
print("[INFO] Focal length X: ", fx)
print("[INFO] Focal length Y: ", fy)
ppx, ppy = depth_intrinsics.ppx, depth_intrinsics.ppy
print("[INFO] Principal point X: ", ppx)
print("[INFO] Principal point Y: ", ppy)
# Identifying depth scaling factor
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
pc = rs.pointcloud()
decimate = rs.decimation_filter()
spatial = rs.spatial_filter
hole = rs.hole_filling_filter
temporal = rs.temporal_filter()
colorizer = rs.colorizer()
align_to = rs.stream.color
align = rs.align(align_to)
frame_counter = 0
try:
print("[INFO] Starting pipeline stream with frame {:d}".format(
frame_counter))
while True:
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
aligned_depth_frame = aligned_frames.get_depth_frame()
aligned_color_frame = aligned_frames.get_color_frame()
depth_colormap = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
color_image = np.asanyarray(aligned_color_frame.get_data())
pts = pc.calculate(aligned_depth_frame)
pc.map_to(aligned_color_frame)
if not aligned_depth_frame or not aligned_color_frame:
print("No depth or color frame, try again...")
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data())
images = np.hstack(
(cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB), depth_colormap))
cv2.imshow(str(frame_counter), images)
key = cv2.waitKey(1)
if key == ord("d"):
landmarks = landmark_detector(color_image)
for n, px in landmarks.items():
z = aligned_depth_frame.get_distance(px[0], px[1])
# Test Landmarks px = [width, height]
pt = rs.rs2_deproject_pixel_to_point(
depth_intrinsics, [px[0], px[1]], z)
w, h = rs.rs2_project_point_to_pixel(depth_intrinsics, pt)
print("Original Pixel:", px)
print("Deprojected Pixel:", [w, h])
return px, w, h
except Exception as ex:
print(ex)
finally:
pipeline.stop()
def main():
if not os.path.exists(args.output):
os.mkdir(args.output)
data_id = len(os.listdir(args.output))
data_path = os.path.join(args.output, '{:04d}'.format(data_id))
color_path = os.path.join(data_path, 'color')
depth_path = os.path.join(data_path, 'depth')
height_map_color_path = os.path.join(data_path, 'height_map_color')
height_map_depth_path = os.path.join(data_path, 'height_map_depth')
label_path = os.path.join(data_path, 'label')
os.mkdir(data_path)
os.mkdir(color_path)
os.mkdir(depth_path)
os.mkdir(height_map_color_path)
os.mkdir(height_map_depth_path)
os.mkdir(label_path)
# initialize urx
rob = urx.Robot(args.ip)
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
gripper = urx.RobotiqGripper(port=args.port)
gripper.activation_request()
gripper.set_speed(0x40)
gripper.set_force(0x80)
# initialize realsense
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
pipeline.start(config)
align = rs.align(rs.stream.color)
i = 57
while i:
_ = pipeline.wait_for_frames()
i -= 1
# load configuration
camera_location = np.loadtxt(args.camera_location)
rob.movel(camera_location, acc=args.a, vel=args.v)
init_angle = rob.getj()[-1]
origin = np.loadtxt(args.origin)
mtx = np.loadtxt(args.projection_matrix)
b_depth_height_map = cv2.imread(args.depth_dir, cv2.IMREAD_ANYDEPTH)
cv2.imwrite(data_path + '/background_depth_height_map.png',
b_depth_height_map)
b_depth_height_map = b_depth_height_map.astype(np.float32)
f = open(os.path.join(data_path, 'file_name.txt'), 'w')
k = 0
num_success = 0
num_fail = 0
try:
for i in range(0, 10):
print('--------i= %s' % i +
'. Attempt to grasp without knowledge--------')
# align the depth frame to color frame
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.first(rs.stream.color)
depth_frame = aligned_frames.get_depth_frame()
color_img = np.asanyarray(color_frame.get_data())
depth_img = (np.asanyarray(depth_frame.get_data()) *
args.z_scale).astype(np.uint16)
warp_color_img = cv2.warpPerspective(color_img, mtx,
(args.width, args.height))
warp_depth_img = cv2.warpPerspective(
depth_img, mtx, (args.width, args.height)).astype(np.float32)
# get center point of object chosen among objects seen by the camera
center_point, rect, object_pts, z = get_center_point(
warp_color_img, warp_depth_img, b_depth_height_map)
episode = []
# approach to the target pose
pose1 = copy.deepcopy(origin)
pose1[0] += center_point[1] * args.ratio # height
pose1[1] += center_point[0] * args.ratio # width
pose1[2] += args.hover_distance
rob.movel(pose1, acc=args.a, vel=args.v)
episode.append(pose1)
# rotate end effector angle
joint_angle = rob.getj()
delta = np.random.uniform(0, np.pi)
angle = init_angle + delta
joint_angle[-1] = angle
rob.movej(joint_angle, args.a, args.v)
pose2 = rob.getl()
episode.append(pose2)
# reach to target pose with end effector rotated
pose3 = rob.getl()
pose3[2] = pose3[2] - args.hover_distance + z
rob.movel(pose3, acc=args.a, vel=args.v)
episode.append(pose3)
# close gripper and obtain gripper status
print('Try grasp:')
gripper.gripper_close()
rob.movel(pose2, acc=args.a, vel=args.v)
status = gripper.get_object_detection_status()
position = gripper.get_gripper_pos()
if (status == 1 or status == 2) and position < 217:
print('Grasp success!')
rob.movel(pose3, acc=args.a, vel=args.v)
gripper.gripper_open()
rob.movel(pose2, acc=args.a, vel=args.v)
rob.movel(camera_location, acc=args.a, vel=args.v)
for j in range(1):
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.first(rs.stream.color)
depth_frame = aligned_frames.get_depth_frame()
color_img = np.asanyarray(color_frame.get_data())
depth_img = (np.asanyarray(depth_frame.get_data()) *
args.z_scale).astype(np.uint16)
warp_color_img = cv2.warpPerspective(
color_img, mtx, (args.width, args.height))
warp_depth_img = cv2.warpPerspective(
depth_img, mtx, (args.width, args.height))
cv2.imwrite(color_path + '/{:06d}.png'.format(k),
color_img)
cv2.imwrite(depth_path + '/{:06d}.png'.format(k),
depth_img)
cv2.imwrite(
height_map_color_path + '/{:06d}.png'.format(k),
warp_color_img)
cv2.imwrite(
height_map_depth_path + '/{:06d}.png'.format(k),
warp_depth_img)
_, rect, object_pts, _ = get_center_point(
warp_color_img, warp_depth_img, b_depth_height_map)
points = get_label(center_point, -delta, 256 - position)
label = draw_label(points, args.width, args.height)
cv2.imwrite(label_path + '/{:06d}.png'.format(k), label)
np.savetxt(label_path + '/{:06d}.good.txt'.format(k),
points)
np.savetxt(label_path + '/{:06d}.rectangle.txt'.format(k),
np.asanyarray(rect))
np.savetxt(
label_path + '/{:06d}.object_points.txt'.format(k),
np.asanyarray(object_pts))
f.write('{:06d}\n'.format(k))
k += 1
print('i=%s, j=%s' % (i, j) +
', all good grasp labels written.')
num_success = num_success + 1
print(
'Now, let\'s grasp it again same to last pose to move to another place.'
)
rob.movel(episode[0], acc=args.a, vel=args.v)
rob.movel(episode[1], acc=args.a, vel=args.v)
rob.movel(episode[2], acc=args.a, vel=args.v)
gripper.gripper_close()
pose4 = copy.deepcopy(origin)
# generate a new center_point randomly.
center_point = [
np.random.randint(57, args.width - 57),
np.random.randint(57, args.height - 57)
]
pose4[0] += center_point[1] * args.ratio
pose4[1] += center_point[0] * args.ratio
pose4[2] += args.hover_distance
rob.movel(pose4, acc=args.a, vel=args.v)
episode[0] = pose4
joint_angle = rob.getj()
delta = np.random.uniform(0, np.pi)
angle = init_angle + delta
joint_angle[-1] = angle
rob.movej(joint_angle, args.a, args.v)
pose5 = rob.getl()
episode[1] = pose5
pose6 = rob.getl()
pose6[2] = pose6[2] - args.hover_distance + z
rob.movel(pose6, acc=args.a, vel=args.v)
episode[2] = pose6
gripper.gripper_open()
print(
'OK,move to another place.Notice that we remember the route and angle unless j=1.'
)
rob.movel(episode[1], acc=args.a, vel=args.v)
rob.movel(camera_location, acc=args.a, vel=args.v)
elif position > 217:
print('Grasp fails!')
cv2.imwrite(color_path + '/{:06d}.png'.format(k), color_img)
cv2.imwrite(depth_path + '/{:06d}.png'.format(k), depth_img)
cv2.imwrite(height_map_color_path + '/{:06d}.png'.format(k),
warp_color_img)
cv2.imwrite(height_map_depth_path + '/{:06d}.png'.format(k),
warp_depth_img)
points = get_label(center_point, -delta, 256)
label = draw_label(points, args.width, args.height,
(0, 0, 255))
cv2.imwrite(label_path + '/{:06d}.png'.format(k), label)
np.savetxt(label_path + '/{:06d}.bad.txt'.format(k), points)
np.savetxt(label_path + '/{:06d}.rectangle.txt'.format(k),
np.asanyarray(rect))
np.savetxt(label_path + '/{:06d}.object_points.txt'.format(k),
np.asanyarray(object_pts))
f.write('{:06d}\n'.format(k))
k += 1
print('i=%s' % i + ', all bad grasp labels written.')
num_fail = num_fail + 1
gripper.gripper_open()
rob.movel(camera_location, acc=args.a, vel=args.v)
else:
gripper.gripper_open()
rob.movel(camera_location, acc=args.a, vel=args.v)
finally:
rob.close()
pipeline.stop()
f.close()
print("Success: " + str(num_success) + ", fail: " + str(num_fail))
import torch
import skvideo.io
import os
# import png
#!=====================================================================================================================
pipeline = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# 设定需要对齐的方式（这里是深度对齐彩色，彩色图不变，深度图变换）
align_to = rs.stream.color
# 设定需要对齐的方式（这里是彩色对齐深度，深度图不变，彩色图变换）
# align_to = rs.stream.depth
alignedFs = rs.align(align_to)
profile = pipeline.start(cfg)
size = (640, 480)
#save as avi
out = cv2.VideoWriter('color_video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30, size)
outdepth = cv2.VideoWriter('depth_video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30, size)
#or save as mp4
#out = cv2.VideoWriter('color_video.mp4',cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 30, size)
#outdepth = cv2.VideoWriter('depth_video.mp4',cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 30, size)
depth_numpy = []
color_numpy = []
all_depth_numpy = []
all_color_numpy = []
depth_fpath = "./numpy/depth.npy"
color_fpath = "./numpy/color.npy"
if os.path.exists(depth_fpath):
def main():
global handin
global currentState
# img_rows, img_cols, maxFrames = 32, 32, 100
img_rows, img_cols, maxFrames = 50, 50, 55
depthFrame = 0
cameraHeightR = 480
cameraWidthR = 848
# cameraWidthR = 640
frameRateR = 60
# frameRateR = 30
# crop parameter
xupam = 350
yupam = 200
xdpam = 250
ydpam = 300
#1,3,4,5,8,9,12,13,14,15
classGest = ['1', '12', '13', '14', '15', '3', '4', '5', '8', '9']
nameGest = [
'call', 'scroll up', 'scroll down', 'right', 'leftback', "like",
'play/pause', 'close', 'click', 'down back'
]
#classGest = ['11', '12', '13','4','8']
#nameGest = ['back', 'scroll up', 'scroll down', 'close 4', 'click 8']
delayGest = 5
delayBol = False
backgroundRemove = True
boxCounter = True
# load the model and weight
json_file = open('3dcnnresult/33/3dcnnmodel.json', 'r')
loaded_model_json = json_file.read()
json_file.close()
loaded_model = model_from_json(loaded_model_json)
# load weights into new model
loaded_model.load_weights("3dcnnresult/33/3dcnnmodel.hd5")
print("Loaded model from disk")
loaded_model.compile(loss=categorical_crossentropy,
optimizer='rmsprop',
metrics=['accuracy'])
conf = loaded_model.model.get_config()
shapeInput, ap = loaded_model.model.get_layer(name="dense_2").input_shape
shapeOutput, sp = loaded_model.model.get_layer(name="dense_2").output_shape
weightDense2 = loaded_model.model.get_layer(
name="dense_2").get_weights()[0]
weightDense22I = loaded_model.model.get_layer(
name="dense_2").get_weights()[1]
weightDense22A = loaded_model.model.get_layer(
name="dense_2").get_weights()[1]
# load the FSM
ytfsmfile = "youtube_fsm.txt"
ytFsmTable = loadFSM(ytfsmfile)
updatedWeight = False
updatedWeight2 = False
''''
print("========================New weight I ==================================")
print(weightDense22I)
print("========================New weight A ==================================")
print(weightDense22A)
'''
#print(newWeight[0])
#NweightDense2 = loaded_model.model.get_layer(name="dense_2").get_weights()[0]
#modelConfig = loaded_model.get_config()
#print(modelConfig)
# setup cv face detection
face_cascade = cv2.CascadeClassifier(
'haarcascades/haarcascade_frontalface_alt2.xml')
# setup Realsense
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR,
rs.format.z16, frameRateR)
config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR,
rs.format.bgr8, frameRateR)
# if using background removal
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# print "Depth Scale is: " , depth_scale
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 # 1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# for text purpose
imgTxt = np.zeros((480, 400, 3), np.uint8)
txt = "OpenCV"
txtLoad = "["
txtDelay = "["
txtRecord = "Capture"
txtDel = "Delay"
txtProbability = "0%"
font = cv2.FONT_HERSHEY_SIMPLEX
framearray = []
ctrDelay = 0
channel = 1
gestCatch = False
gestStart = False
startTime = 0
endTime = 0
x, y, w, h = 0, 0, 0, 0
align_to = rs.stream.color
align = rs.align(align_to)
num_frames = 0
im_width, im_height = (848, 480)
# max number of hands we want to detect/track
num_hands_detect = 1
score_thresh = 0.37
try:
while True:
dataImg = []
# Wait for a coherent pair of frames: depth and color
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()
color_frame = aligned_frames.get_color_frame()
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
#print(scores)
num_frames += 1
image_np = color_image
try:
image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
except:
print("Error converting to RGB")
boxes, scores = detector_utils.detect_objects(
image_np, detection_graph, sess)
for i in range(num_hands_detect):
if (scores[i] > score_thresh):
handin = True
#print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
break
if handin == True:
gestStart = True
else:
gestStart = False
if (backgroundRemove == True):
# Remove background - Set pixels further than clipping_distance to grey
# grey_color = 153
grey_color = 0
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)
color_image = bg_removed
draw_image = color_image
else:
draw_image = color_image
# face detection here
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
if gestCatch == False:
faces = face_cascade.detectMultiScale(gray, 1.1, 2)
#print("face: ",len(faces))
ctr = 0
idxFace = -1
minDist = 9999
if len(faces) > 0:
for f in faces:
xh, yh, wh, hh = f
farea = wh * hh
midxf = int(xh + (wh * 0.5))
midyf = int(yh + (hh * 0.5))
depth_imageS = depth_image * depth_scale
deptRef = depth_imageS.item(midyf, midxf)
if deptRef <= minDist:
idxFace = ctr
minDist = deptRef
ctr = ctr + 1
#print("id face", idxFace)
if idxFace >= 0:
x, y, w, h = faces[idxFace]
cv2.rectangle(draw_image, (x, y), (x + w, y + h),
(255, 0, 0), 2)
# crop the face then pass to resize
midx = int(x + (w * 0.5))
midy = int(y + (h * 0.5))
xUp = (x - (w * 3))
yUp = (y - (h * 1.5))
xDn = ((x + w) + (w * 1))
yDn = ((y + h) + (h * 2))
if xUp < 1: xUp = 0
if xDn >= cameraWidthR: xDn = cameraWidthR
if yUp < 1: yUp = 0
if yDn >= cameraHeightR: yDn = cameraHeightR
if handin == False:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
(xDn.__int__(), yDn.__int__()), (0, 0, 255), 2)
else:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
(xDn.__int__(), yDn.__int__()), (0, 255, 0), 2)
cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10,
(255, 0, 0))
# region of interest
roi_gray = gray[yUp.__int__():yDn.__int__(),
xUp.__int__():xDn.__int__()]
#print(cv2.useOptimized())
'''''
stateText = "State " + str(currentState)
cv2.putText(imgTxt, stateText, (10, 200), font, 1, (255, 255, 255), 2,
cv2.LINE_AA)
avtext = "Available Gesture"
cv2.putText(imgTxt, avtext, (10, 230), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
for i in range(0, len(availableGest)):
availGestText = "G" + availableGest[i] + ", "
cv2.putText(imgTxt, availGestText, (10 + (i * 80), 260), font, 1, (255, 255, 255), 2,
cv2.LINE_AA)
'''
# if handin == True and depthFrame==10:
# depth_imageS = depth_image * depth_scale
# deptRef = depth_imageS.item(midy, midx)
# boxColor = color_image.copy()
# checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image,handin)
# find the depth of middle point of face
if backgroundRemove == True and gestCatch == False:
#if backgroundRemove == True:
e1 = cv2.getTickCount()
depth_imageS = depth_image * depth_scale
deptRef = depth_imageS.item(midy, midx)
# print(clipping_distance)
clipping_distance = (deptRef + 0.2) / depth_scale
boxColor = color_image.copy()
#handin = checkhandIn(boxCounter, deptRef, xUp, yUp, xDn, yDn, depth_imageS, draw_image)
##handin = checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image)
#print(handin)
e2 = cv2.getTickCount()
times = (e2 - e1) / cv2.getTickFrequency()
#print(times)
#handin = False
##if handin == True:
## gestStart = True
##else:
## gestStart = False
# print("gest start = ", gestStart)
if delayBol == False and gestStart == True:
# your code execution
if depthFrame < maxFrames:
frame = cv2.resize(roi_gray, (img_rows, img_cols))
framearray.append(frame)
depthFrame = depthFrame + 1
txtLoad = txtLoad + "["
gestCatch = True
# print(depthFrame)
if depthFrame == maxFrames:
dataImg.append(framearray)
xx = np.array(dataImg).transpose((0, 2, 3, 1))
X = xx.reshape(
(xx.shape[0], img_rows, img_cols, maxFrames, channel))
X = X.astype('float32')
print('X_shape:{}'.format(X.shape))
#==================== Update the Weight =======================================
newWeightI = []
newWeightA = []
# find the available gesture in the current state
availableGest = getGesture(currentState, ytFsmTable)
print(availableGest)
availG, ignoreG = translateAvailableGest(
availableGest, classGest)
print(availG)
print(ignoreG)
if updatedWeight:
weightI = manipWeight(weightDense22I, ignoreG, 1000)
newWeightI.append(weightDense2)
newWeightI.append(weightI)
if updatedWeight2:
maxClass = 10
weightA = manipWeight(weightDense22A, availG, 1000)
newWeightA.append(weightDense2)
newWeightA.append(weightA)
#=================================================================================
if updatedWeight == False and updatedWeight2 == False:
newWeightI.append(weightDense2)
newWeightI.append(weightDense22A)
loaded_model.model.get_layer(
name="dense_2").set_weights(newWeightI)
if handin == True:
# do prediction
resc = loaded_model.predict_classes(X)[0]
res = loaded_model.predict_proba(X)[0]
# find the result of prediction gesture
resultC = classGest[resc]
nameResultG = nameGest[resc]
for a in range(0, len(res)):
print("Gesture {}: {} ".format(
str(nameGest[a]), str(res[a] * 100)))
else:
resultC = 0
nameResultG = "not enough frame recorded"
print(
"==============================================================="
)
if updatedWeight2:
loaded_model.model.get_layer(
name="dense_2").set_weights(newWeightA)
# do prediction
resc2 = loaded_model.predict_classes(X)[0]
res2 = loaded_model.predict_proba(X)[0]
# find the result of prediction gesture
resultC2 = classGest[resc2]
nameResultG2 = nameGest[resc2]
for a in range(0, len(res2)):
print("Gesture {}: {} ".format(
str(nameGest[a]), str(res2[a] * 100)))
# show text of gesture result
imgTxt = np.zeros((480, 400, 3), np.uint8)
#txt = "Gesture-" + str(resultFsm)
if updatedWeight2:
if res2[resc2] > res[resc]:
txt = "ignored gesture"
act = -1
else:
txt = nameResultG
act = resultC
else:
txt = nameResultG
act = resultC
print(act)
# check with FSM for finding the next state
currentState = getNextState(currentState, act, ytFsmTable)
print(currentState)
framearray = []
#dataImg = []
txtLoad = ""
depthFrame = 0
gestCatch = False
handin = False
delayBol = True
cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txtRecord, (10, 50), font, 1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txt, (10, 160), font, 2, (255, 255, 255),
2, cv2.LINE_AA)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
# depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# print(delayBol)
if delayBol == True:
ctrDelay = ctrDelay + 1
txtDelay = txtDelay + "["
txtDel = "Delay"
cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txtDel, (10, 100), font, 1,
(255, 255, 255), 2, cv2.LINE_AA)
if ctrDelay == delayGest:
ctrDelay = 0
txtDelay = ""
delayBol = False
gestCatch = False
handin = False
gestStart = False
draw_image = cv2.flip(draw_image, 1)
# Stack both images horizontally
images = np.hstack((draw_image, imgTxt))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
finally:
# Stop streaming
pipeline.stop()
def run_loop(bag_path, seg_model, seg_opts, save_images=False, output_mode=0):
if save_images:
create_folders()
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
config = rs.config()
# Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
rs.config.enable_device_from_file(config, args.input)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale) # can be commented out
if output_mode == 0 or output_mode == 1:
# Create opencv window to render image in
cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
# pre_seg_mask_sum = None # previous frame path segmentation area - isn't being used right now
# Streaming loop
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get intrinsic in Open3d format for mode 2 and 3 for point cloud output
if output_mode == 1 or output_mode == 2:
intrinsic = o3d.camera.PinholeCameraIntrinsic(
get_intrinsic_matrix(color_frame))
# Get depth frame
depth_frame = frames.get_depth_frame()
# Print intrinsics and extrinsics - not necessary : can be commented out
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
color_image = np.asanyarray(color_frame.get_data())
### Add SEGMENTATION part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track - chosen classes for the model selected (we'd like an oversegmentation of the path)
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
if idx == idx_limit: # 1st frame detection needs to be robust
pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
new_seg_sum = np.sum(seg_mask)
diff = abs(new_seg_sum - pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
pred_color = colorEncode(
pred,
loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=5)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
## removed this part
planes_mask = np.ones_like(depth_array).astype(np.uint8)
planes_mask = np.dstack(
(planes_mask, planes_mask, planes_mask))
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
edges = planes_mask_binary - nd.morphology.binary_dilation(
planes_mask_binary
) # edges calculation - between travesable and non-traversable path
#############################################
if output_mode == 1 or output_mode == 2:
odepth_image = o3d.geometry.Image(depth_array * edges)
ocolor_image = o3d.geometry.Image(color_image * np.dstack(
(edges, edges, edges)))
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
ocolor_image,
odepth_image,
depth_scale=1.0 / depth_scale,
depth_trunc=10, # set to 10 metres
convert_rgb_to_intensity=False)
temp = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, intrinsic)
temp.transform(flip_transform)
# temp = temp.voxel_down_sample(0.03)
# Point cloud output of frame is appended to the list
if output_mode == 1 or output_mode == 2:
output_list.append(temp)
# image format conversion for cv2 visualization/output
if output_mode == 0 or output_mode == 1 or save_images == True:
pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
## for displaying seg_mask
seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
seg_mask = cv2.cvtColor(
seg_mask, cv2.COLOR_GRAY2BGR) # segmentation binary mask
final_output_mask = cv2.cvtColor(
planes_mask_binary,
cv2.COLOR_GRAY2BGR) # final traversable path mask
if output_mode == 0 or output_mode == 1:
# # Blending rgb and depth images for display - can check alignment with this as well
alpha = 0.2
beta = (1.0 - alpha)
dst = cv2.addWeighted(
color_image, alpha, pred_color, beta, 0.0
) # color and segmentation output from ADE20K model blended
dst2 = cv2.addWeighted(depth_color_image, alpha, color_image,
beta,
0.0) # color and depth blended together
##################################
### delete later if needed - color image masked by final traversable path
final_output = color_image * planes_mask_binary_3d
mask = (final_output[:, :, 0] == 0) & (
final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
final_output[:, :, :3][mask] = [255, 255, 255]
######
### Select outputs for visualization - we've chosen some as default
image_set1 = np.vstack((dst, dst2))
# image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
# image_set2 = np.vstack((dst, final_output))
image_set2 = np.vstack((edges, final_output))
### Choose which images you want to display from above
combined_images = np.concatenate((image_set1, image_set2),
axis=1)
if save_images == True:
# Outputs saved - you can modify this
cv2.imwrite("output/visualization/frame%d.png" % idx,
combined_images)
cv2.imwrite("data/color/frame%d.png" % idx,
color_image) # save frame as JPEG file
cv2.imwrite("data/depth/frame%d.png" % idx,
depth_array) # save frame as JPEG file
cv2.imwrite("output/edges/frame%d.png" % idx,
edges) # save frame as JPEG file
cv2.imwrite("output/segmentation_mask/frame%d.png" % idx,
seg_mask) # save frame as JPEG file
cv2.imwrite("output/output_mask/frame%d.png" % idx,
final_output_mask) # save frame as JPEG file
if output_mode == 0 or output_mode == 1:
try:
cv2.imshow('Full Stream', combined_images)
except TypeError as e:
print(idx, e)
key = cv2.waitKey(1)
# if pressed escape exit program
if key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
if output_mode == 0 or output_mode == 1:
cv2.destroyAllWindows()
return
def gesture():
#return('gesture')
#global testDir
#global img_rows, img_cols, maxFrames
#global depthFrame
#crop parameter
#global xupam
#global yupam
#global xdpam
#global ydpam
#global classGest
#global delayGest
#global delayBol
#load the model and weight
#global json_file
#global loaded_model_json
#global loaded_model
#setup cv face detection
#global face_cascade
# setup Realsense
# Configure depth and color streams
# for text purpose
global handin
global facex
global facey
global txt
global txtLoad
global txtDelay
global txtRecord
global txtDel
global txtProbability
global font
#global framearray
#global ctrDelay
#global channel
#global gestCatch
#global gestStart
#global x,y,w,h
#global vc
#global rval , firstFrame
#global heightc, widthc, depthcol
#global imgTxt
#global resultC
#global count
#global stat
pipeline = rs.pipeline()
K.clear_session()
testDir = "videoTest/"
img_rows, img_cols, maxFrames = 32, 32, 55
depthFrame = 0
#crop parameter
xupam = 350
yupam = 200
xdpam = 250
ydpam = 300
depthFrame = 0
cameraHeightR = 480
#cameraWidthR = 848
cameraWidthR = 848
frameRateR = 60
classGest = ['1','11','12','13','4','5','7','8']
delayGest = 20
delayBol = False
framearray = []
ctrDelay = 0
channel = 1
gestCatch = False
gestStart = False
backgroundRemove = True
x,y,w,h = 0,0,0,0
count=0
boxCounter = True
#load the model and weight
json_file = open('3dcnnresult/24/3dcnnmodel.json', 'r')
loaded_model_json = json_file.read()
json_file.close()
# load weights into new model
loaded_model = model_from_json(loaded_model_json)
#loaded_model.load_weights("3dcnnresult/3dcnnmodel.hd5")
loaded_model.load_weights("3dcnnresult/24/3dcnnmodel.hd5")
#setup cv face detection
face_cascade = cv2.CascadeClassifier('haarcascades/haarcascade_frontalface_alt2.xml')
config = rs.config()
config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR, rs.format.z16, frameRateR)
config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR, rs.format.bgr8, frameRateR)
'''vc = cv2.VideoCapture(0)
rval , firstFrame = vc.read()
heightc, widthc, depthcol = firstFrame.shape
imgTxt = np.zeros((heightc, 400, 3), np.uint8)
#print('tryyyyy1')'''
stat =0
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# print "Depth Scale is: " , depth_scale
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 2 # 1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
#print("tes====================")
# for text purpose
imgTxt = np.zeros((480, 400, 3), np.uint8)
txt = "OpenCV"
txtLoad = "["
txtDelay = "["
txtRecord = "Capture"
txtDel = "Delay"
txtProbability = "0%"
font = cv2.FONT_HERSHEY_SIMPLEX
align_to = rs.stream.color
align = rs.align(align_to)
#print("Loaded model from disk")
count=0
loaded_model.compile(loss=categorical_crossentropy,
optimizer='rmsprop', metrics=['accuracy'])
while True:
if True:
dataImg = []
# Wait for a coherent pair of frames: depth and color
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()
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())
if (backgroundRemove == True):
# Remove background - Set pixels further than clipping_distance to grey
# grey_color = 153
grey_color = 0
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)
color_image = bg_removed
draw_image = color_image
else:
draw_image = color_image
#face detection here
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
#cv2.imshow('gray', gray)
#cv2.waitKey(5000)
faces = face_cascade.detectMultiScale(gray, 1.1, 2)
if gestCatch == False:
faces = face_cascade.detectMultiScale(gray, 1.1, 2)
if len(faces) > 0:
'''x, y, w, h = faces[0] '''
for f in faces:
xh, yh, wh, hh = f
farea = wh * hh
if farea > 5000 and farea < 18000:
x, y, w, h = f
fArea = w*h
'''
midx = int(x + (w * 0.5))
midy = int(y + (h * 0.5))
xUp = (x - (w * 3))
yUp = (y - (h * 1.5))
xDn = ((x + w) + (w * 1))
yDn = ((y + h) + (h * 2))
if xUp < 1: xUp = 0
if xDn >= cameraWidthR: xDn = cameraWidthR
if yUp < 1: yUp = 0
if yDn >= cameraHeightR: yDn = cameraHeightR
if handin == False:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 0, 255),
2)
else:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 255, 0),
2)
cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10, (255, 0, 0))
# region of interest
roi_gray = gray[yUp.__int__():yDn.__int__(), xUp.__int__():xDn.__int__()]
'''
midx = int(x + (w * 0.5))
midy = int(y + (h * 0.5))
xUp = (x - (w * 3))
yUp = (y - (h * 1.5))
xDn = ((x + w) + (w * 1))
yDn = ((y + h) + (h * 2))
if xUp < 1: xUp = 0
if xDn >= cameraWidthR: xDn = cameraWidthR
if yUp < 1: yUp = 0
if yDn >= cameraHeightR: yDn = cameraHeightR
if handin == False:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 0, 255), 2)
else:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()), (xDn.__int__(), yDn.__int__()), (0, 255, 0),
2)
cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10, (255, 0, 0))
roi_color = color_image[yUp.__int__():yDn.__int__(), xUp.__int__():xDn.__int__()]
#find the depth of middle point of face
if backgroundRemove == True and gestCatch == False:
#e1 = cv2.getTickCount()
depth_imageS = depth_image * depth_scale
deptRef = depth_imageS.item(midy, midx)
# print(clipping_distance)
clipping_distance = (deptRef + 0.2) / depth_scale
boxColor = color_image.copy()
handin = checkhandIn(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image)
e2 = cv2.getTickCount()
#times = (e2 - e1) / cv2.getTickFrequency()
#print(times)
if handin == True:
gestStart = True
else:
gestStart = False
if delayBol == False and gestStart == True:
if depthFrame < maxFrames:
frame = cv2.resize(roi_color, (img_rows, img_cols))
framearray.append(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY))
depthFrame = depthFrame+1
txtLoad = txtLoad+"["
count=count+1
gestCatch = True
#print(depthFrame)
if depthFrame == maxFrames:
dataImg.append(framearray)
xx = np.array(dataImg).transpose((0, 2, 3, 1))
X = xx.reshape((xx.shape[0], img_rows, img_cols, maxFrames, channel))
X = X.astype('float32')
#print('X_shape:{}'.format(X.shape))
#do prediction
resc = loaded_model.predict_classes(X)[0]
res = loaded_model.predict_proba(X)[0]
resultC = classGest[resc]
#print("X=%s, Probability=%s" % (resultC, res[resc]*100))
for r in range(0,8):
print("prob: " + str(res[r]*100))
#show text
#imgTxt = np.zeros((480, 400, 3), np.uint8)
txt = "Gesture-" + str(resultC)
txtProbability = str(res[resc]*100)+"%"
framearray = []
#dataImg = []
txtLoad = ""
depthFrame = 0
handin = False
gestCatch = False
delayBol = True
#cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA)
#.putText(imgTxt, txtRecord, (10, 50), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
#cv2.putText(imgTxt, txt, (10, 200), font, 2, (255, 255, 255), 2, cv2.LINE_AA)
#cv2.putText(imgTxt, txtProbability, (10, 250), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
# depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
#print(delayBol)
if delayBol == True:
ctrDelay = ctrDelay+1
txtDelay = txtDelay + "["
#txtDel = "Delay"
#cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1, (255, 255, 255), 2, cv2.LINE_AA)
#cv2.putText(imgTxt, txtDel, (10, 100), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
if ctrDelay == delayGest:
ctrDelay = 0
txtDelay = ""
delayBol = False
# Stack both images horizontally
#images = np.hstack((draw_image, imgTxt))
# put the text here
# Show images
#cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
#cv2.imshow('RealSense', images)
#cv2.waitKey(1)
if count==maxFrames:
#vc.release()
K.clear_session()
pipeline.stop()
return resultC
'''
def run_loop(bag_path, seg_model, seg_opts, save_images=False):
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
config = rs.config()
# Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
rs.config.enable_device_from_file(config, args.input)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
# Create opencv window to render image in
cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
pre_seg_mask_sum = None # previous frame path segmentation area
# Streaming loop
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get depth frame
depth_frame = frames.get_depth_frame()
# Get intrinsics and extrinsics
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
color_image = np.asanyarray(color_frame.get_data())
### Add Segmentation part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
if idx == idx_limit: # 1st frame detection needs to be robust
pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
new_seg_sum = np.sum(seg_mask)
diff = abs(new_seg_sum - pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
pred_color = colorEncode(
pred,
loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=5)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
## removed this part
planes_mask = np.ones_like(depth_array).astype(np.uint8)
planes_mask = np.dstack(
(planes_mask, planes_mask, planes_mask))
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
edges = planes_mask_binary - nd.morphology.binary_dilation(
planes_mask_binary) # edges calculation
edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
#############################################
# for cv2 output
pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
# if save_images == True:
# cv2.imwrite("data_/color/frame%d.png" % idx, color_image) # save frame as JPEG file
# cv2.imwrite("data_/depth/frame%d.png" % idx, depth_array) # save frame as JPEG file
# cv2.imwrite("data_/color_depth/frame%d.png" % idx, depth_color_image) # save frame as JPEG file
# cv2.imwrite("data_/thresholded_color/frame%d.png" % idx, thresholded_color_image) # save frame as JPEG file
# # cv2.imwrite("data_/thresholded_depth/frame%d.png" % idx, thresholded_depth_image) # save frame as JPEG file
# # Blending images
alpha = 0.2
beta = (1.0 - alpha)
dst = cv2.addWeighted(color_image, alpha, pred_color, beta, 0.0)
# kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(11,11))
# res = cv2.morphologyEx(planes_mask,cv2.MORPH_OPEN,kernel)
dst2 = cv2.addWeighted(depth_color_image, alpha, color_image, beta,
0.0)
## for displaying seg_mask
seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
seg_mask = cv2.cvtColor(seg_mask, cv2.COLOR_GRAY2BGR)
##################################
### delete later
final_output = color_image * planes_mask_binary_3d
mask = (final_output[:, :, 0] == 0) & (
final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
final_output[:, :, :3][mask] = [255, 255, 255]
######
# if np.sum(planes_mask) == depth_array.shape[0]*depth_array.shape[1]:
# image_set1 = np.vstack((dst, color_image))
# else:
image_set1 = np.vstack((color_image, depth_color_image))
# image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
image_set2 = np.vstack((dst, final_output))
# image_set2 = np.vstack((edges, final_output))
combined_images = np.concatenate((image_set1, image_set2), axis=1)
if save_images == True:
cv2.imwrite("./meeting_example/frame%d.png" % idx,
combined_images)
try:
cv2.imshow('Full Stream', combined_images)
except TypeError as e:
print(idx, e)
key = cv2.waitKey(1)
# if pressed escape exit program
if key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
cv2.destroyAllWindows()
# if save_images == True:
# pkl.dump( threshold_mask, open( "data_/depth_threshold.pkl", "wb" ) )
# print("Mask pickle saved")
return
def extract_frames(pipe,
cfg,
save_path,
post_processing=False,
save_colorize=True,
save_pc=False,
visualize=True):
"""Helper function to align and extract rgb-d image from bagfile.
Check more saving options in arguments.
Args:
pipe: pyrealsense2 pipeline.
cfg: pyrealsense2 pipeline configuration.
save_path: Path to save the extracted frames.
save_colorize: Save colorized depth maps visualization.
save_pc: Save point cloud data.
visualize: Visualize the video frames during saving.
"""
# Configurations
if save_colorize:
colorizer = rs.colorizer()
if save_pc:
pc = rs.pointcloud()
points = rs.points()
# Save path
if not os.path.exists(save_path):
os.makedirs(save_path)
# Start the pipe
i = 0
profile = pipe.start(cfg)
device = profile.get_device()
playback = device.as_playback()
playback.set_real_time(
False) # Make sure this is False or frames get dropped
while True:
try:
# Wait for a conherent pairs of frames: (rgb, depth)
pairs = pipe.wait_for_frames()
# Align depth image to rgb image first
align = rs.align(rs.stream.color)
pairs = align.process(pairs)
color_frame = pairs.get_color_frame()
depth_frame = pairs.get_depth_frame()
if not depth_frame or not color_frame:
continue
# Post-processing
if post_processing:
depth_frame = post_processing(depth_frame)
# Get rgb-d images
color_img = np.asanyarray(color_frame.get_data())
color_img = cv2.cvtColor(color_img, cv2.COLOR_RGB2BGR)
depth_img = np.asanyarray(depth_frame.get_data())
print('Frame {}, Depth Image {}, Color Image {}'.format(
i + 1, depth_img.shape, color_img.shape))
# Save as loseless formats
cv2.imwrite(os.path.join(save_path, '{}_rgb.png'.format(i)),
color_img, [cv2.IMWRITE_PNG_COMPRESSION, 0])
np.save(os.path.join(save_path, '{}_depth.npy'.format(i)),
depth_img)
if save_colorize:
# Save colorized depth map
depth_img_colorized = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
cv2.imwrite(
os.path.join(save_path,
'{}_depth_colorized.jpg'.format(i)),
depth_img_colorized) # No need for lossless here
if save_pc:
# NOTE: Point cloud calculation takes longer time.
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
points.export_to_ply(
os.path.join(save_path, '{}_pc.ply'.format(i)),
color_frame)
i += 1
if visualize:
# Stack both images horizontally
images = np.vstack((color_img, depth_img_colorized))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_NORMAL)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
except Exception as e:
print(e)
break
# Clean pipeline
pipe.stop()
print('{} frames saved in total.'.format(i))
return
def aligned(self, frames):
self.align = rs.align(rs.stream.color)
frames = self.align.process(frames)
aligned_depth_frame = frames.get_depth_frame()
return aligned_depth_frame
import numpy as np
import cv2
W = 848
H = 480
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30)
config.enable_stream(rs.stream.color, W, H, rs.format.bgr8, 30)
print("[INFO] start streaming...")
pipeline.start(config)
aligned_stream = rs.align(rs.stream.color) # alignment between color and depth
point_cloud = rs.pointcloud()
print("[INFO] loading model...")
# download model from: https://github.com/opencv/opencv/wiki/TensorFlow-Object-Detection-API#run-network-in-opencv
net = cv2.dnn.readNetFromTensorflow(
"frozen_inference_graph.pb",
"faster_rcnn_inception_v2_coco_2018_01_28.pbtxt")
while True:
frames = pipeline.wait_for_frames()
frames = aligned_stream.process(frames)
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame().as_depth_frame()
points = point_cloud.calculate(depth_frame)
verts = np.asanyarray(points.get_vertices()).view(np.float32).reshape(
def start(self):
self.pipeline.start(self.config)
self.alignment = rs.align(rs.stream.color)
def __init__(self, scanDuration, rootDir, sharpening, configFile, fps, width, height, visualPreset, laserPower, exposure, gain) :
self.scanDuration = scanDuration
self.fps = fps
self.rootDir = rootDir
self.sharpening = sharpening
self.width = width
self.height = height
self.configFile = configFile
self.captureName = None
self.closed = False
if self.configFile :
try :
with open(self.configFile) as file:
configString = json.load(file)
configDict = configString
configString = json.dumps(configString)
self.fps = int(configDict["stream-fps"])
self.width = int(configDict["stream-width"])
self.height = int(configDict["stream-height"])
Logger.printSuccess("Parameters successfully loaded !")
except :
Logger.printError("Could not read the RealSense configuration file")
self.window = cv2.namedWindow("Capture", cv2.WINDOW_AUTOSIZE)
Logger.printInfo("Initializing scanner ...")
try :
#Initialization
self.pipeline = rs2.pipeline()
self.config = rs2.config()
#Enable streams
self.config.enable_stream(rs2.stream.depth, self.width, self.height, rs2.format.z16, self.fps)
self.config.enable_stream(rs2.stream.color, self.width, self.height, rs2.format.bgr8, self.fps)
#Start streaming
self.profile = self.pipeline.start(self.config)
except Exception as e :
Logger.printError("Unable to start the stream, gonna quit.\nException -> " + str(e))
exit()
self.device = self.profile.get_device()
self.depthSensor = self.device.first_depth_sensor()
try :
#Set parameters
if configFile :
advancedMode = rs2.rs400_advanced_mode(self.device)
print(configString)
advancedMode.load_json(json_content=configString)
else :
"""
0 -> Custom
1 -> Default
2 -> Hand
3 -> High Accuracy
4 -> High Density
5 -> Medium Density
"""
self.depthSensor.set_option(rs2.option.visual_preset, visualPreset)
self.depthSensor.set_option(rs2.option.laser_power, laserPower)
self.depthSensor.set_option(rs2.option.gain, gain)
self.depthSensor.set_option(rs2.option.exposure, exposure)
#self.depthSensor.set_option(rs2.option.max_distance, 4.0)
except Exception as e :
Logger.printError("Unable to set one parameter on the RealSense, gonna continue to run.\nException -> " + str(e))
pass
self.intrinsics = self.profile.get_stream(rs2.stream.depth).as_video_stream_profile().get_intrinsics()
#Create an align object -> aligning depth frames to color frames
self.aligner = rs2.align(rs2.stream.color)
Logger.printSuccess("Scanner successfully initialized !")
self.depthDataset = []
import pyrealsense2 as rs
WIDTH = int(1280)
HEIGHT = int(720)
# Configure depth and color streams
pipeline = rs.pipeline()
realsense_config = rs.config()
realsense_config.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, 30)
realsense_config.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.bgr8, 30)
# Record stream (color and depth) to file
realsense_config.enable_record_to_file('lego_detection_test.bag')
# Create alignment primitive with color as its target stream:
alignment_stream = rs.align(rs.stream.color)
# Start streaming
profile = pipeline.start(realsense_config)
# Getting the depth sensor's depth scale
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# Display barcode and QR code location
def display(frame, decoded_objects):
# Loop over all decoded objects
for decoded_object in decoded_objects:
points = decoded_object.polygon
def main():
#initialize node
rospy.init_node('image_save')
rospy.Subscriber("chatter", Point, callback_pos)
rospy.Subscriber("chatter_ori", Point, callback_ori)
rospy.Subscriber("/camera/color/image_raw", Image, realsense_callback)
rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image,
realsense_callback_depth)
# 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()
# Declare RealSense pipeline, encapsulating the actual device and sensors
pipe = 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)
# 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
align = rs.align(align_to)
#Start streaming with default recommended configuration
pipe.start(config)
try:
# Wait for the next set of frames from the camera
frames = pipe.wait_for_frames()
# unaligned
#depth_frame = frames.get_depth_frame() is a 640x360 depth image
#color_frame = frames.get_color_frame()
# 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()
if not aligned_depth_frame or not color_frame:
pass
# Convert images to numpy arrays
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
#depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
points = pc.calculate(aligned_depth_frame)
images = np.hstack((color_image, depth_image))
cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Align Example', images)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
#name = "1"
#print("Saving to 1.ply...")
#points.export_to_ply(name + ".ply", color_frame)
#cv2.imwrite(name + ".png",depth_image)
#cv2.imwrite(name + ".jpg",color_image)
#print("Done")
finally:
pipe.stop()
processFlag = False
rospy.spin()
def __init__(self, lower=(96, 59, 53,), upper=(160, 255, 130), callback=None) -> None:
self.lower, self.upper = lower, upper
# os.system('rosnode kill image_server')
self.pipeline = rs.pipeline()
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)
# set dep-th units, this should give us better resolution sub 5m?
device: rs.device = rs.context().query_devices()[0]
advnc_mode = rs.rs400_advanced_mode(device)
depth_table_control_group: STDepthTableControl = advnc_mode.get_depth_table()
depth_table_control_group.depthUnits = 500
advnc_mode.set_depth_table(depth_table_control_group)
# Start streaming
profile = self.pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor: DepthSensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
align_to = rs.stream.color
self.align = rs.align(align_to)
for i in range(10):
try:
sensors = sum([dev.query_sensors() for dev in rs.context().query_devices()], [])
color_sensor = next(
sensor for sensor in sensors if sensor.get_info(rs.camera_info.name) == "RGB Camera")
break
except Exception as e:
print(e)
sleep(0.3)
print("Setting RGB camera sensor settings")
# exposure: 166.0
# white balance: 4600.0
# gain: 64.0
# auto exposure enabled: 1.0
# exposure: 166.0
# white balance: 4600.0
# gain: 64.0
color_sensor.set_option(rs.option.saturation, 60)
color_sensor.set_option(rs.option.exposure, 166)
color_sensor.set_option(rs.option.enable_auto_exposure, 0)
color_sensor.set_option(rs.option.white_balance, 4600)
color_sensor.set_option(rs.option.enable_auto_white_balance, 0)
color_sensor.set_option(rs.option.gain, 64)
align = rs.align(align_to)
# color_sensor.set_option(rs.option.enable_auto_exposure, 0)
print("auto exposure enabled: {}".format(color_sensor.get_option(rs.option.enable_auto_exposure)))
print("exposure: {}".format(color_sensor.get_option(rs.option.exposure))) # 166
print("white balance: {}".format(color_sensor.get_option(rs.option.white_balance)))
print("gain: {}".format(color_sensor.get_option(rs.option.gain))) # 64
self.average = StreamingMovingAverage(20)
self.average_raw = StreamingMovingAverage(20)
self.average_fps = StreamingMovingAverage(20)
self.average_area = StreamingMovingAverage(20)
self.color = None
self.distance = None
self.fps = 0
self.area = 0
sleep(2)
def main():
# if not os.path.exists(args.directory):
# os.mkdir(args.directory)
try:
for s in range(15,16):
read_path ="/home/user/python_projects/utils-GJ/realsense_bag/kist_scene/"+str(s)+".bag"
save_path = "/home/user/python_projects/DenseFusion_yolact_base/living_lab_video/"+str(s)+"/"
begin = time.time()
index = 0
config = rs.config()
config.enable_stream(rs.stream.color)
config.enable_stream(rs.stream.depth)
pipeline = rs.pipeline()
rs.config.enable_device_from_file(config, read_path)
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("scale : " + str(depth_scale))
profile = pipeline.get_active_profile()
color_stream = profile.get_stream(rs.stream.color)
color_profile = rs.video_stream_profile(color_stream)
color_intrinsics = color_profile.get_intrinsics()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
print("*** color intrinsics ***")
print(color_intrinsics)
print("*** depth intrinsics ***")
print(depth_intrinsics)
while True:
if time.time() - begin > 22:
break
time.sleep(0.02)
frames = pipeline.wait_for_frames()
# align the deph to color frame
aligned_frames = align.process(frames)
# get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame()
aligned_depth_image = np.asanyarray(aligned_depth_frame.get_data())
# scaled_depth_image = depth_image * depth_scale
# convert color image to BGR for OpenCV
color_frame = frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
# r, g, b = cv2.split(color_image)
# color_image = cv2.merge((b, g, r))
# cv2.imshow("color", color_image)
# cv2.imshow("aligned_depth_image", aligned_depth_image)
# cv2.waitKey(0)
print("index : " + str(index))
if not os.path.exists(save_path):
os.mkdir(save_path)
cv2.imwrite(save_path+"depth_image" + str(index) + ".png", aligned_depth_image)
cv2.imwrite(save_path+"color_image" + str(index) + ".png", color_image)
# cv2.imwrite("/home/user/kist_scene/scene" + str(i) + "/depth_image" + str(index) + ".png", aligned_depth_image)
# cv2.imwrite("/home/user/kist_scene/scene" + str(i) + "/color_image" + str(index) + ".png", color_image)
index += 1
finally:
pass
def camThread(LABELS, results, frameBuffer, camera_mode, camera_width, camera_height, background_transparent_mode, background_img):
global fps
global detectfps
global lastresults
global framecount
global detectframecount
global time1
global time2
global cam
global window_name
global depth_scale
global align_to
global align
# Configure depth and color streams
# Or
# Open USB Camera streams
if camera_mode == 0:
pipeline = rs.pipeline()
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)
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
align_to = rs.stream.color
align = rs.align(align_to)
window_name = "RealSense"
elif camera_mode == 1:
cam = cv2.VideoCapture(0)
if cam.isOpened() != True:
print("USB Camera Open Error!!!")
sys.exit(0)
cam.set(cv2.CAP_PROP_FPS, 30)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, camera_width)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_height)
window_name = "USB Camera"
cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
while True:
t1 = time.perf_counter()
# 0:= RealSense Mode
# 1:= USB Camera Mode
if camera_mode == 0:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
if frameBuffer.full():
frameBuffer.get()
color_image = np.asanyarray(color_frame.get_data())
elif camera_mode == 1:
# USB Camera Stream Read
s, color_image = cam.read()
if not s:
continue
if frameBuffer.full():
frameBuffer.get()
frames = color_image
height = color_image.shape[0]
width = color_image.shape[1]
frameBuffer.put(color_image.copy())
res = None
if not results.empty():
res = results.get(False)
detectframecount += 1
imdraw = overlay_on_image(frames, res, LABELS, camera_mode, background_transparent_mode,
background_img, depth_scale=depth_scale, align=align)
lastresults = res
else:
imdraw = overlay_on_image(frames, lastresults, LABELS, camera_mode, background_transparent_mode,
background_img, depth_scale=depth_scale, align=align)
cv2.imshow(window_name, cv2.resize(imdraw, (width, height)))
if cv2.waitKey(1)&0xFF == ord('q'):
# Stop streaming
if pipeline != None:
pipeline.stop()
sys.exit(0)
## Print FPS
framecount += 1
if framecount >= 15:
fps = "(Playback) {:.1f} FPS".format(time1/15)
detectfps = "(Detection) {:.1f} FPS".format(detectframecount/time2)
framecount = 0
detectframecount = 0
time1 = 0
time2 = 0
t2 = time.perf_counter()
elapsedTime = t2-t1
time1 += 1/elapsedTime
time2 += elapsedTime
print("Depth2 Scale is: ", depth_scale3)
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 #1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
clipping_distance2 = clipping_distance_in_meters / depth_scale2
clipping_distance3 = clipping_distance_in_meters / depth_scale3
# 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
align = rs.align(align_to)
align2 = rs.align(align_to)
align3 = rs.align(align_to)
imgforder_name = 'imgs_front/'
depforder_name = 'depths_front/'
imgfile_name = 'image_front'
depfile_name = 'depth_front'
imgforder_name2 = 'imgs_left/'
depforder_name2 = 'depths_left/'
imgfile_name2 = 'image_left'
depfile_name2 = 'depth_left'
imgforder_name3 = 'imgs_right/'
depforder_name3 = 'depths_right/'
frameset = pipe.wait_for_frames()
color_frame = frameset.get_color_frame()
depth_frame = frameset.get_depth_frame()
# Cleanup:
pipe.stop()
print("Frames Captured")
color = np.asanyarray(color_frame.get_data())
plt.rcParams["axes.grid"] = False
plt.rcParams['figure.figsize'] = [12, 6]
colorizer = rs.colorizer()
# Create alignment primitive with color as its target stream:
align = rs.align(rs.stream.color)
frameset = align.process(frameset)
# Update color and depth frames:
aligned_depth_frame = frameset.get_depth_frame()
colorized_depth = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
# Show the two frames together:
images = np.hstack((color, colorized_depth))
plt.imshow(images)
depth = np.asanyarray(aligned_depth_frame.get_data())
# Get data scale from the device and convert to meters
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
def detect_video_realtime_mp():
# Configure depth and color streams
p1 = Process(target=predict_bbox_mp,
args=(original_frames, predicted_data))
p2 = Process(target=postprocess_mp,
args=(boundingBoxes, original_frames, processed_frames))
p3 = Process(target=Show_Image_mp,
args=(processed_frames, original_frames))
p4 = Process(target=socketVideoStream,
args=("10.0.0.36", 8080, processed_frames))
p1.start()
p2.start()
p3.start()
p4.start()
while True:
try:
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
if not depth_frame or not color_frame:
continue
violate = set()
color_image = color_frame.get_data()
color_image = np.asanyarray(color_image)
# align images
align = rs.align(rs.stream.color)
frameset = align.process(frames)
# Update color and depth frames:
aligned_depth_frame = frameset.get_depth_frame()
depth = np.asanyarray(aligned_depth_frame.get_data())
depthFrames.put(depth)
original_frames.put(color_image)
if not predicted_data.empty():
pred_bbox = predicted_data.get()
numberOfPeople = len(pred_bbox)
bboxes = []
vectors = []
if numberOfPeople >= 1:
for bbox in pred_bbox:
(sx, sy, ex, ey) = bbox
bboxes.append(bbox)
w = sx + (ex - sx) / 2
h = sy + (ey - sy) / 2
vectors.append(get3d(int(w), int(h), frames))
if (len(bboxes) > 0):
boundingBoxes.put((bboxes, vectors))
except Exception as e:
print('Error is ', str(e))
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print "Depth Scale is: " , depth_scale
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 #1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# 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
align = rs.align(align_to)
# Streaming loop
try:
while True:
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# frames.get_depth_frame() is a 640x360 depth image
# 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()
config_2.enable_device(sn_list[1])
config_2.enable_stream(rs.stream.depth, WIDTH, HEIGHT, rs.format.z16, 30)
config_2.enable_stream(rs.stream.color, WIDTH, HEIGHT, rs.format.bgr8, 30)
# Start streaming from both cameras
cfg1 = pipeline_1.start(config_1)
cfg2 = pipeline_2.start(config_2)
profile = cfg1.get_stream(rs.stream.depth)
intr1 = profile.as_video_stream_profile().get_intrinsics()
print('intr1:', intr1)
profile = cfg2.get_stream(rs.stream.depth)
intr2 = profile.as_video_stream_profile().get_intrinsics()
print('intr2:', intr2)
align1 = rs.align(rs.stream.color)
align2 = rs.align(rs.stream.color)
if not os.path.exists('tmp'):
os.makedirs('tmp')
cam_dir_1 = 'tmp/depth_cam1'
cam_dir_2 = 'tmp/depth_cam2'
if not os.path.exists(cam_dir_1):
os.makedirs(os.path.join(cam_dir_1, 'depth'))
os.makedirs(os.path.join(cam_dir_1, 'color'))
if not os.path.exists(cam_dir_2):
os.makedirs(os.path.join(cam_dir_2, 'depth'))
os.makedirs(os.path.join(cam_dir_2, 'color'))
with open(os.path.join(cam_dir_1, 'sn.txt'), 'w') as fout:
fout.write(sn_list[0] + '\n')
with open(os.path.join(cam_dir_1, 'intr.txt'), 'w') as fout:
