def is_streaming(device_id):
try:
c = rs.config()
c.enable_device(device_id) # device_id is in fact the serial_number
p = rs.pipeline()
p.start(c)
p.stop()
return False
except RuntimeError:
return True
def pyreassenseCap():
# 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
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
depth_image = np.asanyarray(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)
# Stack both images horizontally
images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.imshow('depth_image',depth_image)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
finally:
# Stop streaming
pipeline.stop()
def enable_imu_device(self, serial_no):
self.pipeline = rs.pipeline()
cfg = rs.config()
cfg.enable_device(serial_no)
try:
self.pipeline.start(cfg)
except Exception as e:
print('ERROR: ', str(e))
return False
# self.sync_imu_by_this_stream = rs.stream.any
active_imu_profiles = []
active_profiles = dict()
self.imu_sensor = None
for sensor in self.pipeline.get_active_profile().get_device().sensors:
for pr in sensor.get_stream_profiles():
if pr.stream_type() == rs.stream.gyro and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if pr.stream_type() == rs.stream.accel and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if self.imu_sensor:
break
if not self.imu_sensor:
print('No IMU sensor found.')
return False
print('\n'.join(['FOUND %s with fps=%s' % (str(ap[0]).split('.')[1].upper(), ap[1].fps()) for ap in active_profiles.items()]))
active_imu_profiles = list(active_profiles.values())
if len(active_imu_profiles) < 2:
print('Not all IMU streams found.')
return False
self.imu_sensor.stop()
self.imu_sensor.close()
self.imu_sensor.open(active_imu_profiles)
self.imu_start_loop_time = time.time()
self.imu_sensor.start(self.imu_callback)
# Make the device use the original IMU values and not already calibrated:
if self.imu_sensor.supports(rs.option.enable_motion_correction):
self.imu_sensor.set_option(rs.option.enable_motion_correction, 0)
return True
def enable_device(self, device_serial, enable_ir_emitter):
"""
Enable an Intel RealSense Device
Parameters:
-----------
device_serial : string
Serial number of the realsense device
enable_ir_emitter : bool
Enable/Disable the IR-Emitter of the device
"""
pipeline = rs.pipeline()
# Enable the device
self._config.enable_device(device_serial)
pipeline_profile = pipeline.start(self._config)
# Set the acquisition parameters
sensor = pipeline_profile.get_device().first_depth_sensor()
sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
self._enabled_devices[device_serial] = (Device(pipeline, pipeline_profile))
def __init__(
self,
g_pool,
device_id=None,
frame_size=DEFAULT_COLOR_SIZE,
frame_rate=DEFAULT_COLOR_FPS,
depth_frame_size=DEFAULT_DEPTH_SIZE,
depth_frame_rate=DEFAULT_DEPTH_FPS,
preview_depth=False,
device_options=(),
record_depth=True,
):
logger.debug("_init_ started")
super().__init__(g_pool)
self._intrinsics = None
self.color_frame_index = 0
self.depth_frame_index = 0
self.context = rs.context()
self.pipeline = rs.pipeline(self.context)
self.pipeline_profile = None
self.preview_depth = preview_depth
self.record_depth = record_depth
self.depth_video_writer = None
self._needs_restart = False
self.frame_size_backup = DEFAULT_COLOR_SIZE
self.depth_frame_size_backup = DEFAULT_DEPTH_SIZE
self.frame_rate_backup = DEFAULT_COLOR_FPS
self.depth_frame_rate_backup = DEFAULT_DEPTH_FPS
self._initialize_device(
device_id,
frame_size,
frame_rate,
depth_frame_size,
depth_frame_rate,
device_options,
)
logger.debug("_init_ completed")
def main():
# Load Detection Model
infer_func, inputs = detector.load_model(model_path, gpu_id)
# Tracklet Color (Visualization)
# (Later, generate colormap)
trk_colors = np.random.rand(32, 3)
if is_realsense is True:
# 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)
# 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)
clipping_distance_in_meters = DEPTH_CLIP_DISTANCE # 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)
# Initialize Tracklet and Tracklet Candidate Object
trackers = []
tracker_cands = []
# Initialize Frame Index
fidx = 0
try:
while True:
# Increase Frame Index
fidx += 1
# Wait for a coherent pair of frames: RBGD
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# DEPTH (Aligned depth frame)
depth_frame = aligned_frames.get_depth_frame()
# RGB
color_frame = aligned_frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert Images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Clip Depth Image
clipped_depth_image = np.where(
(depth_image > clipping_distance) | (depth_image <= 0),
DEPTH_CLIPPED_VALUE, depth_image)
# Apply ColorMap on DEPTH Image
# depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# DETECTION MODULE
curr_dets, DET_TIME = snu_detector(color_image, infer_func,
inputs)
# MultiModal Tracking MODULE
trackers, tracker_cands, MMMOT_TIME = snu_mmmot(
color_image, clipped_depth_image, fidx, curr_dets,
motparams, trackers, tracker_cands)
# Detection Visualization
if is_vis_detection is True:
visualize_detections(color_image, curr_dets, line_width=2)
# MMMOT Visualization
if is_vis_tracking is True:
visualize_tracklets(color_image,
trackers,
trk_colors,
line_width=3)
# Detection Speed Visualization
det_fps = "Detector Speed: " + str(round(1000 / DET_TIME,
2)) + " (fps)"
mmmot_fps = "Tracker Speed: " + str(round(
1000 / MMMOT_TIME, 2)) + " (fps)"
cv2.putText(color_image,
det_fps, (10, 20),
CV_FONT,
1.3, (255, 0, 255),
thickness=2)
cv2.putText(color_image,
mmmot_fps, (10, 50),
CV_FONT,
1.3, (255, 0, 255),
thickness=2)
# Visualization Window (using OpenCV-Python)
cv2.imshow('Tracking', color_image)
# OpenCV imshow window
cv2.waitKey(1)
finally:
# Stop Streaming
pipeline.stop()
# Destroy All opencv windows
cv2.destroyAllWindows()
else:
# Sequence Path
seq_basepath = '../DATA/'
sequences = ['ETH-Crossing', 'KITTI-17']
# For every image sequences
for seq_num, seq in enumerate(sequences):
# Sequence Number
seq_num += 1
# Current Sequence Path
seqpath = seq_basepath + seq
# Current Image Sequence Paths
seq_imgspath = seqpath + '/imgs/'
# Load Image Sequence Lists
imgNameList = os.listdir(seq_imgspath)
imgList = [seq_imgspath + filename for filename in imgNameList]
imgList.sort()
# Generalize Save Path
# Initialize Tracklet and Tracklet Candidate Object
trackers = []
tracker_cands = []
# For every frames in the image sequence
for fidx, frameName in enumerate(imgList):
# Frame Index Starts from 1
fidx += 1
# Load Current Frame Image
# frame = io.imread(frameName)
frame = cv2.imread(frameName)
# DETECTION MODULE
curr_dets, DET_TIME = snu_detector(frame, infer_func, inputs)
# MultiModal Tracking MODULE
trackers, tracker_cands, MMMOT_TIME = snu_mmmot(
frame, [], fidx, curr_dets, motparams, trackers,
tracker_cands)
# Detection Visualization
if is_vis_detection is True:
visualize_detections(frame, curr_dets, line_width=2)
# MMMOT Visualization
if is_vis_tracking is True:
visualize_tracklets(frame,
trackers,
trk_colors,
line_width=3)
# Detection Speed Visualization
det_fps = "Detector Speed: " + str(round(1000 / DET_TIME,
2)) + " (fps)"
mmmot_fps = "Tracker Speed: " + str(round(
1000 / MMMOT_TIME, 2)) + " (fps)"
cv2.putText(frame,
det_fps, (10, 20),
CV_FONT,
1.3, (255, 0, 255),
thickness=2)
cv2.putText(frame,
mmmot_fps, (10, 50),
CV_FONT,
1.3, (255, 0, 255),
thickness=2)
# Visualization Window (using OpenCV-Python)
cv2.imshow('Tracking', frame)
# OpenCV imshow window
cv2.waitKey(1)
# Destroy openCV window
cv2.destroyAllWindows()
def main(_argv):
pipe = rs.pipeline()
# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.color)
# Start streaming with requested config
pipe.start(cfg)
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'
os.environ['CUDA_VISIBLE_DEVICES'] = FLAGS.gpu
logger = tf.get_logger()
logger.disabled = True
logger.setLevel(logging.FATAL)
set_memory_growth()
cfg = load_yaml(FLAGS.cfg_path)
model = ArcFaceModel(size=cfg['input_size'],
backbone_type=cfg['backbone_type'],
training=False)
ckpt_path = tf.train.latest_checkpoint('./checkpoints/' + cfg['sub_name'])
if ckpt_path is not None:
print("[*] load ckpt from {}".format(ckpt_path))
model.load_weights(ckpt_path)
else:
print("[*] Cannot find ckpt from {}.".format(ckpt_path))
exit()
if FLAGS.update:
print('Face bank updating...')
targets, names = prepare_facebank(cfg, model)
print('Face bank updated')
else:
targets, names = load_facebank(cfg)
print('Face bank loaded')
if FLAGS.save:
video_writer = cv2.VideoWriter('./recording.avi',
cv2.VideoWriter_fourcc(*'XVID'), 10,
(640, 480))
# frame rate 6 due to my laptop is quite slow...
while True:
frame = pipe.wait_for_frames()
img = frame
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
bboxes, landmarks, faces = align_multi(
cfg, img, min_confidence=FLAGS.min_confidence, limits=3)
bboxes = bboxes.astype(int)
embs = []
for face in faces:
if len(face.shape) == 3:
face = np.expand_dims(face, 0)
face = face.astype(np.float32) / 255.
embs.append(l2_norm(model(face)).numpy())
list_min_idx = []
list_score = []
for emb in embs:
dist = [euclidean(emb, target) for target in targets]
min_idx = np.argmin(dist)
list_min_idx.append(min_idx)
list_score.append(dist[int(min_idx)])
list_min_idx = np.array(list_min_idx)
list_score = np.array(list_score)
list_min_idx[list_score > FLAGS.threshold] = -1
for idx, box in enumerate(bboxes):
frame = utils.draw_box_name(box, landmarks[idx],
names[list_min_idx[idx] + 1], frame)
frame = cv2.resize(frame, (640, 480))
cv2.imshow('face Capture', frame)
key = cv2.waitKey(1) & 0xFF
if FLAGS.save:
video_writer.write(frame)
if key == ord('q'):
break
if FLAGS.save:
video_writer.release()
pipe.stop()
#!/usr/bin/python
# -*- coding: UTF-8 -*-
import cv2 as cv
import numpy as np
import math
import pyrealsense2 as rs
import logging
# Configure depth and color streams...
# ...from Camera 1
pipeline_1 = rs.pipeline()
config_1 = rs.config()
# config_1.enable_device('908212070822') #上 自己
config_1.enable_device('912112073118') #上 手臂
config_1.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config_1.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# ...from Camera 2
pipeline_2 = rs.pipeline()
config_2 = rs.config()
config_2.enable_device('939622072919') #下
config_2.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config_2.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# Start streaming from both cameras
pipeline_1.start(config_1)
pipeline_2.start(config_2)
# dep
align_to_1 = rs.stream.color
align_1 = rs.align(align_to_1)
def main():
# initialize the known distance from the camera to the object, which
# preset distance from samples
KNOWN_DISTANCE = 390
# initialize the known object width, which in this case, the piece of
# paper, unit mm
KNOWN_WIDTH = 252
KNOWN_HEIGHT = 201
window_name = 'capture'
'''
image = cv2.imread(fn)
marker = find_marker(image)
#通过摄像头标定获取的像素焦距
focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH
print('focalLength = ',focalLength)
draw_bounding_box(image, marker)
print('output reference image to focal.png')
cv2.imwrite('focal.png', image)
'''
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
# 640x480, 640x360
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)
cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
cv2.moveWindow(window_name, 0, 0)
cnt = 0
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_image = np.asanyarray(color_frame.get_data())
#depth_image = np.asanyarray(depth_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)
original = color_image.copy()
marker = find_marker(color_image)
if marker == 0:
print(marker)
continue
#dist = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
ck.draw_bounding_box(color_image, marker)
depth_intrinsics = rs.video_stream_profile(
depth_frame.profile).get_intrinsics()
# u, v is 2D pixel coordinate
# return p is 3D (x, y, z)
def get_point(u, v):
d = depth_frame.get_distance(u, v)
p = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [u, v],
d)
return p
def dist_3d(p1, p2):
d = math.sqrt(
math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2) +
math.pow(p1[2] - p2[2], 2))
return d
def get_marker_xyz(marker):
u = int(marker[0][0])
v = int(marker[0][1])
p0 = get_point(u, v)
u = int(marker[1][0])
v = int(marker[1][1])
p1 = get_point(u, v)
d = dist_3d(p0, p1)
return d
d = get_marker_xyz(marker) * 1000
dist_from_rs = depth_frame.get_distance(int(marker[0][0]),
int(marker[0][1]))
dist_from_rs *= 1000
#print('marker:{} dist_rs:{}'.format(marker, dist_from_rs))
# cv2.putText(color_image, "%4.0fmm" % dist,
# (color_image.shape[1] - 500, color_image.shape[0] - 60),
# cv2.FONT_HERSHEY_SIMPLEX,
# 1.2, (0, 255, 0), 3)
cv2.putText(
color_image, "rs: %4dmm" % dist_from_rs,
(color_image.shape[1] - 500, color_image.shape[0] - 60),
cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 255), 3)
cv2.putText(
color_image, "wc: %4dmm" % d,
(color_image.shape[1] - 500, color_image.shape[0] - 20),
cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 127), 3)
# show a frame
cv2.imshow(window_name, color_image)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 0x1B:
break
elif key & 0xFF == ord('s'):
fn = 'result-{:02d}.png'.format(cnt)
print('save result to {}'.format(fn))
cv2.imwrite(fn, color_image)
cv2.imwrite('origin-{:02d}.png'.format(cnt), original)
cnt += 1
finally:
cv2.destroyAllWindows()
# Stop streaming
pipeline.stop()
def __init__(self):
self.pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = self.pipeline.start(config)
def __init__(self, name, size):
self.name = name
self.markersize = size
self.cnd = 0
self.frameId = 0
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, 1920, 1080, rs.format.bgr8, 30)
self.pipeline.start(config)
# Check for camera calibration data
# c_x = 643.47548083
# c_y = 363.67742746
# f_x = 906.60886808
# f_y = 909.34831447
# k_1 = 0.16962942
# k_2 = -0.5560001
# p_1 = 0.00116353
# p_2 = -0.00122694
# k_3 = 0.52491878
# self.cameraMatrix = np.array([[f_x, 0, c_x],
# [0, f_y, c_y],
# [0, 0, 1]])
# self.distCoeffs = np.array([k_1, k_2, p_1, p_2, k_3])
self.cameraMatrix = np.array([[1.38726465e+03, 0.00000000e+00, 9.67009977e+02],
[0.00000000e+00, 1.39067726e+03, 5.44111718e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.distCoeffs = np.array([ 0.1772611, -0.57056992, -0.0008356, 0.00099024, 0.52153116])
# dist_coef = np.array([0, 0, 0, 0, 0])
# self.cameraMatrix = np.array([[906.10541873, 0.0, 643.12531806],
# [0.0, 904.68643316, 359.79710938],
# [0.0, 0.0, 1.0 ]])
# self.distCoeffs = np.array([1.53041876e-01, -4.08438606e-01, 1.53722452e-03, -3.95946669e-04, 2.56666605e-01])
# if not os.path.exists('/home/iclab/wrs_ws/src/aruco_hand_eye/cfg/calibration.pckl'):
# print("You need to calibrate the camera you'll be using. See calibration project directory for details.")
# self.cameraMatrix = [[603.00869939, 0.0, 318.46049727],
# [0.0, 601.50770586, 251.87010006],
# [0.0, 0.0, 1.0 ]]
# self.distCoeffs = [[7.59282092e-02, 2.21483627e-01, 1.41152268e-03, -4.71388619e-04, -1.18482976e+00]]
# #exit()
# else:
# f = open('/home/iclab/wrs_ws/src/aruco_hand_eye/cfg/calibration.pckl', 'rb')
# (self.cameraMatrix, self.distCoeffs, _, _) = pickle.load(f)
# f.close()
# if self.cameraMatrix is None or self.distCoeffs is None:
# print("Calibration issue. Remove ./calibration.pckl and recalibrate your camera with CalibrateCamera.py.")
# exit()
# print(self.cameraMatrix)
# print(' ')
# print(self.distCoeffs)
# Create grid board object we're using in our stream
# board = aruco.GridBoard_create(
# markersX=2,
# markersY=2,
# markerLength=0.09,
# markerSeparation=0.01,
# dictionary=ARUCO_DICT)
# Create vectors we'll be using for rotations and translations for postures
self.rvecs = None
self.tvecs = None
self.rvecs_arr = np.zeros((3, NUMBER))
self.tvecs_arr = np.zeros((3, NUMBER))
# cam = cv2.VideoCapture('gridboardiphonetest.mp4')
# self.cam_left = cv2.VideoCapture(5)
# self.cam_right = cv2.VideoCapture(10)
self.cam = None
self.QueryImg = None
self.init_server()
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
self.QueryImg = np.asanyarray(color_frame.get_data())
def read_detect(self):
global detect_flag
global detections
#global classes
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pipeline.start(config)
detect_count = 0
recognize_count = 0
end_count = 0
person_exists = False
recognize_start_flag = False
recognize_end_flag = False
img_sequence = []
count = 0
try:
while True:
read_start = time.time()
frames = pipeline.wait_for_frames()
frames.keep()
color_frame = frames.get_color_frame()
if not color_frame:
continue
orig_image = np.asanyarray(color_frame.get_data())
detect_size = opt.detect_size
img, _, _, _ = resize_square(orig_image, height=detect_size, color=(127.5,127.5,127.5)) #img is of shape (416,416,3)
img = img[:, :, ::-1].transpose(2,0,1) #bgr->rgb
img = np.ascontiguousarray(img, dtype=np.float32)
img /= 255.0
read_end = time.time()
#print('read a frame cost:%.3f'%(read_end - read_start))
detect_start = time.time()
detections = None
img = torch.from_numpy(img).unsqueeze(0).to(device)
if detect_count % self.detect_sample_step == 0:
predictions = self.detect_model(img)
predictions = predictions[predictions[:, :, 4]>opt.conf_thres]
if len(predictions)>0:
detections = non_max_suppression(predictions.unsqueeze(0), opt.conf_thres, opt.nms_thres)
if detections is not None:
person_exists = True
#print(detections)
else:
person_exists = False
detect_end = time.time()
#print('detect a frame cost time:%.3f seconds using cpu'%(detect_end - detect_start))
detect_count += 1
detect_size = opt.detect_size
orig_image_copy = orig_image.copy() #deep copy
#Person Detect
if person_exists:
image_with_box, box = draw_boxes(orig_image, detect_size, detections, self.detect_classes)
recognize_start_flag = True
self.detect_sample_step = 1
recognize_size = opt.recognize_size
cv2.imwrite('data/output/copy%s.jpg'%recognize_count, orig_image_copy)
image_for_recognize = prep_image_for_recognize(orig_image_copy, recognize_size, box)
if (recognize_count % self.recognize_sample_step) == 0:
#print(count)
img_sequence.append(torch.FloatTensor(image_for_recognize))
if not os.path.exists('data/test/%s'%count):
os.makedirs('data/test/%s'%count)
cv2.imwrite('data/test/%s/%s.jpg'%(count, recognize_count), image_for_recognize)
recognize_count += 1
if recognize_count % (self.recognize_sample_step * opt.temporal_sample_length) == 0:
assert len(img_sequence)==opt.temporal_sample_length, 'got a wrong length of image sequence '
video_tensor = torch.stack(img_sequence, 0)
self.Q.put((video_tensor, orig_image))
img_sequence = []
count += 1
if self.recognition_mark is not None:
cv2.putText(image_with_box, self.recognition_mark, (10,50), cv2.FONT_HERSHEY_TRIPLEX, 1,(0,0,255),1)
cv2.imshow('recognition', image_with_box)
cv2.waitKey(30)
#No Person Detected
else:
if recognize_start_flag == False:
cv2.putText(orig_image, 'No person', (10,50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0,0,255), 1)
cv2.imshow('recognition', orig_image)#show Image with mark 'No Person'
#cv2.imwrite('data/output/img%s.jpg'%Image_count, orig_image)
cv2.waitKey(30)
#pass
else:
end_count += 1
if end_count==2:
recognize_start_flag = False
self.detect_sample_step = opt.detect_sample_step
self.recognition_mark = None
end_count = 0
except Exception as e:
print(e)
pass
def extract_from_bag(bag_file, FPS):
config = rs.config()
pipeline = rs.pipeline()
config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, FPS)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, FPS)
rs.config.enable_device_from_file(config, bag_file, repeat_playback=False)
# Start
profile = pipeline.start(config)
# this makes it so no frames are dropped while writing video
playback = profile.get_device().as_playback()
playback.set_real_time(False)
align_to = rs.stream.color
align = rs.align(align_to)
# Get data scale from the device
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
count = 0
depth_set = []
color_set = []
frame_index = []
time_stamps = []
start_time = 0
frame_num = -1
while True:
try:
frames = pipeline.wait_for_frames()
frames.keep()
if frame_num == frames.get_frame_number():
continue
frame_num = frames.get_frame_number()
# print(frame_num)
if count == 0:
time_stamps.append(0)
start_time = frames.get_timestamp()
else:
time_stamps.append(frames.get_timestamp() - start_time)
frame_index.append(frame_num)
if frames.size() < 2:
# Inputs are not ready yet
continue
except (RuntimeError):
print('Total frame count:', count)
pipeline.stop()
break
# align the deph to color frame
aligned_frames = align.process(frames)
# Align depth frame according to color frame
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
# validate that both frames are valid
if not depth_frame or not color_frame:
continue
depth_image = np.asanyarray(depth_frame.get_data())
# convert to meters
depth_image = depth_image * depth_scale
color_image = np.asanyarray(color_frame.get_data())
depth_set.append(depth_image)
color_set.append(color_image)
count += 1
print("Video processing done!")
frame_index = np.asarray(frame_index) - frame_index[0]
time_stamps = np.asarray(time_stamps) / 1000
return depth_set, color_set, frame_index, time_stamps
def __init__(self,
width=WIDTH,
height=HEIGHT,
enable_rgb=True,
enable_depth=True,
device_id=None):
self.device_id = "829212070352" # Lab: "828112071102" Home:"829212070352"
self.enable_rgb = enable_rgb
self.enable_depth = enable_depth
self.width = width
self.height = height
self.resize = (width != WIDTH) or (height != HEIGHT)
self.pipeline = None
if self.enable_depth or self.enable_rgb:
self.pipeline = rs.pipeline()
config = rs.config()
# if we are provided with a specific device, then enable it
if None != self.device_id:
config.enable_device(self.device_id)
if enable_depth:
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16,
60) # depth
if enable_rgb:
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8,
60) # rgb
# Start streaming
profile = self.pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
if self.enable_depth:
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
if self.enable_rgb:
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
self.align = rs.align(align_to)
time.sleep(1) # let camera warm up
# initialize frame state
# original color image
self.color_image = None
self.color_frame = None
self.depth_image = None
self.depth_frame = None
self.frame_count = 0
self.start_time = time.time()
self.frame_time = self.start_time
self.running = True
#For displaying (including marker detection)
self.images = None
#For detected markers
self.useful_color_image = None
# Aruco marker part
# Load the dictionary that was used to generate the markers.
self.dictionary = cv2.aruco.Dictionary_get(
cv2.aruco.DICT_ARUCO_ORIGINAL)
# Initialize the detector parameters using default values
self.parameters = cv2.aruco.DetectorParameters_create()
def main():
rsContext = rs.context()
# folderPath = '/home/sload/Desktop/ShaharSarShalom/VideoStreamSamples/'
#recordedFile = os.path.join(folderPath, '20200209_163025.bag')
#recordedFile = os.path.join(folderPath, '20200209_163301.bag')
recordedFile = os.path.join('/home/sload/Desktop/ShaharSarShalom/forSS/recs_160220_wallAt255cmFromCamera', '20200216_174116_secondRec.bag')
dstFolder = '/home/sload/Desktop/ShaharSarShalom/VideoStreamSamples/20200209_163301/VideoFrames'
parser = ArgumentParser(add_help=False)
args = parser.add_argument_group('Options')
args.add_argument('-isDebug', '--isDebug', default=True, help='')
args.add_argument('-debugPath', '--debugPath', default='/home/sload/Desktop/ShaharSarShalom/VideoStreamSamples/20200209_163301/DebugPath', help='')
args.add_argument('-isStreamColorImg', '--isStreamColorImg',default=True,help='')
args.add_argument('-isStreamInfraredImg', '--isStreamInfraredImg', default=True, help='')
args.add_argument('-isStreamDepthImg', '--isStreamDepthImg', default=True, help='')
args = parser.parse_args()
config = args
device = rsContext.load_device(recordedFile)
device = device.as_playback()
pipe = rs.pipeline(rsContext)
pipelineProfile = pipe.start()
# Declare a point cloud object
pc = rs.pointcloud()
depthPostProcessing = DepthPostProcessing()
try:
#for i in range(0, 300):
videoFrameCounter = 0
while True:
# Wait for all configured streams to produce a frame
frames = pipe.wait_for_frames()
# frames.size()
# frame = frames.as_frame()
if config.isStreamColorImg:
videoFrameCounter += 1
videoFrame = frames.get_color_frame()
if videoFrame.is_frame() and CheckFrameValidity(videoFrame):
if False:
PlotVideoFrame(videoFrame)
# WriteVideoFrame(videoFrame = videoFrame, dstFolder = dstFolder, fileIndex= videoFrameCounter)
if config.isStreamInfraredImg:
frame = frames.get_infrared_frame()
if CheckFrameValidity(frame):
depth_data = frame.get_data()
np_image = np.asanyarray(depth_data)
cv2.equalizeHist(np_image, np_image)
np_image = cv2.applyColorMap(np_image, cv2.COLORMAP_JET)
cv2.imshow("Infrared Image", np_image)
cv2.namedWindow("Infrared Image", cv2.WINDOW_AUTOSIZE)
cv2.waitKey(30)
if config.isStreamDepthImg:
depthFrame = frames.get_depth_frame()
if CheckFrameValidity(depthFrame):
PlotDepthFrame(depthFrame)
# Plot the depth map as a point cloud
# PlotPointCloud(pc, depthFrame, videoFrame)
processedDepthFrame = depthPostProcessing.ApplyPostFiltering(depthFrame)
PlotDepthFrame(processedDepthFrame)
PlotDepthFrame(depthFrame)
# print(f.profile)
device.resume()
finally:
pipe.stop()
def AD_GetWalk(index):
global INDEX, SUBJECTPATH, STATUSBOOL, STATUSARR
INDEX = index
try:
print('trying1')
global FINALORIGINALPATH
print('trying2')
FINALORIGINALPATH = SUBJECTPATH + '/WALK' + str(INDEX) + "/ORIGINAL"
print('trying3')
os.makedirs(FINALORIGINALPATH)
print('trying4')
except OSError:
print('Error in creation of directory')
else:
print('Successfully created directory')
# Capture the images and store them in 'newSubject/WALK(index)/ORIGINAL
print("Started")
getframe = False
#################################################################################
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# 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 = 3 #5.5 meter optimum for gait recognition
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:
imnumber = 0
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()
# 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 = 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)
gray = cv2.cvtColor(
bg_removed,
cv2.COLOR_BGR2GRAY) # We do some colour transformations.
#depthwhite = detect2(gray, aligned_depth_frame, clipping_distance*depth_scale) # We get the output of our detect function.
im = Image.fromarray(gray)
#im = Image.fromarray(depthwhite)
imnumber += 1
im.save(FINALORIGINALPATH + '/' + "{}.jpeg".format(imnumber))
cv2.imshow('Align Example', gray)
#cv2.imshow('Align Example', depthwhite)
print('KT', str(KILLTHREAD))
k = cv2.waitKey(1)
if k & 0xFF == ord("q") or KILLTHREAD: # Exit condition
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
# coding: utf-8
__author__ = "Nicolas Huynh"
__create_date__ = "07/10/2019"
__version__ = "0.1.0"
__maintainer__ = "Nicolas Huynh"
__email__ = "*****@*****.**"
__status__ = "Development"
import matplotlib.pyplot as plt
import numpy
import pandas
import pyrealsense2
from AcquisitionT265 import AcquisitionT265
# Declare RealSense pipeline, encapsulating the actual device and sensors
pipeline = pyrealsense2.pipeline()
cfg = pyrealsense2.config()
cfg.enable_stream(pyrealsense2.stream.pose)
pipeline.start(cfg)
try:
dataArray = []
plt.ion()
fig, ax = plt.subplots()
origin = numpy.array([0.0, 0.0, 0.0])
v0 = numpy.array([0.0, 0.0, 0.0])
a0 = numpy.array([0.0, 0.0, 0.0])
for i in range(30):
data = AcquisitionT265(pipeline, origin, v0, a0)
dataArray.append(data.get_data_pt())
df = pandas.DataFrame(dataArray,
def record_15():
global ix
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# 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()
#Enabling High Accuracy Preset
depth_sensor.set_option(rs.option.visual_preset, 3.0)
preset_name = depth_sensor.get_option_value_description(
rs.option.visual_preset, 3.0)
#Enabling Emitter To increase accuracy
enable_ir_emitter = True
depth_sensor.set_option(rs.option.emitter_enabled,
1 if enable_ir_emitter else 0)
#GET the depth scale from the SDK
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 = 6 #10 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)
colorizer = rs.colorizer() # TO colorize the depth image
#FILTERS
dec_filter = rs.decimation_filter(
) # Decimation - reduces depth frame density
dec_filter.set_option(rs.option.filter_magnitude, 4)
spat_filter = rs.spatial_filter(
) # Spatial - edge-preserving spatial smoothing
spat_filter.set_option(rs.option.filter_magnitude, 5)
spat_filter.set_option(rs.option.filter_smooth_alpha, 1)
spat_filter.set_option(rs.option.filter_smooth_delta, 50)
spat_filter.set_option(rs.option.holes_fill, 3)
temp_filter = rs.temporal_filter() # Temporal - reduces temporal noise
hole_filling = rs.hole_filling_filter() #FIll ALL THE HOLES
#FILTERS END
out = cv2.VideoWriter('data/read.avi',
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 10,
(1280, 780))
#Some Variables
count = 0
answer = {}
cv2.namedWindow("W")
cv2.setMouseCallback("W", mouse_callback)
ix = False
# Streaming loop
try:
while True:
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# frames.get_depth_frame() is a 1280x720 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
# aligned_depth_frame = dec_filter.process(aligned_depth_frame)
aligned_depth_frame = spat_filter.process(
aligned_depth_frame) #Applying Filter
aligned_depth_frame = temp_filter.process(
aligned_depth_frame) #Applying Filter
aligned_depth_frame = hole_filling.process(
aligned_depth_frame) #Applying Filter
color_frame = aligned_frames.get_color_frame() #Getting RGB 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()) #Getting final depth image
#Colorize the depth image
colorized_depth = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
# #See the depth image
# cv2.imshow("colorized depth",rescale(colorized_depth,50))
depth_image[depth_image > 5000] = 0
color_image = np.asanyarray(
color_frame.get_data()) #Getting final RGB frame
if count == 0:
answer[count] = [
depth_image, color_image
] #0 is the first image. can be used for background subtraction
count += 1
continue
if ix:
color_image1, check, values = apply_cascade(
color_image.copy(),
'models/cascades/haarcascade_lefteye_2splits.xml')
if check:
answer[count] = [depth_image, color_image]
count += 1
cv2.imshow('W', rescale(color_image, 90))
if len(answer) == 15:
break
finally:
pipeline.stop() #Turn off the camera
cv2.destroyAllWindows() #Remove all the windows
out.release()
import pickle
output = open('data/record_15.pkl', 'wb')
pickle.dump(answer, output)
output.close()
def initialize(self):
print("Initialize")
# openpifpaf configuration
class Args:
source = 0
checkpoint = None
basenet = None
dilation = None
dilation_end = None
headnets = ['pif', 'paf']
dropout = 0.0
quad = 1
pretrained = False
keypoint_threshold = None
seed_threshold = 0.2
force_complete_pose = False
debug_pif_indices = []
debug_paf_indices = []
connection_method = 'max'
fixed_b = None
pif_fixed_scale = None
profile_decoder = None
instance_threshold = 0.05
device = torch.device(type="cpu")
disable_cuda = True
scale = 1
key_point_threshold = 0.05
head_dropout = 0.0
head_quad = 0
default_kernel_size = 1
default_padding = 0
default_dilation = 1
head_kernel_size = 1
head_padding = 0
head_dilation = 0
cross_talk = 0.0
two_scale = False
multi_scale = False
multi_scale_hflip = False
paf_th = 0.1
pif_th = 0.1
decoder_workers = None
experimental_decoder = False
extra_coupling = 0.0
self.args = Args()
model, _ = nets.factory_from_args(self.args)
model = model.to(self.args.device)
# model.cuda()
self.processor = decoder.factory_from_args(self.args, model)
# realsense configuration
try:
config = rs.config()
config.enable_device(self.params["device_serial"])
config.enable_stream(rs.stream.depth, self.width, self.height,
rs.format.z16, 30)
config.enable_stream(rs.stream.color, self.width, self.height,
rs.format.bgr8, 30)
self.pointcloud = rs.pointcloud()
self.pipeline = rs.pipeline()
cfg = self.pipeline.start(config)
# profile = cfg.get_stream(rs.stream.color) # Fetch stream profile for depth stream
# intr = profile.as_video_stream_profile().get_intrinsics() # Downcast to video_stream_profile and fetch intrinsics
# print (intr.fx, intr.fy)
# depth_scale = cfg.get_device().first_depth_sensor().get_depth_scale()
# print("Depth Scale is: " , depth_scale)
# sys.exit(-1)
except Exception as e:
print("Error initializing camera")
print(e)
sysexit(-1)
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--mirror',
help='flip video horizontally',
action='store_true')
parser.add_argument('--model', help='.tflite model path.', required=False)
parser.add_argument('--res',
help='Resolution',
default='640x480',
choices=['480x360', '640x480', '1280x720'])
parser.add_argument('--videosrc',
help='Which video source to use',
default='/dev/video0')
parser.add_argument('--h264',
help='Use video/x-h264 input',
action='store_true')
parser.add_argument('--jpeg',
help='Use image/jpeg input',
action='store_true')
args = parser.parse_args()
default_model = 'models/posenet_mobilenet_v1_075_%d_%d_quant_decoder_edgetpu.tflite'
if args.res == '480x360':
src_size = (640, 480)
appsink_size = (480, 360)
model = args.model or default_model % (353, 481)
elif args.res == '640x480':
src_size = (640, 480)
appsink_size = (640, 480)
model = args.model or default_model % (481, 641)
elif args.res == '1280x720':
src_size = (1280, 720)
appsink_size = (1280, 720)
model = args.model or default_model % (721, 1281)
print('Loading model: ', model)
engine = PoseEngine(model, mirror=args.mirror)
input_shape = engine.get_input_tensor_shape()
inference_size = (input_shape[2], input_shape[1])
n = 0
sum_process_time = 0
sum_inference_time = 0
fps_counter = avg_fps_counter(30)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, src_size[0], src_size[1],
rs.format.z16, 30)
config.enable_stream(rs.stream.color, src_size[0], src_size[1],
rs.format.rgb8, 30)
pipeline.start(config)
c = rs.colorizer()
align_to = rs.stream.color
align = rs.align(align_to)
while True:
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
color = aligned_frames.get_color_frame()
depth = aligned_frames.get_depth_frame()
if not depth: continue
color_data = color.as_frame().get_data()
color_image_rgb = np.asanyarray(color_data)
color_image = cv2.cvtColor(color_image_rgb, cv2.COLOR_RGB2BGR)
depth_colormap = c.colorize(depth)
depth_data = depth_colormap.as_frame().get_data()
depth_image = np.asanyarray(depth_data)
depth_image = cv2.cvtColor(depth_image, cv2.COLOR_RGB2BGR)
if (color_image_rgb.shape[0] != appsink_size[1]
or color_image_rgb.shape[1] != appsink_size[0]):
color_image_rgb = cv2.resize(color_image_rgb,
dsize=appsink_size,
interpolation=cv2.INTER_NEAREST)
start_time = time.monotonic()
outputs, inference_time = engine.DetectPosesInImage(color_image_rgb)
end_time = time.monotonic()
n += 1
sum_process_time += 1000 * (end_time - start_time)
sum_inference_time += inference_time
avg_inference_time = sum_inference_time / n
text_line = 'PoseNet: %.1fms (%.2f fps) TrueFPS: %.2f Nposes %d' % (
avg_inference_time, 1000 / avg_inference_time, next(fps_counter),
len(outputs))
if args.mirror:
color_image = cv2.flip(color_image, 1)
depth_image = cv2.flip(depth_image, 1)
shadow_text(color_image, 10, 20, text_line)
shadow_text(depth_image, 10, 20, text_line)
for pose in outputs:
draw_pose(color_image, pose, src_size, appsink_size)
draw_pose(depth_image, pose, src_size, appsink_size)
cv2.imshow('color', color_image)
cv2.imshow('depth', depth_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
return
def rip_bag(path_people_bag, path_save_photo):
if not os.path.exists(path_people_bag):
print("Файла не найдена")
return 0
clear_dir(path_save_photo)
try:
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 60)
config.enable_device_from_file(path_people_bag, repeat_playback=False)
profile = pipeline.start(config)
playback = profile.get_device().as_playback()
playback.set_real_time(False)
colorizer = rs.colorizer()
i = 0
while True:
try:
frames = pipeline.wait_for_frames()
playback.pause()
except RuntimeError as e:
print("error, ", str(e))
# pipeline.stop()
print("Файл закрылся")
#del pipeline
print("del pipeline")
# config = None
print("del config")
# del frames
print("del frames")
print("Переменные удалены")
break
else:
i += 1
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
print("Saving frame:", i)
color_frame = np.asanyarray(color_frame.get_data())
depth_image = np.asanyarray(depth_frame.get_data())
img = cv2.cvtColor(color_frame, cv2.COLOR_BGR2RGB)
f = open(os.path.join(path_save_photo, str(i) + '.csv'), 'w')
for y in range(480):
for x in range(640):
dist = depth_frame.get_distance(x, y)
f.write(str(dist))
f.write(',')
f.write('\n')
f.close()
cv2.imwrite(os.path.join(path_save_photo, str(i) + '.png'), img)
cv2.imwrite(os.path.join(path_save_photo, str(i) + '_dep.png'), depth_image)
if i >= 50:
break
playback.resume()
except BaseException as e:
print("Errorrrrr", str(e))
def get_frames(filename):
# Configure options and start stream
pipeline = rs.pipeline()
config = rs.config()
config.enable_device_from_file(filename)
config.enable_all_streams()
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
global depth_scale
depth_scale = depth_sensor.get_depth_scale()
frame_num = 0
frames_processed = 0
frames_calculated = 0
frames_to_skip = 1
color_images = []
depth_images = []
while True:
# Get frame from bag
frames = pipeline.wait_for_frames()
# Stop if the bag is done playing
if frames.frame_number < frame_num:
break
else:
frame_num = frames.frame_number
# Align depth to colour
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Obtain depth and colour frames
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
# If their is either no depth or color frame try again
if not depth_frame or not color_frame:
continue
# Decimation Filter
dec_filter = rs.decimation_filter()
# Edge-preserving smoothing
spat_filter = rs.spatial_filter()
# Apply Filters
depth_frame = dec_filter.process(depth_frame)
depth_frame = spat_filter.process(depth_frame)
if frames_processed % frames_to_skip == 0:
# Obtain Intrinsics
depth_intrinsics = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
depth_image = (np.asanyarray(depth_frame.get_data())*depth_scale).astype(np.float32)
depth_image[depth_image > 10] = 0
depth_image[depth_image < 0.3] = 0
color_image = np.asanyarray(color_frame.get_data())
if color_image.shape[0] != depth_image.shape[0]:
color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
# color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
color_image = cv2.resize(color_image, (depth_image.shape[1], depth_image.shape[0])).astype(np.uint8)
depth_images.append(depth_image)
color_images.append(color_image)
frames_calculated += 1
print("", end='')
print("\rFrame Number: {}".format(frames_calculated), end='')
if frames_processed >= 500:
break
frames_processed += 1
pipeline.stop()
print("")
return color_images, depth_images, color_intrinsics, depth_intrinsics
def __init__(self,image_size=(640,480)):
self.pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, image_size[0], image_size[1], rs.format.z16, 30)
config.enable_stream(rs.stream.color, image_size[0], image_size[1], rs.format.bgr8, 30)
self.profile = self.pipeline.start(config)
if min_val < new < max_val:
return False
return True
NetworkTables.initialize(server='roborio-166-frc.local')
sd = NetworkTables.getTable('SmartDashboard')
# sd.putNumber('someNumber', 1234)
# otherNumber = sd.getNumber('otherNumber')
WIDTH = 640
HEIGHT = 480
POINT_SAMPLES = 5
pipe = rs2.pipeline(
) # The camera's API sucks, but at least I can guarantee setings
config = rs2.config()
config.enable_stream(rs2.stream.color, WIDTH, HEIGHT, rs2.format.bgr8, 60)
config.enable_stream(rs2.stream.depth, WIDTH, HEIGHT, rs2.format.z16, 60)
profile = pipe.start(config)
s = profile.get_device().query_sensors()[1]
s.set_option(rs2.option.brightness, 0)
s.set_option(rs2.option.contrast, 100)
s.set_option(rs2.option.exposure, 45)
s.set_option(rs2.option.gain, 75)
s.set_option(rs2.option.gamma, 100)
s.set_option(rs2.option.hue, 0)
s.set_option(rs2.option.saturation, 50)
s.set_option(rs2.option.sharpness, 0)
s.set_option(rs2.option.white_balance, 2800)
def main():
global handin
#img_rows, img_cols, maxFrames = 32, 32, 100
img_rows, img_cols, maxFrames = 32, 32, 55
depthFrame = 0
cameraHeightR = 480
cameraWidthR = 848
#cameraWidthR = 640
frameRateR = 60
#frameRateR = 30
#crop parameter
xupam = 350
yupam = 200
xdpam = 250
ydpam = 300
classGest = ['1','11','12','13','4','5','7','8']
delayGest = 10
delayBol = False
backgroundRemove = True
boxCounter = True
#load the model and weight
json_file = open('3dcnnresult/24/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/24/3dcnnmodel.hd5")
print("Loaded model from disk")
loaded_model.compile(loss=categorical_crossentropy,
optimizer='rmsprop', metrics=['accuracy'])
#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)
'''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(clipping_distance)
# 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
timeStart = 0
timeEnd = 0
x,y,w,h = 0,0,0,0
align_to = rs.stream.color
align = rs.align(align_to)
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() # 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())
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
#cv2.imshow('color_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 len(faces) > 0:
if gestCatch == False and delayBol == False:
for f in faces:
xf, yf, wf, hf = f
farea = wf*hf
#print(farea)
if farea > 9000 and farea < 12000:
x, y, w, h = f
#gestStart = True
#x, y, w, h = faces[0]
else:
x, y, w, h = x, y, w, h
fArea = w*h
#print(fArea)
#if fArea > 3000:
# crop the face then pass to resize
cv2.rectangle(draw_image, (x, y), (x + w, y + h), (255, 0, 0), 2)
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:
depth_imageS = depth_image*depth_scale
deptRef = depth_imageS.item(midy, midx)
#print(clipping_distance)
clipping_distance = (deptRef + 0.2)/depth_scale
#print(clipping_distance)
boxColor = color_image.copy()
handin = checkhandIn(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image)
timeStart = time.time()
if handin == True:
gestStart = True
else:
gestStart = False
#print("gest start = ", gestStart)
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+"["
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))
timeEnd = time.time()
numTime = timeEnd - timeStart
fps = 55/numTime
print("fps: ",fps)
#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
gestCatch = False
handin = False
delayBol = True
#gestStart = False
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, 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
gestCatch = False
handin = False
gestStart = False
# 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 get_deprojection_position(obj_name='bottle'):
# Create a pipeline
pipeline = rs.pipeline()
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.disable_stream(rs.stream.color)
# Start streaming
profile = pipeline.start(config)
# 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)
depth_point = [0, 0, 0]
# 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()
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
color_image = np.asanyarray(color_frame.get_data())
color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
# objs = get_object(color_image, obj_name)
objs = []
res = objectTrack(frame)
objs = objs.append(findPixelLocation(res))
if objs:
obj_depth = aligned_depth_frame.get_distance(
objs[-1][0], objs[-1][1])
print('%s pixel: (%d, %d) depth: %f' %
(obj_name, objs[-1][0], objs[-1][1], obj_depth))
depth_intrin = aligned_depth_frame.profile.as_video_stream_profile(
).intrinsics
# depth_pixel = [depth_intrin.ppx, depth_intrin.ppy]
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, objs[-1], obj_depth)
# print('xyz:', depth_point)
print('rotate:', rotate_coordinate(depth_point, rotate_angle))
cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Align Example', color_image)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
return rotate_coordinate(depth_point, rotate_angle)
#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Sun Mar 21 14:06:40 2021 @author: wouter """ import pyrealsense2 as rs import numpy as np import cv2 import time a = 0 k = 0 i = 0 pipeline = rs.pipeline() config = rs.config() cx = 0 cy = 0 config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15) config.enable_stream(rs.stream.infrared, 640, 480, rs.format.y8, 15) cfg = pipeline.start(config) dev = cfg.get_device() colorizer = rs.colorizer() colorizer.set_option(rs.option.color_scheme, 0) #0 0-9 depth_sensor = dev.first_depth_sensor() depth_sensor.set_option(rs.option.visual_preset, 2) #2 0-5 emitter = depth_sensor.get_option(rs.option.emitter_enabled)
def main(argv):
size, file_cnt = read_parameters(argv)
pipeline = rs.pipeline()
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()
auto_calibrated_device = rs.auto_calibrated_device(device)
if not auto_calibrated_device:
print("The connected device does not support auto calibration")
return
config.enable_stream(rs.stream.depth, 256, 144, rs.format.z16, 90)
conf = pipeline.start(config)
calib_dev = rs.auto_calibrated_device(conf.get_device())
def on_chip_calib_cb(progress):
print(". ")
while True:
try:
operation = input("Please select what the operation you want to do\nc - on chip calibration\nt - tare calibration\ng - get the active calibration\nw - write new calibration\ne - exit\n")
if operation == 'c':
print("Starting on chip calibration")
new_calib, health = calib_dev.run_on_chip_calibration(file_cnt, on_chip_calib_cb, 5000)
print("Calibration completed")
print("health factor = ", health)
if operation == 't':
print("Starting tare calibration")
ground_truth = float(input("Please enter ground truth in mm\n"))
new_calib, health = calib_dev.run_tare_calibration(ground_truth, file_cnt, on_chip_calib_cb, 5000)
print("Calibration completed")
print("health factor = ", health)
if operation == 'g':
calib = calib_dev.get_calibration_table()
print("Calibration", calib)
if operation == 'w':
print("Writing the new calibration")
calib_dev.set_calibration_table(new_calib)
calib_dev.write_calibration()
if operation == 'e':
pipeline.stop()
return
print("Done\n")
except Exception as e:
print(e)
except:
print("A different Error")
corners = np.int0(corners)
for i in corners:
x_corner, y_corner = i.ravel()
return x_corner, y_corner
"""
WRITE SYSTEM PROPERTIES
"""
print("Python Version: " + sys.version)
print("Windows Version: " + platform.platform())
"""
DEFINE STREAM PROPERTIES
"""
#streams
pipeline = rs.pipeline() #create pipeline
config = rs.config() #create a configuration
config.enable_stream(rs.stream.color, columns, rows, rs.format.bgr8,
f_rate) #get the color stream
config.enable_stream(rs.stream.depth, columns, rows, rs.format.z16,
f_rate) #get the depth stream
config.enable_stream(rs.stream.infrared, 1, columns, rows, rs.format.y8,
f_rate) #get left IR streams
config.enable_stream(rs.stream.infrared, 2, columns, rows, rs.format.y8,
f_rate) #get right IR streams
# start streaming
profile = config.resolve(pipeline)
profile = pipeline.start(config)
# get the depth sensor scale
import pyrealsense2 as pyrs
import pathlib
file_path = "./{}/{}".format(data_path, name_path)
#Create directories
if write:
pathlib.Path(file_path).mkdir(parents=True, exist_ok=True)
fourcc = cv2.VideoWriter_fourcc(*'MJPG')
color_out = cv2.VideoWriter('./{}/{}/color.avi'.format(data_path, name_path), fourcc, 15.0, (640, 480))
depth_out = cv2.VideoWriter('./{}/{}/depth.avi'.format(data_path, name_path), fourcc, 15.0, (640, 480))
#Set up camera
cam = pyrs.pipeline()
config = pyrs.config()
config.enable_stream(pyrs.stream.color, res_col[0], res_col[1], pyrs.format.bgr8, fps)
config.enable_stream(pyrs.stream.depth, res_dep[0], res_dep[1], pyrs.format.z16, fps)
cam.start(config)
index = 0
while True:
#Get frame
frames = cam.wait_for_frames()
def start(self):
# Start streaming
self.pipeline = rs.pipeline()
self.pipeline.start(self.config)
print('pipline start')
def main():
global fx, fy, cx, cy, plane_x, plane_y, plane_z
# args = build_argparser().parse_args() # wzy change
parser = ArgumentParser(add_help=False)
args = parser.add_argument_group('Options')
args.cpu_extension = False
args.model = 'frozen_darknet_yolov3_model.xml'
args.bin = 'frozen_darknet_yolov3_model.bin'
args.device = 'MYRIAD'
args.labels = 'frozen_darknet_yolov3_model.mapping'
args.input = 'cam'
args.prob_threshold = 0.8
args.iou_threshold = 0.6
args.raw_output_message = True
args.no_show = False
# ------------- 1. Plugin initialization for specified device and load extensions library if specified -------------
log.info("Creating Inference Engine...")
ie = IECore()
if args.cpu_extension and 'CPU' in args.device:
ie.add_extension(args.cpu_extension, "CPU")
# -------------------- 2. Reading the IR generated by the Model Optimizer (.xml and .bin files) --------------------
log.info("Loading network")
# wzy change
#net = ie.read_network(args.model, os.path.splitext(args.model)[0] + ".bin")
# net = ie.read_network('frozen_darknet_yolov3_model.xml', "frozen_darknet_yolov3_model.bin")
net = ie.read_network(args.model, args.bin)
# ---------------------------------- 3. Load CPU extension for support specific layer ------------------------------
if "CPU" in args.device:
supported_layers = ie.query_network(net, "CPU")
not_supported_layers = [
l for l in net.layers.keys() if l not in supported_layers
]
if len(not_supported_layers) != 0:
log.error(
"Following layers are not supported by the plugin for specified device {}:\n {}"
.format(args.device, ', '.join(not_supported_layers)))
log.error(
"Please try to specify cpu extensions library path in sample's command line parameters using -l "
"or --cpu_extension command line argument")
sys.exit(1)
assert len(net.inputs.keys(
)) == 1, "Sample supports only YOLO V3 based single input topologies"
# ---------------------------------------------- 4. Preparing inputs -----------------------------------------------
log.info("Preparing inputs")
input_blob = next(iter(net.inputs))
# Defaulf batch_size is 1
net.batch_size = 1
# Read and pre-process input images
n, c, h, w = net.inputs[input_blob].shape
if args.labels:
with open(args.labels, 'r') as f:
labels_map = [x.strip() for x in f]
else:
labels_map = None
# with open('frozen_darknet_yolov3_model.mapping', 'r') as f:
# labels_map = [x.strip() for x in f]
# input_stream = 0 if args.input == "cam" else args.input
#
# is_async_mode = True
# cap = cv2.VideoCapture(input_stream)
#
# number_input_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
# number_input_frames = 1 if number_input_frames != -1 and number_input_frames < 0 else number_input_frames
#
# wait_key_code = 1
#
# # Number of frames in picture is 1 and this will be read in cycle. Sync mode is default value for this case
# if number_input_frames != 1:
# ret, frame = cap.read()
# else:
# is_async_mode = False
# wait_key_code = 0
# ----------------------------------------- 5. Loading model to the plugin -----------------------------------------
log.info("Loading model to the plugin")
exec_net = ie.load_network(network=net,
num_requests=2,
device_name=args.device)
cur_request_id = 0
next_request_id = 1
render_time = 0
parsing_time = 0
# ----------------------------------------------5.1 realsense input video stream -----------------------------------
# Configure depth and color streams
pipeline = rs.pipeline()
# 创建 config 对象：
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
get_img_flag = False
count = 0
while not (get_img_flag and count > 10): # 多取几幅图片，前几张不清晰
# Wait for a coherent pair of frames（一对连贯的帧）: depth and color
frames = pipeline.wait_for_frames()
print('wait for frames in the first loop')
get_color_frame = frames.get_color_frame()
if not get_color_frame: # 如果color没有得到图像，就continue继续
continue
color_frame = np.asanyarray(get_color_frame.get_data())
get_img_flag = True # 跳出循环
count += 1
# ----------------------------------------------- 6. Doing inference -----------------------------------------------
log.info("Starting inference...")
print(
"To close the application, press 'CTRL+C' here or switch to the output window and press ESC key"
)
print(
"To switch between sync/async modes, press TAB key in the output window"
)
is_async_mode = True
try:
while True:
# Here is the first asynchronous point: in the Async mode, we capture frame to populate the NEXT infer request
# in the regular mode, we capture frame to the CURRENT infer request
# >>>>>>>>>>> calculate time sum >>>>>>>>>>>>>>>#
cpu_start = time()
# print('--------------------------new loop---------------------------------')
# if is_async_mode:
# ret, next_frame = cap.read()
# else:
# ret, frame = cap.read()
#
# if not ret:
# break
# Wait for a coherent pair of frames（一对连贯的帧）: depth and color
next_frames = pipeline.wait_for_frames()
get_next_color_frame = next_frames.get_color_frame()
next_color_frame = np.asanyarray(get_next_color_frame.get_data())
# cv2.imshow("color", next_color_frame)
if is_async_mode:
request_id = next_request_id
in_frame = cv2.resize(color_frame, (w, h))
else:
request_id = cur_request_id
in_frame = cv2.resize(color_frame, (w, h))
# resize input_frame to network size
in_frame = in_frame.transpose(
(2, 0, 1)) # Change data layout from HWC to CHW
in_frame = in_frame.reshape((n, c, h, w))
# Start inference
start_time = time()
exec_net.start_async(request_id=request_id,
inputs={input_blob: in_frame})
det_time = time() - start_time
# Collecting object detection results
objects = list()
if exec_net.requests[cur_request_id].wait(-1) == 0:
output = exec_net.requests[cur_request_id].outputs
start_time = time()
for layer_name, out_blob in output.items():
out_blob = out_blob.reshape(
net.layers[net.layers[layer_name].parents[0]].shape)
layer_params = YoloParams(net.layers[layer_name].params,
out_blob.shape[2])
# log.info("Layer {} parameters: ".format(layer_name))
layer_params.log_params()
objects += parse_yolo_region(out_blob, in_frame.shape[2:],
next_color_frame.shape[:-1],
layer_params,
args.prob_threshold)
parsing_time = time() - start_time
# Filtering overlapping boxes with respect to the --iou_threshold CLI parameter
objects = sorted(objects,
key=lambda obj: obj['confidence'],
reverse=True)
for i in range(len(objects)):
if objects[i]['confidence'] == 0:
continue
for j in range(i + 1, len(objects)):
if intersection_over_union(
objects[i], objects[j]) > args.iou_threshold:
objects[j]['confidence'] = 0
# Drawing objects with respect to the --prob_threshold CLI parameter
objects = [
obj for obj in objects
if obj['confidence'] >= args.prob_threshold
]
#if len(objects) and args.raw_output_message:
# log.info("\nDetected boxes for batch {}:".format(1))
# log.info(" Class ID | Confidence | XMIN | YMIN | XMAX | YMAX | COLOR ")
origin_im_size = color_frame.shape[:-1]
count = 1
for obj in objects:
#print('for obj count:')
#print(count)
count = count + 1
# Validation bbox of detected object
if obj['xmax'] > origin_im_size[1] or obj[
'ymax'] > origin_im_size[0] or obj['xmin'] < 0 or obj[
'ymin'] < 0:
continue
# color = (int(min(obj['class_id'] * 12.5, 255)),
# min(obj['class_id'] * 7, 255), min(obj['class_id'] * 5, 255))
color = (100, 100, 150)
det_label = labels_map[obj['class_id']] if labels_map and len(labels_map) >= obj['class_id'] else \
str(obj['class_id'])
# if args.raw_output_message:
# log.info(
# "{:^9} | {:10f} | {:4} | {:4} | {:4} | {:4} | {} ".format(det_label, obj['confidence'], obj['xmin'],
# obj['ymin'], obj['xmax'], obj['ymax'],
# color))
cv2.rectangle(color_frame, (obj['xmin'], obj['ymin']),
(obj['xmax'], obj['ymax']), color, 2)
cv2.putText(
color_frame, "#" + str(obj['class_id']) + ' ' +
str(round(obj['confidence'] * 100, 1)) + ' %',
(obj['xmin'], obj['ymin'] - 7), cv2.FONT_HERSHEY_COMPLEX,
0.6, color, 1)
# print("obj['class_id']: ", obj['class_id'])
send_data_byte = bytes(0)
if len(objects) == 0:
pickup_leftup_rightdown_corner = [
-1, 0, 0, 0, 0, cpu_start, -1
]
for i in range(len(pickup_leftup_rightdown_corner)):
#print(pickup_leftup_rightdown_corner[i])
pickup_senddata = str(
pickup_leftup_rightdown_corner[i]) + ','
# print(pickup_senddata.encode())
send_data_byte += pickup_senddata.encode()
# print(send_data_byte)
conn.send(send_data_byte)
else:
objects = sorted(
objects, key=lambda obj: obj['class_id'],
reverse=False) ##sort lambda 函数，按class_id值从小到大排列
obj = objects[0]
target_leftup_rightdown_corner = [
1, obj['xmin'], obj['ymin'], obj['xmax'], obj['ymax'],
cpu_start, obj['class_id']
]
for i in range(len(target_leftup_rightdown_corner)):
target_senddata = str(
target_leftup_rightdown_corner[i]) + ','
send_data_byte += target_senddata.encode()
conn.send(send_data_byte)
# Draw performance stats over frame
inf_time_message = "Inference time: N\A for async mode" if is_async_mode else \
"Inference time: {:.3f} ms".format(det_time * 1e3)
render_time_message = "OpenCV rendering time: {:.3f} ms".format(
render_time * 1e3)
async_mode_message = "Async mode is on. Processing request {}".format(cur_request_id) if is_async_mode else \
"Async mode is off. Processing request {}".format(cur_request_id)
parsing_message = "YOLO parsing time is {:.3f} ms".format(
parsing_time * 1e3)
cv2.putText(color_frame, inf_time_message, (15, 15),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (200, 10, 10), 1)
cv2.putText(color_frame, render_time_message, (15, 45),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
cv2.putText(color_frame, async_mode_message,
(10, int(origin_im_size[0] - 20)),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
cv2.putText(color_frame, parsing_message, (15, 30),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
start_time = time()
if not args.no_show:
cv2.imshow("DetectionResults", color_frame)
render_time = time() - start_time
if is_async_mode:
cur_request_id, next_request_id = next_request_id, cur_request_id
color_frame = next_color_frame
if not args.no_show:
#key = cv2.waitKey(wait_key_code)
key = cv2.waitKey(1)
# ESC key
if key == 27:
break
# Tab key
if key == 9:
exec_net.requests[cur_request_id].wait()
is_async_mode = not is_async_mode
# log.info("Switched to {} mode".format("async" if is_async_mode else "sync"))
cpu_end = time()
#print('>>>>>>>>>>>>>>>>>>>>>>>>>>cpu time : ', cpu_end-cpu_start)
finally:
pipeline.stop()
cv2.destroyAllWindows()
def process_frames(rgb_dir, dpt_dir, dpt_raw_dir, rgb_itc_dir, dpt_itc_dir,
lbl_dir):
"""
reads color and depth frame
aligns the frames to the color frame
writes depth frame, color frame and intrinsics to files
:param lbl_dir: directory to save label file to
:param dpt_raw_dir: directory to save unaligned depth frames to
:param rgb_dir: directory to save color frames to
:param dpt_dir: directory to save depth frames to
:param rgb_itc_dir: directory to save color intrinsics to
:param dpt_itc_dir: directory to save depth intrinsics to
"""
prefix = args.bag_file.split(".")[0] # name of file without extension
index_file = open(args.directory + "/" + "all.txt", "a")
try:
config = rs.config()
print(args.bag_directory + args.bag_file)
rs.config.enable_device_from_file(config,
args.bag_directory + args.bag_file,
repeat_playback=False)
pipeline = rs.pipeline()
config.enable_stream(rs.stream.depth)
config.enable_stream(rs.stream.color)
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
rgb_profile = profile.get_stream(rs.stream.color)
dpt_profile = profile.get_stream(rs.stream.depth)
rgb_intrinsics = rgb_profile.as_video_stream_profile().get_intrinsics()
dpt_intrinsics = dpt_profile.as_video_stream_profile().get_intrinsics()
i = 0
while True:
frames = pipeline.wait_for_frames()
dpt_raw_frame = frames.get_depth_frame()
aligned_frames = align.process(frames)
bgr_frame = aligned_frames.get_color_frame()
dpt_frame = aligned_frames.get_depth_frame()
bgr_image = np.asanyarray(bgr_frame.get_data())
rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_RGB2BGR)
dpt_image = np.asanyarray(dpt_frame.get_data())
dpt_raw_image = np.asanyarray(dpt_raw_frame.get_data())
print("Saving frame:", i)
rgb_file = rgb_dir + "/" + prefix + "_" + str(i).zfill(6) + ".jpg"
dpt_file = dpt_dir + "/" + prefix + "_" + str(i).zfill(6) + ".png"
dpt_raw_file = dpt_raw_dir + "/" + prefix + "_" + str(i).zfill(
6) + ".png"
rgb_itc_file = rgb_itc_dir + "/" + prefix + "_" + str(i).zfill(
6) + ".json"
dpt_itc_file = dpt_itc_dir + "/" + prefix + "_" + str(i).zfill(
6) + ".json"
lbl_file = lbl_dir + "/" + prefix + "_" + str(i).zfill(6) + ".json"
cv2.imwrite(rgb_file, rgb_image)
cv2.imwrite(dpt_file, dpt_image)
cv2.imwrite(dpt_raw_file, dpt_raw_image)
write_intrinsics(rgb_intrinsics, rgb_itc_file)
write_intrinsics(dpt_intrinsics, dpt_itc_file)
index_file.write(
strip(rgb_file) + " " + strip(dpt_file) + " " +
strip(rgb_itc_file) + " " + strip(dpt_itc_file) + " " +
strip(lbl_file) + " " + strip(dpt_raw_file) + "\n")
time.sleep(1.000) # dont use all frames
i += 1
finally:
index_file.close()
pass
def run(self):
try:
machien = {'id': 1, 'ip': 'http://192.168.0.10'}
response_machien = requests.put(SERVER_URL + "/myfarm/register/ip",
data=machien)
print(f" machien ip set server : {response_machien.status_code}")
# GUI 코드
while True:
response_user = requests.get(SERVER_URL + "/user/info/3")
print(f" user data : {response_user.status_code}")
if response_user.status_code == 200:
break
time.sleep(5)
# while True :
# time.sleep(1)
# print("exist check ...")
# pipeline.start()
# frames = pipeline.wait_for_frames()
# color_frame = frames.get_color_frame()
# color_image = np.asarray(color_frame.get_data())
## take the only location of mushroom pot -> 1/3 * width,1/2*height
# recent_image = color_image[100:350, 290:550]
# check_image = cv2.imread('./check.jpg')[100:350, 290:550]
# cv2.imwrite('./rec.jpg',check_image)
# cv2.imwrite('./recent.jpg',recent_image)
# hist_recent = cv2.calcHist(recent_image, [0,1], None, [180,256], [0,180,0,256])
# hist_check = cv2.calcHist(check_image, [0,1], None, [180,256], [0,180,0,256])
# number = cv2.compareHist(hist_recent, hist_check, cv2.HISTCMP_CORREL)
# print(number)
# pipeline.stop()
# if number > 0.4:
# print('Not exist')
# else:
# 배지입력 확인
# print("Exist !!")
# break
while self.isRun:
params = {'pin': PIN}
response = requests.get(SERVER_URL + '/farm/exist',
params=params)
if response.status_code == 200:
prg_id = response.text
break
else:
print("Not prg")
time.sleep(5) # 429 에러 방지
params = {'id': prg_id, 'type': 'custom'}
response = requests.get(SERVER_URL + '/farm/data', params=params)
result = json.loads(response.text)
water_num = result['water']
temp_array = result['temperature']
hum_array = result['humidity']
params_status = {'id': "1", "status": "true"}
response_status = requests.put(SERVER_URL + '/myfarm/status',
params=params_status)
print(f"machien status : {response_status.status_code}")
data_len = len(temp_array)
data = [None] * data_len
for i in range(0, data_len):
temp_array[i] = str(temp_array[i]['setting_value'])
hum_array[i] = str(hum_array[i]['setting_value'])
data[i] = R + C + temp_array[i] + S + hum_array[i]
print(f"water_num : {water_num}")
print(f"temp_array : {temp_array}")
print(f"hum_array : {hum_array}")
print(f"total_data : {data}")
# return True if data else False
# HOUR * 24 / water_num
WATER_TIME = HOUR * 24 / water_num
serial_send_len = 0
hour = 0 # 시간 초로 변환
# WATER_TIME
water_time = 10000 # 값 받아 오면 연산할 물주기 시간
# MOTER_TIME
moter_time = MOTER_TIME
picTime = 100000
now = datetime.datetime.now()
Arduino = serial.Serial(port='COM5', baudrate=9600)
print(f"data success : {data}")
seconds = 0
dt2 = None
loadTime = 0
hum = 0
temp = 0
while self.isRun:
dt1 = datetime.datetime.now()
result = dt1 - now
seconds = int(result.total_seconds()) - loadTime
print(
f"Python hour : {hour}, water_time : {water_time} moter_time : {moter_time} image : {picTime}"
)
print(f"Python seconds : {seconds}")
if Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
hum = code[18:20]
temp = code[38:40]
# socket_data(temp, hum)
self.signal.emit(str(temp), str(hum))
if seconds - 2 <= hour <= seconds + 2:
if len(data) - 1 < serial_send_len:
response_status_prg = requests.put(
SERVER_URL + f'/farm/end?id={prg_id}')
print(
f"prg status : {response_status_prg.status_code}")
params_status = {'id': "1", "status": "false"}
response_status = requests.put(SERVER_URL +
'/myfarm/status',
params=params_status)
print(
f"machien status : {response_status.status_code}")
break
Arduino.write(data[serial_send_len].encode('utf-8'))
req_data_humi_temp = {
'prgId': prg_id,
'tempValue': temp,
'humiValue': hum
}
humi_temp_res = requests.post(SERVER_URL + "/data/add",
data=req_data_humi_temp)
print(f"Python res_temp : {humi_temp_res.status_code}")
serial_send_len += 1
hour += DAY
if seconds - 2 <= water_time <= seconds + 2:
Arduino.write(WATER_ORDER.encode('utf-8'))
dt2 = datetime.datetime.now()
while Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
if code[0:3] == 'end':
loadTime += int((datetime.datetime.now() -
dt2).total_seconds())
break
water_time += WATER_TIME
if seconds - 2 <= moter_time - HOUR / 3 <= seconds + 2:
if pipeline_check == False:
pipeline_check = True
Arduino.write(MOTER_ORDER.encode('utf-8'))
dt2 = datetime.datetime.now()
# 3d config
device = rs.context().query_devices()[0]
advnc_mode = rs.rs400_advanced_mode(device)
depth_table_control_group = advnc_mode.get_depth_table(
)
depth_table_control_group.disparityShift = 60
depth_table_control_group.depthClampMax = 4000
depth_table_control_group.depthClampMin = 0
advnc_mode.set_depth_table(depth_table_control_group)
pipeline1 = rs.pipeline()
config1 = rs.config()
config1.enable_stream(rs.stream.depth, rs.format.z16,
30)
config1.enable_stream(rs.stream.color, rs.format.bgr8,
30)
# Start streaming
profile1 = pipeline1.start(config)
# Get stream profile and camera intrinsics
profile = pipeline1.get_active_profile()
depth_profile = rs.video_stream_profile(
profile1.get_stream(rs.stream.depth))
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 1)
file_name_3d = [0, 90, 180, 270]
file_names_3d = [
'./3dScan{}.ply'.format(i) for i in file_name_3d
]
i = 0
while Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
print(f"i : {i}")
if code[0:3] == 'end':
loadTime += int((datetime.datetime.now() -
dt2).total_seconds())
break
take_3Dpicture(file_names_3d[i], pipeline1,
decimate, pc)
i += 0 if i >= 3 else 1
files = {'ply': open(file_names_3d[0], 'rb')}
req_data_3d = [("machineid", 1)]
res_3d = requests.post(SERVER_URL + "/upload/ply",
files=files,
data=req_data_3d)
print(f"Python res_3d : {res_3d.status_code}")
pipeline1.stop()
moter_time += MOTER_TIME
pipeline_check = False
else:
moter_time += 50
if seconds - 2 <= picTime + 30 <= seconds + 2:
if pipeline_check == False:
pipeline_check = True
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor(
)
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
# Convert images to numpy arrays
color_image = np.asanyarray(color_frame.get_data())
cv2.imwrite('./color_sensor.jpg', color_image)
files = {
'compost':
('compost.jpg', open('./color_sensor.jpg', 'rb'))
}
data1 = [('machineid', 1)]
response_image = requests.post(SERVER_URL +
"/upload/compost",
files=files,
data=data1)
print(
f"Python res_image : {response_image.status_code}")
pipeline.stop()
picTime += D2_TIME
# Turning moter
# MUSHROOM DETECTION , requeset
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
# detections = detection(color_image)
# boxes = get_shiitake_location(detections['detection_boxes'], detections['detection_classes'],
# 0.5)
# print(boxes)
pipeline_check = False
pipeline.stop()
# for box in boxes:
# size = get_size(box)
# res = make_data(52, box, size)
# print(res)
else:
picTime += 50
except Exception:
self.errors.emit(100)
self.isRun = False
#####################################################
## Export to PLY ##
#####################################################
# First import the library
import pyrealsense2 as rs
# 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()
#Start streaming with default recommended configuration
pipe.start();
try:
# Wait for the next set of frames from the camera
frames = pipe.wait_for_frames()
# Fetch color and depth frames
depth = frames.get_depth_frame()
color = frames.get_color_frame()
# Tell pointcloud object to map to this color frame
pc.map_to(color)
# Generate the pointcloud and texture mappings
def start_pipeline(gui: bool):
save_path = "./out/cameraparams/"
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.rgb8, 30)
pipeline.start(config)
print('[INFO] Starting pipeline')
profile = pipeline.get_active_profile()
# sensor = profile.get_device().first_roi_sensor()
# roi = sensor.get_region_of_interest()
# sensor.set_region_of_interest(roi)
# profile = pipeline.start(config)
depth_profile = rs.video_stream_profile(profile.get_stream(
rs.stream.depth))
color_profile = rs.video_stream_profile(profile.get_stream(
rs.stream.color))
depth_intrinsics = depth_profile.get_intrinsics()
color_intrinsics = color_profile.get_intrinsics()
depth_extrinsics = depth_profile.get_extrinsics_to(color_profile)
color_extrinsics = color_profile.get_extrinsics_to(depth_profile)
print("- depth extrinsics:", depth_extrinsics)
print("- color extrinsics:", color_extrinsics)
color_profile = rs.video_stream_profile(profile.get_stream(
rs.stream.color))
color_intrinsics = color_profile.get_intrinsics()
color_extrinsics = color_profile.get_extrinsics_to(depth_profile)
# Identifying depth scaling factor
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
clipping_distance_in_meters = 1
clipping_distance = clipping_distance_in_meters / depth_scale
pc = rs.pointcloud()
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))
time.sleep(
1
) # Giving a second to auto exposrure, important when gui isn't used
while True:
frames = pipeline.wait_for_frames()
# depth_frame = frames.get_depth_frame()
non_aligned_color_frame = frames.get_color_frame()
# Aligning color and depth frames, aligned frames are
# getting re-sampled from the original one, so they are
# not exactly the same points.
aligned_frames = align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
depth_colormap = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
color_image = np.asanyarray(non_aligned_color_frame.get_data())
color_image_from_depth = np.asanyarray(color_frame.get_data())
pts = pc.calculate(depth_frame)
pc.map_to(color_frame)
if not depth_frame or not color_frame:
print("No depth or color frame, try again...")
continue
depth_image = np.asanyarray(depth_frame.get_data())
# np.savetxt("out/depth.csv", depth_image, delimiter=",")
if gui:
images = np.hstack(
(cv2.cvtColor(color_image,
cv2.COLOR_BGR2RGB), depth_colormap))
# cv2.cvtColor(color_image, cv2.COLOR_BGR2RGBA)
# cv2.imwrite(os.path.join(save_path, "{:d}.color.png".format(frame_counter)),
# cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR))
cv2.imshow(str(frame_counter), images)
key = cv2.waitKey(1)
if key == ord("d"):
cv2.destroyAllWindows()
print(
"[rs_pipeline] Running landmark detection on a color image..."
)
landmarks2d = landmark_detector(color_image)
landmarks3d = pixels_to_landmarks(color_intrinsics,
color_frame, depth_frame,
depth_scale)
print(
"[rs_pipeline] Computing point cloud mesh elements...")
vert, col, face = create_pc_mesh(
intrinsics=color_intrinsics,
points=pts,
color_frame=color_frame,
depth_frame=depth_frame,
depth_scale=depth_scale)
print(
"[INFO] Everything's ready, passing back to Thrift...")
return color_image, landmarks2d, landmarks3d, [
vert, col, face
]
if key == ord("q"):
break
else:
print(
"[rs_pipeline] Running landmark detection on a color image..."
)
landmarks2d = landmark_detector(color_image)
landmarks3d = pixels_to_landmarks(color_intrinsics,
color_frame, depth_frame,
depth_scale)
print("[rs_pipeline] Computing point cloud mesh elements...")
vert, col, face = create_pc_mesh(intrinsics=color_intrinsics,
points=pts,
color_frame=color_frame,
depth_frame=depth_frame,
depth_scale=depth_scale)
print("[INFO] Everything's ready, passing back to Thrift...")
return color_image, landmarks2d, landmarks3d, [vert, col, face]
except Exception as ex:
print(ex)
finally:
pipeline.stop()
