def capture_thread_func(svo_filepath=None):
global image_np_global, depth_np_global, exit_signal, new_data
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
input_type = sl.InputType()
if svo_filepath is not None:
input_type.set_from_svo_file(svo_filepath)
init_params = sl.InitParameters(input_t=input_type)
init_params.camera_resolution = sl.RESOLUTION.HD720
init_params.camera_fps = 30
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init_params.coordinate_units = sl.UNIT.METER
init_params.svo_real_time_mode = False
# Open the camera
err = zed.open(init_params)
print(err)
while err != sl.ERROR_CODE.SUCCESS:
err = zed.open(init_params)
print(err)
sleep(1)
image_mat = sl.Mat()
depth_mat = sl.Mat()
runtime_parameters = sl.RuntimeParameters()
image_size = sl.Resolution(width, height)
while not exit_signal:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(image_mat, sl.VIEW.LEFT, resolution=image_size)
zed.retrieve_measure(depth_mat,
sl.MEASURE.XYZRGBA,
resolution=image_size)
lock.acquire()
image_np_global = load_image_into_numpy_array(image_mat)
depth_np_global = load_depth_into_numpy_array(depth_mat)
new_data = True
lock.release()
sleep(0.01)
zed.close()
def print_text(self):
glDisable(GL_BLEND)
wnd_size = sl.Resolution()
wnd_size.width = glutGet(GLUT_WINDOW_WIDTH)
wnd_size.height = glutGet(GLUT_WINDOW_HEIGHT)
if len(self.objects_name) > 0:
for obj in self.objects_name:
pt2d = self.compute_3D_projection(obj.position,
self.projection, wnd_size)
glColor4f(obj.color[0], obj.color[1], obj.color[2],
obj.color[3])
glWindowPos2f(pt2d[0], pt2d[1])
for i in range(len(obj.name)):
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18,
ctypes.c_int(ord(obj.name[i])))
glEnable(GL_BLEND)
def OpenCamera():
print("grab Depth Sensing image... Press 'Esc' to quit")
init = sl.InitParameters(camera_resolution=sl.RESOLUTION.HD2K,
depth_mode=sl.DEPTH_MODE.ULTRA,
coordinate_units=sl.UNIT.METER,
coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP)
zed = sl.Camera()
status = zed.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
res = sl.Resolution()
res.width = 2208 # 720
res.height = 1242 # 404
point_cloud = sl.Mat(res.width, res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU)
return point_cloud, zed, res
def main():
# Create a ZED camera object
zed = sl.Camera()
# Set configuration parameters
init = sl.InitParameters()
init.camera_resolution = sl.RESOLUTION.HD720
init.depth_mode = sl.DEPTH_MODE.ULTRA
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
zed.close()
exit(1)
res = sl.Resolution()
res.width = 720
res.height = 404
point_cloud = sl.Mat(res.width, res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU)
camera_model = zed.get_camera_information().camera_model
# Create OpenGL viewer
viewer = gl.GLViewer()
viewer.init(len(sys.argv), sys.argv, camera_model, res)
while viewer.is_available():
if zed.grab() == sl.ERROR_CODE.SUCCESS:
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU,
res)
viewer.updateData(point_cloud)
viewer.exit()
zed.close()
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
# Enable object detection module
obj_param = sl.ObjectDetectionParameters()
obj_param.detection_model = sl.DETECTION_MODEL.MULTI_CLASS_BOX
# Defines if the object detection will track objects across images flow.
obj_param.enable_tracking = False # if True, enable positional tracking
if obj_param.enable_tracking:
zed.enable_positional_tracking()
zed.enable_object_detection(obj_param)
res = sl.Resolution()
res.width = 720
res.height = 404
camera_model = zed.get_camera_information().camera_model
# Create OpenGL viewer
viewer = gl.GLViewer()
viewer.init(camera_model, res)
# Configure object detection runtime parameters
obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
obj_runtime_param.detection_confidence_threshold = 50
point_cloud = sl.Mat(res.width, res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU)
objects = sl.Objects()
init_params.camera_resolution = sl.RESOLUTION.HD720
init_params.camera_fps = 30
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init_params.coordinate_units = sl.UNIT.METER
init_params.svo_real_time_mode = False
# Open the camera
status = zed.open(init_params)
print(status)
time.sleep(1)
left_image_mat = sl.Mat()
# right_image_mat = sl.Mat()
runtime_parameters = sl.RuntimeParameters()
image_size = sl.Resolution(width, height)
zed_image_num = 0
while not exit_signal:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(left_image_mat, sl.VIEW.LEFT, resolution=image_size)
# zed.retrieve_image(right_image_mat, sl.VIEW.RIGHT, resolution=image_size)
# print(left_image_mat.get_data())
left_image = load_image_into_numpy_array(left_image_mat)
# right_image = load_image_into_numpy_array(right_image_mat)
# print(left_image)
cv2.imshow("ZED-L", left_image)
# cv2.imshow("ZED-R", right_image)
# cv2.imshow("ZED-L", left_image_mat.get_data())
# cv2.imshow("ZED-R", right_image_mat.get_data())
obj_param.enable_body_fitting = True # Smooth skeleton move
obj_param.enable_tracking = True # Track people across images flow
obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST
# Enable Object Detection module
zed.enable_object_detection(obj_param)
obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
obj_runtime_param.detection_confidence_threshold = 40
# Get ZED camera information
camera_info = zed.get_camera_information()
# 2D viewer utilities
display_resolution = sl.Resolution(
min(camera_info.camera_resolution.width, 1280),
min(camera_info.camera_resolution.height, 720))
image_scale = [
display_resolution.width / camera_info.camera_resolution.width,
display_resolution.height / camera_info.camera_resolution.height
]
# Create OpenGL viewer
viewer = gl.GLViewer()
viewer.init(camera_info.calibration_parameters.left_cam,
obj_param.enable_tracking)
# Create ZED objects filled in the main loop
bodies = sl.Objects()
image = sl.Mat()
def main(argv):
thresh = 0.25
darknet_path = "/home/apsync/GitHub/darknet/"
config_path = darknet_path + "cfg/yolov4-tiny.cfg"
weight_path = darknet_path + "yolov4-tiny.weights"
meta_path = darknet_path + "cfg/coco.data"
svo_path = None
zed_id = 0
help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -z <zed_id>'
try:
opts, args = getopt.getopt(argv, "hc:w:m:t:s:z:", [
"config=", "weight=", "meta=", "threshold=", "svo_file=", "zed_id="
])
except getopt.GetoptError:
log.exception(help_str)
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
log.info(help_str)
sys.exit()
elif opt in ("-c", "--config"):
config_path = arg
elif opt in ("-w", "--weight"):
weight_path = arg
elif opt in ("-m", "--meta"):
meta_path = arg
elif opt in ("-t", "--threshold"):
thresh = float(arg)
elif opt in ("-s", "--svo_file"):
svo_path = arg
elif opt in ("-z", "--zed_id"):
zed_id = int(arg)
input_type = sl.InputType()
if svo_path is not None:
log.info("SVO file : " + svo_path)
input_type.set_from_svo_file(svo_path)
else:
# Launch camera by id
input_type.set_from_camera_id(zed_id)
init = sl.InitParameters(input_t=input_type)
init.coordinate_units = sl.UNIT.METER
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use ULTRA depth mode
# Start the ROS nodes for the images and the depth information
rospy.init_node('darknet_ros')
rospy.loginfo('darknet yolo node started')
pub = rospy.Publisher('/darknet_ros/detection_image', Image, queue_size=10)
pub2 = rospy.Publisher('/darknet_ros/distance_array',
LaserScan,
queue_size=50)
pub3 = rospy.Publisher('/darknet_ros/color_image', Image, queue_size=10)
pub4 = rospy.Publisher('/darknet_ros/nine_sector_image',
Image,
queue_size=10)
pub5 = rospy.Publisher('/darknet_ros/9sectorarray',
PointCloud,
queue_size=50)
cam = sl.Camera()
if not cam.is_opened():
log.info("Opening ZED Camera...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
log.error(repr(status))
exit()
runtime = sl.RuntimeParameters()
# Use STANDARD sensing mode
runtime.sensing_mode = sl.SENSING_MODE.FILL
mat = sl.Mat()
mat_lv = sl.Mat()
point_cloud_mat = sl.Mat()
mat_9_sec = sl.Mat()
# Import the global variables. This lets us instance Darknet once,
# then just call performDetect() again without instancing again
global metaMain, netMain, altNames # pylint: disable=W0603
assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)"
if not os.path.exists(config_path):
raise ValueError("Invalid config path `" +
os.path.abspath(config_path) + "`")
if not os.path.exists(weight_path):
raise ValueError("Invalid weight path `" +
os.path.abspath(weight_path) + "`")
if not os.path.exists(meta_path):
raise ValueError("Invalid data file path `" +
os.path.abspath(meta_path) + "`")
if netMain is None:
netMain = load_net_custom(config_path.encode("ascii"),
weight_path.encode("ascii"), 0,
1) # batch size = 1
if metaMain is None:
metaMain = load_meta(meta_path.encode("ascii"))
if altNames is None:
# In thon 3, the metafile default access craps out on Windows (but not Linux)
# Read the names file and create a list to feed to detect
try:
with open(meta_path) as meta_fh:
meta_contents = meta_fh.read()
import re
match = re.search("names *= *(.*)$", meta_contents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as names_fh:
names_list = names_fh.read().strip().split("\n")
altNames = [x.strip() for x in names_list]
except TypeError:
pass
except Exception:
pass
color_array = generate_color(meta_path)
# get parameters for depth sensing
width = round(cam.get_camera_information().camera_resolution.width / 2)
height = round(cam.get_camera_information().camera_resolution.height / 2)
res = sl.Resolution()
res.width = width
res.height = height
angle_offset = 0 - (depth_hfov_deg / 2)
increment_f = depth_hfov_deg / (distances_array_length)
#Initialize laserscan node
scan = LaserScan()
scan.header.frame_id = 'zed_horizontal_scan'
scan.angle_min = angle_offset
scan.angle_max = depth_hfov_deg / 2
scan.angle_increment = increment_f
scan.range_min = DEPTH_RANGE_M[0]
scan.range_max = DEPTH_RANGE_M[1]
scan.intensities = []
scan.time_increment = 0
fps = 1
#Initialize pointcloud node
channel = ChannelFloat32()
channel.name = "depth_range"
channel.values = [DEPTH_RANGE_M[0], DEPTH_RANGE_M[1]]
pointcloud = PointCloud()
pointcloud.header.frame_id = 'zed_9_sector_scan'
pointcloud.channels = [channel]
while not rospy.is_shutdown():
start_time = time.time() # start time of the loop
current_time = rospy.Time.now()
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat, sl.VIEW.LEFT)
image = mat.get_data()
cam.retrieve_image(mat_lv, sl.VIEW.LEFT)
image_lv = mat_lv.get_data()
cam.retrieve_image(mat_9_sec, sl.VIEW.DEPTH)
nine_sector_image = mat_9_sec.get_data()
scan.header.stamp = current_time
pointcloud.header.stamp = current_time
scan.scan_time = fps
cam.retrieve_measure(point_cloud_mat, sl.MEASURE.XYZRGBA)
depth = point_cloud_mat.get_data()
print("\033[0;0H")
distances_from_depth_image(point_cloud_mat, distances,
DEPTH_RANGE_M[0], DEPTH_RANGE_M[1],
width, height,
obstacle_line_thickness_pixel)
scan.ranges = distances
distance_center = np.mean(distances[34:38])
# Publish the distance information for the mavros node
pub2.publish(scan)
# provide distance info to video stream and create image with horizontal distance information
# Draw a horizontal line to visualize the obstacles' line
x1, y1 = int(0), int(height)
x2, y2 = int(width * 2), int(height)
line_thickness = obstacle_line_thickness_pixel
cv2.line(image_lv, (x1, y1), (x2, y2), (0, 255, 0),
thickness=line_thickness)
#print("Distance to Camera at position {},{} (image center): {:1.3} m".format(width, height, distance_center))
if distance_center > DEPTH_RANGE_M[1]:
cv2.circle(image_lv, (width, height), 20, (0, 255, 0), 2)
elif distance_center <= DEPTH_RANGE_M[1] and distance_center > 10:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 50), (height + 50)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
cv2.circle(image_lv, (width, height), 20, (0, 255, 0), 2)
elif distance_center > 5 and distance_center <= 10:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 70), (height + 65)),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 255), 2)
cv2.circle(image_lv, (width, height), 20, (0, 255, 255), 4)
else:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 100), (height + 70)),
cv2.FONT_HERSHEY_DUPLEX, 2, (0, 0, 255), 2)
cv2.circle(image_lv, (width, height), 20, (0, 0, 255), -1)
cv2.rectangle(image_lv, (0, 100),
((width * 2 - 5), (height * 2)), (30, 30, 255),
3)
# create 9 sector image with distance information
# divide view into 3x3 matrix
box = np.empty(4, dtype=int)
pxstep = int(width * 2 / 3)
pystep = int(height * 2 / 3)
gx = 0
gy = 0
# draw sector lines on image
while gx < (width * 2):
cv2.line(nine_sector_image, (gx, 0), (gx, (height * 2)),
color=(0, 0, 255),
thickness=2)
gx += pxstep
while gy <= (height * 2):
cv2.line(nine_sector_image, (0, gy), ((width * 2), gy),
color=(0, 0, 255),
thickness=2)
gy += pystep
# measure sector depth and printout in sectors
gx = 0
gy = 0
i = 0
box[1] = pystep / 2 + gy
box[2] = pxstep
box[3] = pystep
while gx < (width * 2 - 2):
gy = 0
box[1] = pystep / 2 + gy
box[0] = pxstep / 2 + gx
# calculate depth of closest object in sector
x, y, z = get_object_depth(depth, box)
sector_obstacle_coordinates[i][0] = x
sector_obstacle_coordinates[i][1] = y
sector_obstacle_coordinates[i][2] = z
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.putText(nine_sector_image,
"Dist.: " + (str(distance) + " m"),
((gx + 10), (gy + pystep - 10)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
gy += pystep
i += 1
while gy < (height * 2):
box[1] = pystep / 2 + gy
# calculate depth of closest object in sector
x, y, z = get_object_depth(depth, box)
sector_obstacle_coordinates[i][0] = x
sector_obstacle_coordinates[i][1] = y
sector_obstacle_coordinates[i][2] = z
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.putText(nine_sector_image,
"Dist.: " + (str(distance) + " m"),
((gx + 10), (gy + pystep - 10)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
gy += pystep
i += 1
gx += pxstep
pointcloud.points = [
Point32(x=sector_obstacle_coordinates[j][0],
y=sector_obstacle_coordinates[j][1],
z=sector_obstacle_coordinates[j][2]) for j in range(9)
]
pub5.publish(pointcloud)
# Do the yolo object detection
detections = detect(netMain, metaMain, image, thresh)
print(
chr(27) + "[2J" + "**** " + str(len(detections)) +
" Detection Results ****")
for detection in detections:
label = detection[0]
confidence = detection[1]
pstring = label + ": " + str(np.rint(100 * confidence)) + "%"
print(pstring)
bounds = detection[2]
y_extent = int(bounds[3])
x_extent = int(bounds[2])
# Coordinates are around the center
x_coord = int(bounds[0] - bounds[2] / 2)
y_coord = int(bounds[1] - bounds[3] / 2)
thickness = 1
x, y, z = get_object_depth(depth, bounds)
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.rectangle(image,
(x_coord - thickness, y_coord - thickness),
(x_coord + x_extent + thickness, y_coord +
(18 + thickness * 4)),
color_array[detection[3]], -1)
cv2.putText(image, pstring + " " + (str(distance) + " m"),
(x_coord + (thickness * 4), y_coord +
(10 + thickness * 4)), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (255, 255, 255), 2)
cv2.rectangle(image,
(x_coord - thickness, y_coord - thickness),
(x_coord + x_extent + thickness,
y_coord + y_extent + thickness),
color_array[detection[3]], int(thickness * 2))
# convert image to ros
imgMsg = bridge.cv2_to_imgmsg(image, encoding="bgra8")
imgMsg.header.stamp = rospy.Time.now()
imgMsg.header.frame_id = "color_image"
imgMsg_lv = bridge.cv2_to_imgmsg(image_lv, encoding="bgra8")
imgMsg_lv.header.stamp = rospy.Time.now()
imgMsg_lv.header.frame_id = "yolo_image"
imgMsg_9sec = bridge.cv2_to_imgmsg(nine_sector_image,
encoding="bgra8")
imgMsg_9sec.header.stamp = rospy.Time.now()
imgMsg_9sec.header.frame_id = "nine_sector_image"
# publish images to ros nodes
pub.publish(imgMsg)
pub3.publish(imgMsg_lv)
pub4.publish(imgMsg_9sec)
fps = (time.time() - start_time)
print("\033[0;0H" + "FPS: {:1.3}".format(1.0 / fps))
#rospy.Rate(1.0).sleep() # 1 Hz
#cv2.destroyAllWindows()
cam.close()
log.info("\nFINISH")
def main():
parser = argparse.ArgumentParser(
description="PyTorch Object Detection Webcam Demo")
parser.add_argument(
"--config-file",
default="configs/caffe2/e2e_mask_rcnn_X_101_32x8d_FPN_1x_caffe2.yaml",
metavar="FILE",
help="path to config file",
)
parser.add_argument(
"--confidence-threshold",
type=float,
default=0.6,
help="Minimum score for the prediction to be shown",
)
parser.add_argument(
"--min-image-size",
type=int,
default=256,
help="Smallest size of the image to feed to the model. "
"Model was trained with 800, which gives best results",
)
parser.add_argument(
"--show-mask-heatmaps",
dest="show_mask_heatmaps",
help="Show a heatmap probability for the top masks-per-dim masks",
action="store_true",
)
parser.add_argument(
"--masks-per-dim",
type=int,
default=2,
help="Number of heatmaps per dimension to show",
)
parser.add_argument(
"opts",
help="Modify model config options using the command-line",
default=None,
nargs=argparse.REMAINDER,
)
parser.add_argument("--svo-filename",
help="Optional SVO input filepath",
default=None)
args = parser.parse_args()
# load config from file and command-line arguments
cfg.merge_from_file(args.config_file)
cfg.merge_from_list(args.opts)
cfg.freeze()
# prepare object that handles inference plus adds predictions on top of image
coco_demo = COCODemo(
cfg,
confidence_threshold=args.confidence_threshold,
show_mask_heatmaps=args.show_mask_heatmaps,
masks_per_dim=args.masks_per_dim,
min_image_size=args.min_image_size,
)
init_cap_params = sl.InitParameters()
if args.svo_filename:
print("Loading SVO file " + args.svo_filename)
init_cap_params.set_from_svo_file(args.svo_filename)
init_cap_params.svo_real_time_mode = True
init_cap_params.camera_resolution = sl.RESOLUTION.HD720
init_cap_params.depth_mode = sl.DEPTH_MODE.ULTRA
init_cap_params.coordinate_units = sl.UNIT.METER
init_cap_params.depth_stabilization = True
init_cap_params.camera_image_flip = sl.FLIP_MODE.AUTO
init_cap_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
cap = sl.Camera()
if not cap.is_opened():
print("Opening ZED Camera...")
status = cap.open(init_cap_params)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
display = True
runtime = sl.RuntimeParameters()
left = sl.Mat()
ptcloud = sl.Mat()
depth_img = sl.Mat()
depth = sl.Mat()
res = sl.Resolution(1280, 720)
py_transform = sl.Transform(
) # First create a Transform object for TrackingParameters object
tracking_parameters = sl.PositionalTrackingParameters(
init_pos=py_transform)
tracking_parameters.set_as_static = True
err = cap.enable_positional_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
running = True
keep_people_only = True
if coco_demo.cfg.MODEL.MASK_ON:
print("Mask enabled!")
if coco_demo.cfg.MODEL.KEYPOINT_ON:
print("Keypoints enabled!")
while running:
start_time = time.time()
err_code = cap.grab(runtime)
if err_code != sl.ERROR_CODE.SUCCESS:
break
cap.retrieve_image(left, sl.VIEW.LEFT, resolution=res)
cap.retrieve_image(depth_img, sl.VIEW.DEPTH, resolution=res)
cap.retrieve_measure(depth, sl.MEASURE.DEPTH, resolution=res)
cap.retrieve_measure(ptcloud, sl.MEASURE.XYZ, resolution=res)
ptcloud_np = np.array(ptcloud.get_data())
img = cv2.cvtColor(left.get_data(), cv2.COLOR_RGBA2RGB)
prediction = coco_demo.select_top_predictions(
coco_demo.compute_prediction(img))
# Keep people only
if keep_people_only:
labels_tmp = prediction.get_field("labels")
people_coco_label = 1
keep = torch.nonzero(labels_tmp == people_coco_label).squeeze(1)
prediction = prediction[keep]
composite = img.copy()
humans_3d = None
masks_3d = None
if coco_demo.show_mask_heatmaps:
composite = coco_demo.create_mask_montage(composite, prediction)
composite = coco_demo.overlay_boxes(composite, prediction)
if coco_demo.cfg.MODEL.MASK_ON:
masks_3d = get_masks3d(prediction, depth)
composite = coco_demo.overlay_mask(composite, prediction)
if coco_demo.cfg.MODEL.KEYPOINT_ON:
# Extract 3D skeleton from the ZED depth
humans_3d = get_humans3d(prediction, ptcloud_np)
composite = coco_demo.overlay_keypoints(composite, prediction)
if True:
overlay_distances(prediction, get_boxes3d(prediction, ptcloud_np),
composite, humans_3d, masks_3d)
composite = coco_demo.overlay_class_names(composite, prediction)
print(" Time: {:.2f} s".format(time.time() - start_time))
if display:
cv2.imshow("COCO detections", composite)
cv2.imshow("ZED Depth", depth_img.get_data())
key = cv2.waitKey(10)
if key == 27:
break # esc to quit
def predrel(argv):
##################################3# yolo detection weight
thresh = 0.25
darknet_path = "/home/lx/DRNet/detection_yolo/libdarknet/"
# config_path = darknet_path + "cfg/yolov3.cfg"
config_path = darknet_path + "cfg/yolov3-voc.cfg"
# weight_path = "yolov3.weights"
weight_path = "/home/lx/DRNet/datasets/data_yolo/backup/yolov3-voc_1300.weights"
#weight_path = "yolov4.weights"
meta_path = darknet_path + "cfg/yolo.data"
# meta_path = "coco.data"
##################################3# spatial relation detection weight
# checkpoint_path = "/home/lx/DRNet/SpatialSense/baselines/runs/drnet/checkpoints/model_datasetReSe_29.pth"
checkpoint_path = "/home/lx/DRNet/SpatialSense/baselines/runs/customon_depthon_apprfeaton_spatialdualmask/checkpoints/model_96.970_28.pth"
# args = parse_args()
svo_path = None
zed_id = 0
# help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -z <zed_id>'
# try:
# opts, args = getopt.getopt(
# argv, "hc:w:m:t:s:z:", ["config=", "weight=", "meta=", "threshold=", "svo_file=", "zed_id="])
# except getopt.GetoptError:
# log.exception(help_str)
# sys.exit(2)
# for opt, arg in opts:
# if opt == '-h':
# log.info(help_str)
# sys.exit()
# elif opt in ("-c", "--config"):
# config_path = arg
# elif opt in ("-w", "--weight"):
# weight_path = arg
# elif opt in ("-m", "--meta"):
# meta_path = arg
# elif opt in ("-t", "--threshold"):
# thresh = float(arg)
# elif opt in ("-s", "--svo_file"):
# svo_path = arg
# elif opt in ("-z", "--zed_id"):
# zed_id = int(arg)
args = parse_args()
input_type = sl.InputType()
if svo_path is not None:
log.info("SVO file : " + svo_path)
input_type.set_from_svo_file(svo_path)
else:
# Launch camera by id
input_type.set_from_camera_id(zed_id)
init = sl.InitParameters(input_t=input_type)
init.coordinate_units = sl.UNIT.METER
cam = sl.Camera()
if not cam.is_opened():
log.info("Opening ZED Camera...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
log.error(repr(status))
exit()
runtime = sl.RuntimeParameters()
# Use STANDARD sensing mode
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
mat = sl.Mat()
depth_img = sl.Mat()
point_cloud_mat = sl.Mat()
resl = sl.Resolution(640, 480)
# Import the global variables. This lets us instance Darknet once,
# then just call performDetect() again without instancing again
global metaMain, netMain, altNames # pylint: disable=W0603
assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)"
if not os.path.exists(config_path):
raise ValueError("Invalid config path `" +
os.path.abspath(config_path) + "`")
if not os.path.exists(weight_path):
raise ValueError("Invalid weight path `" +
os.path.abspath(weight_path) + "`")
if not os.path.exists(meta_path):
raise ValueError("Invalid data file path `" +
os.path.abspath(meta_path) + "`")
if netMain is None:
netMain = load_net_custom(config_path.encode("ascii"),
weight_path.encode("ascii"), 0,
1) # batch size = 1
if metaMain is None:
metaMain = load_meta(meta_path.encode("ascii"))
if altNames is None:
# In thon 3, the metafile default access craps out on Windows (but not Linux)
# Read the names file and create a list to feed to detect
try:
with open(meta_path) as meta_fh:
meta_contents = meta_fh.read()
import re
match = re.search("names *= *(.*)$", meta_contents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as names_fh:
names_list = names_fh.read().strip().split("\n")
altNames = [x.strip() for x in names_list]
except TypeError:
pass
except Exception:
pass
###load relation detection weight
phrase_encoder = RecurrentPhraseEncoder(300, 300)
checkpoint = torch.load(checkpoint_path)
model = DRNet_depth(phrase_encoder, 512, 3, args)
model.cuda()
model.eval()
model.load_state_dict(checkpoint['state_dict'])
model.cuda()
color_array = generate_color(meta_path)
log.info("Running...")
key = ''
mark2 = 0
while key != 113: # for 'q' key
start_time = time.time() # start time of the loop
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat, sl.VIEW.LEFT, resolution=resl)
image = mat.get_data()
# print(image)
ih, iw = image.shape[:2]
# cam.retrieve_image(depth_img, sl.VIEW.DEPTH, resolution=resl)
cam.retrieve_measure(point_cloud_mat,
sl.MEASURE.XYZ,
resolution=resl)
depth = point_cloud_mat.get_data()
# Do the detection
detections = detect(
netMain, metaMain, image, depth, thresh
) #####object label , confidence , boundingbox , i , depthdata
time1 = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S')
cv2.imwrite(
'/home/lx/DRNet/experiment/qualitative_evaluation/10_28_raw/image_nobbox'
+ time1 + '_' + str(mark2) + '.jpg', image)
# print(detections)
# log.info(chr(27) + "[2J"+"**** " + str(len(detections)) + " Results ****")
global relprelist
relprelist = []
relprelist1 = []
########### bits after image compression
# global byteimage
# img_encode = cv2.imencode('.jpg', image, [cv2.IMWRITE_JPEG_QUALITY, 99])[1]
# byteimage = img_encode.tostring()
# print (len(byteimage))
########### bits after image compression
detectnum = len(detections)
#### batchsize data input
mark = 0
sub_obj_list = []
for idx in range(0, detectnum):
label_sub = detections[idx][0]
sub1 = phrase2vec(label_sub, 2, 300)
sub = torch.unsqueeze(torch.Tensor(np.array(sub1, np.float32)),
dim=0)
bounds_sub = detections[idx][2]
box3d_sub1 = detections[idx][4]
box3d_sub = torch.unsqueeze(torch.Tensor(box3d_sub1), dim=0)
subbbox = getBBox(bounds_sub)
for idx1 in range(idx + 1, detectnum):
label_obj = detections[idx1][0]
obj1 = phrase2vec(label_obj, 2, 300)
obj = torch.unsqueeze(torch.Tensor(
np.array(obj1, np.float32)),
dim=0)
bounds_obj = detections[idx1][2]
box3d_obj1 = detections[idx1][4]
box3d_obj = torch.unsqueeze(torch.Tensor(box3d_obj1),
dim=0)
objbbox = getBBox(bounds_obj)
bbox_mask = spatial_fea(subbbox, objbbox, ih,
iw) ###bbox空间特征
bbox_img = img_fea(subbbox, objbbox, ih, iw, 1.25,
image) ###bbox图像特征
if mark < 1:
label_allsub = sub
label_allobj = obj
box3_allsub = box3d_sub
box3_allobj = box3d_obj
mask_allbbox = bbox_mask
img_allbbox = bbox_img
else:
label_allsub = torch.cat((label_allsub, sub), dim=0)
label_allobj = torch.cat((label_allobj, obj), dim=0)
img_allbbox = torch.cat((img_allbbox, bbox_img), dim=0)
mask_allbbox = torch.cat((mask_allbbox, bbox_mask),
dim=0)
box3_allsub = torch.cat((box3_allsub, box3d_sub),
dim=0)
box3_allobj = torch.cat((box3_allobj, box3d_obj),
dim=0)
sub_obj_list.append([label_sub, label_obj])
mark += 1
thickness = 1
cv2.rectangle(image,
(subbbox[2] - thickness, subbbox[0] - thickness),
(subbbox[3] + thickness, subbbox[1] + thickness),
color_array[detections[idx][3]],
int(thickness * 2))
fh = open(
'/home/lx/DRNet/experiment/qualitative_evaluation/10_28_raw/rel'
+ time1 + '_' + str(mark2) + '.txt',
'w',
encoding='utf-8')
fh1 = open(
'/home/lx/DRNet/experiment/qualitative_evaluation/10_28_num1/rel'
+ time1 + '_' + str(mark2) + '.txt',
'w',
encoding='utf-8')
if mark > 0:
relprelabel = model(label_allsub.cuda(), label_allobj.cuda(),
img_allbbox.cuda(), mask_allbbox.cuda(), 1,
box3_allsub.cuda(), box3_allobj.cuda(),
args)
for idxx, label in enumerate(relprelabel):
rellabel_t = label.argmax()
sub_t = object_categories.index(sub_obj_list[idxx][0])
obj_t = object_categories.index(sub_obj_list[idxx][1])
relpair_t = [sub_t, rellabel_t.item(), obj_t]
rellabel = predicate_categories[rellabel_t]
relpair = [
sub_obj_list[idxx][0], rellabel, sub_obj_list[idxx][1]
]
fh.write(str(relpair) + '\r\n')
if (not relpair_t in relprelist1):
fh1.write(
str(relpair_t).replace('[', '').replace(
']', '').replace(',', ' ') + '\r\n')
relprelist1.append(relpair_t)
relprelist = relprelist1
cv2.imshow("ZED", image)
cv2.imwrite(
'/home/lx/DRNet/experiment/qualitative_evaluation/10_28_raw/image'
+ time1 + '_' + str(mark2) + '.jpg', image)
fh.close()
fh1.close()
key = cv2.waitKey(5)
mark2 += 1
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
cam.close()
log.info("\nFINISH")
batch_handler = BatchSystemHandler(batch_parameters.latency * 2)
else:
batch_parameters.enable = False
obj_param = sl.ObjectDetectionParameters(
batch_trajectories_parameters=batch_parameters)
obj_param.detection_model = sl.DETECTION_MODEL.MULTI_CLASS_BOX
# Defines if the object detection will track objects across images flow.
obj_param.enable_tracking = True
zed.enable_object_detection(obj_param)
camera_infos = zed.get_camera_information()
# Create OpenGL viewer
viewer = gl.GLViewer()
point_cloud_res = sl.Resolution(
min(camera_infos.camera_resolution.width, 720),
min(camera_infos.camera_resolution.height, 404))
point_cloud_render = sl.Mat()
viewer.init(camera_infos.camera_model, point_cloud_res,
obj_param.enable_tracking)
# Configure object detection runtime parameters
obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
detection_confidence = 60
obj_runtime_param.detection_confidence_threshold = detection_confidence
# To select a set of specific object classes
obj_runtime_param.object_class_filter = [sl.OBJECT_CLASS.PERSON]
# To set a specific threshold
obj_runtime_param.object_class_detection_confidence_threshold = {
sl.OBJECT_CLASS.PERSON: detection_confidence
}
