def main():
# Create a Camera object
zed = sl.Camera()
pcd = PointCloud()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER # Use milliliter units (for depth measurements)
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
while 1:
# A new image is available if grab() returns SUCCESS
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
zed.retrieve_image(image, sl.VIEW.VIEW_LEFT)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA)
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = round(image.get_width() / 2)
y = round(image.get_height() / 2)
err, point_cloud_value = point_cloud.get_value(x, y)
numPoints = 1280*720*3
numColors = 1280*720*1
points = np.arange(numPoints).reshape((-1, 3))
colors = np.arange(numColors).reshape((-1, 1))
begin = datetime.datetime.now()
my = point_cloud.get_data()
aa = my.flatten()
aa.resize((numColors,4))
aa = np.array(aa)
filPoints = aa[~np.isnan(aa).any(axis=1)]
filPoints = filPoints[::15,:]
points = filPoints[:,0:3].reshape(-1,3)
colors = filPoints[:,3].reshape(-1,1)
print(colors.shape)
print(points.shape)
# filPoints = points[~np.isnan(points).any(axis=1)]
pcd.points = Vector3dVector(points)
pcd.paint_uniform_color([0,1,0])
for i in range(len(colors)):
A, R, G, B = float2decimal(colors[i][0])
print("values of BGR: ", B,G,R)
pcd.colors[i] = [B/255, G/255, R/255]
end = datetime.datetime.now()
k = end - begin
draw_geometries([pcd])
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] +
point_cloud_value[1] * point_cloud_value[1] +
point_cloud_value[2] * point_cloud_value[2])
if not np.isnan(distance) and not np.isinf(distance):
distance = round(distance)
print("Distance to Camera at ({0}, {1}): {2} mm\n".format(x, y, distance))
# Increment the loop
# i = i + 1
else:
print("Can't estimate distance at this position, move the camera\n")
sys.stdout.flush()
# Close the camera
zed.close()
def main(argv):
thresh = 0.25
darknet_path="../libdarknet/"
config_path = darknet_path + "cfg/yolov3-tiny.cfg"
weight_path = "yolov3-tiny.weights"
meta_path = "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
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()
point_cloud_mat = sl.Mat()
# Prepare new image size to retrieve half-resolution images
image_size = cam.get_camera_information().camera_resolution
image_size.width = image_size.width
image_size.height = image_size.height
depth_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.F32_C1)
# Create an RGBA sl.Mat object
depth_zed_show = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
# 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)
log.info("Running...")
key = ''
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)
image = mat.get_data()
cam.retrieve_measure(
point_cloud_mat, sl.MEASURE.XYZRGBA)
cam.retrieve_measure(depth_zed, sl.MEASURE.DEPTH)
depth = point_cloud_mat.get_data()
depth_ocv = depth_zed.get_data()
# Retrieve the normalized depth image
cam.retrieve_image(depth_zed_show, sl.VIEW.DEPTH)
# Use get_data() to get the numpy array
depth_zed_show_num = depth_zed_show.get_data()
# Do the detection
detections = detect(netMain, metaMain, image, thresh)
grey_color = 153
log.info(chr(27) + "[2J"+"**** " + str(len(detections)) + " Results ****")
for detection in detections:
label = detection[0]
confidence = detection[1]
pstring = label+": "+str(np.rint(100 * confidence))+"%"
log.info(pstring)
bounds = detection[2]
y_extent = int(bounds[3])
x_extent = int(bounds[2])
# Coordinates are around the sidecorner
x_coord = int(bounds[0] - x_extent/2)
y_coord = int(bounds[1] - y_extent/2)
#boundingBox = [[x_coord, y_coord], [x_coord, y_coord + y_extent], [x_coord + x_extent, y_coord + y_extent], [x_coord + x_extent, y_coord]]
bounds2 = [x_coord,y_coord]
thickness = 1
x, y, z = get_object_depth(depth, bounds)
print(bounds)
print(bounds2)
bounds3 = [x_coord + x_extent,y_coord]
image_size = cam.get_camera_information().camera_resolution
width = image_size.width
height = image_size.height
#print(width)
#print(height)
#１ピクセル計算
distance = math.sqrt(x * x + y * y + z * z)
pixel_realsize_x = distance / 1280 * 1.92
pixel_realsize_y = distance / 720 * 1.06
distance_x = (x_extent) * pixel_realsize_x
distance_y = (y_extent) * pixel_realsize_y
#ROI
roi = image[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent]
roi_depth = depth_ocv[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent]
#print(roi_depth.shape)
roi_depth_show = depth_zed_show_num[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent]
#print(roi_depth_show.shape)
roi_depth_show_gray = cv2.cvtColor(roi_depth_show, cv2.COLOR_RGB2GRAY)
#depth_image_3d = np.dstack((roi_depth,roi_depth,roi_depth,roi_depth))
#bg_removed = np.where((depth_image_3d > z) | (depth_image_3d <= 0), grey_color, roi)
#深度データを与えると2値を返す
# 方法2 （OpenCVで実装）
ret, th = cv2.threshold(roi_depth_show_gray, 0, 255, cv2.THRESH_OTSU)
print("ret{}".format(ret))
depth_image_3d = np.dstack((th,th,th,th))
# th = threshold_otsu(roi_depth_show_gray)
contours, hierarchy = cv2.findContours(th, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)
for i in range(0, len(contours)):
if len(contours[i]) > 0:
# remove small objects
if cv2.contourArea(contours[i]) < 800:
continue
cv2.polylines(roi, contours[i], True, (255, 255, 255), 5)
image[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent] = depth_image_3d
##表示
distance_string = "v:{1:.2f},h{0:.2f}".format(distance_x,distance_y)
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, label + " " + (str(distance_string) + " 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))
cv2.imshow("ZED", image)
key = cv2.waitKey(5)
log.info("FPS: {}".format(1.0 / (time.time() - start_time)))
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
cam.close()
log.info("\nFINISH")
def main():
if not sys.argv or len(sys.argv) != 4:
sys.stdout.write("Usage: \n\n")
sys.stdout.write(" ZED_SVO_Export A B C \n\n")
sys.stdout.write(
"Please use the following parameters from the command line:\n")
sys.stdout.write(" A - SVO file path (input) : \"path/to/file.svo\"\n")
sys.stdout.write(
" B - AVI file path (output) or image sequence folder(output) :\n")
sys.stdout.write(
" \"path/to/output/file.avi\" or \"path/to/output/folder\"\n"
)
sys.stdout.write(" C - Export mode: 0=Export LEFT+RIGHT AVI.\n")
sys.stdout.write(" 1=Export LEFT+DEPTH_VIEW AVI.\n")
sys.stdout.write(
" 2=Export LEFT+RIGHT image sequence.\n")
sys.stdout.write(
" 3=Export LEFT+DEPTH_VIEW image sequence.\n")
sys.stdout.write(
" 4=Export LEFT+DEPTH_16Bit image sequence.\n")
sys.stdout.write(" A and B need to end with '/' or '\\'\n\n")
sys.stdout.write("Examples: \n")
sys.stdout.write(
" (AVI LEFT+RIGHT): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/file.avi\" 0\n"
)
sys.stdout.write(
" (AVI LEFT+DEPTH): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/file.avi\" 1\n"
)
sys.stdout.write(
" (SEQUENCE LEFT+RIGHT): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\" 2\n"
)
sys.stdout.write(
" (SEQUENCE LEFT+DEPTH): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\" 3\n"
)
sys.stdout.write(
" (SEQUENCE LEFT+DEPTH_16Bit): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\""
" 4\n")
exit()
# Get input parameters
svo_input_path = Path(sys.argv[1])
output_path = Path(sys.argv[2])
output_as_video = True
app_type = AppType.LEFT_AND_RIGHT
if sys.argv[3] == "1" or sys.argv[3] == "3":
app_type = AppType.LEFT_AND_DEPTH
if sys.argv[3] == "4":
app_type = AppType.LEFT_AND_DEPTH_16
# Check if exporting to AVI or SEQUENCE
if sys.argv[3] != "0" and sys.argv[3] != "1":
output_as_video = False
if not output_as_video and not output_path.is_dir():
sys.stdout.write(
"Input directory doesn't exist. Check permissions or create it.\n",
output_path, "\n")
exit()
# Specify SVO path parameter
init_params = sl.InitParameters()
init_params.set_from_svo_file(str(svo_input_path))
init_params.svo_real_time_mode = False # Don't convert in realtime
init_params.coordinate_units = sl.UNIT.MILLIMETER # Use milliliter units (for depth measurements)
# Create ZED objects
zed = sl.Camera()
# Open the SVO file specified as a parameter
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
sys.stdout.write(repr(err))
zed.close()
exit()
# Get image size
image_size = zed.get_camera_information().camera_resolution
width = image_size.width
height = image_size.height
width_sbs = width * 2
# Prepare side by side image container equivalent to CV_8UC4
svo_image_sbs_rgba = np.zeros((height, width_sbs, 4), dtype=np.uint8)
# Prepare single image containers
left_image = sl.Mat()
right_image = sl.Mat()
depth_image = sl.Mat()
video_writer = None
if output_as_video:
# Create video writer with MPEG-4 part 2 codec
video_writer = cv2.VideoWriter(
str(output_path), cv2.VideoWriter_fourcc('M', '4', 'S', '2'),
max(zed.get_camera_information().camera_fps, 25),
(width_sbs, height))
if not video_writer.isOpened():
sys.stdout.write(
"OpenCV video writer cannot be opened. Please check the .avi file path and write "
"permissions.\n")
zed.close()
exit()
rt_param = sl.RuntimeParameters()
rt_param.sensing_mode = sl.SENSING_MODE.FILL
# Start SVO conversion to AVI/SEQUENCE
sys.stdout.write("Converting SVO... Use Ctrl-C to interrupt conversion.\n")
nb_frames = zed.get_svo_number_of_frames()
while True:
if zed.grab(rt_param) == sl.ERROR_CODE.SUCCESS:
svo_position = zed.get_svo_position()
# Retrieve SVO images
zed.retrieve_image(left_image, sl.VIEW.LEFT)
if app_type == AppType.LEFT_AND_RIGHT:
zed.retrieve_image(right_image, sl.VIEW.RIGHT)
elif app_type == AppType.LEFT_AND_DEPTH:
zed.retrieve_image(right_image, sl.VIEW.DEPTH)
elif app_type == AppType.LEFT_AND_DEPTH_16:
zed.retrieve_measure(depth_image, sl.MEASURE.DEPTH)
if output_as_video:
# Copy the left image to the left side of SBS image
svo_image_sbs_rgba[0:height,
0:width, :] = left_image.get_data()
# Copy the right image to the right side of SBS image
svo_image_sbs_rgba[0:, width:, :] = right_image.get_data()
# Convert SVO image from RGBA to RGB
ocv_image_sbs_rgb = cv2.cvtColor(svo_image_sbs_rgba,
cv2.COLOR_RGBA2RGB)
# Write the RGB image in the video
video_writer.write(ocv_image_sbs_rgb)
else:
# Generate file names
filename1 = output_path / ("left%s.png" %
str(svo_position).zfill(6))
filename2 = output_path / (
("right%s.png" if app_type == AppType.LEFT_AND_RIGHT else
"depth%s.png") % str(svo_position).zfill(6))
# Save Left images
cv2.imwrite(str(filename1), left_image.get_data())
if app_type != AppType.LEFT_AND_DEPTH_16:
# Save right images
cv2.imwrite(str(filename2), right_image.get_data())
else:
# Save depth images (convert to uint16)
cv2.imwrite(str(filename2),
depth_image.get_data().astype(np.uint16))
# Display progress
progress_bar((svo_position + 1) / nb_frames * 100, 30)
# Check if we have reached the end of the video
if svo_position >= (nb_frames - 1): # End of SVO
sys.stdout.write("\nSVO end has been reached. Exiting now.\n")
break
if output_as_video:
# Close the video writer
video_writer.release()
zed.close()
return 0
def extract_xyz(x_pixel_M1, y_pixel_M1, x_pixel_M2, y_pixel_M2, outputfilename,
svofilename):
x_world_M1 = []
y_world_M1 = []
z_world_M1 = []
x_world_M2 = []
y_world_M2 = []
z_world_M2 = []
# x_pixel_M1 = [item[8] for item in xy_M1]
# y_pixel_M1 = [item[9] for item in xy_M1]
# x_pixel_M2 = [item[8] for item in xy_M2]
# y_pixel_M2 = [item[9] for item in xy_M2]
#filepath = "/home/avnish/zed_marmoset_recording/20200922.svo"
#print("SVO file : ", filepath.rsplit('/', 1)[1])
# Create a Camera object
# Create a InitParameters object and set configuration parameters
#init_params = sl.InitParameters(svo_input_filename=filepath,svo_real_time_mode=False)
#InitParameters init_params; // Set initial parameters
#init_params.sdk_verbose = True; // Enable verbose mode
#init_params.input.setFromSVOFile("/path/to/file.svo"); // Selects the and SVO file to be read
input_type = sl.InputType()
print("reading " + svofilename)
input_type.set_from_svo_file(svofilename)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
init.depth_mode = sl.DEPTH_MODE.ULTRA
init.coordinate_units = sl.UNIT.MILLIMETER # Use millimeter units (for depth measurements)
#init.depth_minimum_distance = 300
#init.depth_maximum_distance = 1500
zed = sl.Camera()
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS:
print('error_details: ', repr(err))
exit(1)
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.FILL
#runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL # Use STANDARD sensing mode
i = 0
pbar = tqdm(total=len(x_pixel_M1))
# image = sl.Mat()
point_cloud = sl.Mat()
try:
while i < len(x_pixel_M1):
# A new image is available if grab() returns SUCCESS
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
# zed.retrieve_image(image, sl.VIEW.VIEW_LEFT)
# Retrieve depth map. Depth is aligned on the left image
# zed.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH)
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
#print(x_pixel_M1[i], y_pixel_M1[i], x_pixel_M2[i], y_pixel_M2[i])
#print(type(x_pixel_M1[i]))
err_M1, point_cloud_value_M1 = point_cloud.get_value(
x_pixel_M1[i], y_pixel_M1[i])
err_M2, point_cloud_value_M2 = point_cloud.get_value(
x_pixel_M2[i], y_pixel_M2[i])
#print('after get value')
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
# x = round(image.get_width() / 2)
# y = round(image.get_height() / 2)
#lens2cage_top_mm=693.42 old
lens2cage_top_mm = 528.00 #Amelia
lens2cage_edge_x_mm = 387.35
lens2cage_edge_y_mm = 387.35
x_world_M1.append(
point_cloud_value_M1[0] +
lens2cage_edge_x_mm) #millimeters; in cage coordinates
y_world_M1.append(-point_cloud_value_M1[1] +
lens2cage_edge_y_mm)
z_world_M1.append(point_cloud_value_M1[2] - lens2cage_top_mm)
x_world_M2.append(point_cloud_value_M2[0] +
lens2cage_edge_x_mm)
y_world_M2.append(-point_cloud_value_M2[1] +
lens2cage_edge_y_mm)
z_world_M2.append(point_cloud_value_M2[2] - lens2cage_top_mm)
# Increment the loop
# print(i)
pbar.update(1)
i += 1
# if (i % 1000) == 0:
# print(i)
# sys.stdout.flush()
pbar.close()
# Close the camera
zed.close()
except OverflowError as error:
print(error)
print("frame_number_completed: {}".format(i))
pbar.close()
zed.close()
finally:
rows = zip(x_world_M1, y_world_M1, z_world_M1, x_world_M2, y_world_M2,
z_world_M2)
print("writing " + outputfilename)
with open(outputfilename, 'w', newline='') as f:
writer = csv.writer(f)
for row in rows:
writer.writerow(row)
print("Done")
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 = False
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 main():
# Create a Camera object
zed = sl.Camera()
# Set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_MEDIUM
init_params.coordinate_units = sl.UNIT.UNIT_METER
#init_params.depth_minimum_distance = 0.3
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
print("Error opening camera")
print(err)
exit(1)
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL
rows = 720
cols = 1280
M = np.float32([[1, 0, 300], [0, 1, 0]])
M2 = np.float32([[1, 0, -300], [0, 1, 0]])
#Capture an ideal image
depth_image = sl.Mat()
zed.retrieve_image(depth_image, sl.VIEW.VIEW_DEPTH)
idealDepthImage = cv2.resize(depth_image.get_data(), (0, 0),
fx=0.5,
fy=0.5)
try:
while True:
depth_image = sl.Mat()
depth_map = sl.Mat()
rgb_left_image = sl.Mat()
rgb_image = sl.Mat()
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(depth_image, sl.VIEW.VIEW_DEPTH)
zed.retrieve_image(rgb_image, sl.VIEW.VIEW_LEFT)
zed.retrieve_measure(depth_map, sl.MEASURE.MEASURE_DEPTH)
hsv_rgb = cv2.cvtColor(rgb_image.get_data(), cv2.COLOR_BGR2HSV)
hsv_rgb = cv2.resize(hsv_rgb, (0, 0), fx=0.5, fy=0.5)
gray_rgb = cv2.cvtColor(rgb_image.get_data(),
cv2.COLOR_BGR2GRAY)
gray_rgb = cv2.resize(gray_rgb, (0, 0), fx=0.5, fy=0.5)
hsvmean = cv2.mean(hsv_rgb)[2]
resizedDepthImage = cv2.resize(depth_image.get_data(), (0, 0),
fx=0.5,
fy=0.5)
#Compare to ideal image
sub = idealDepthImage - resizedDepthImage
sub = abs(sub)
threshold = 200
sub[sub >= threshold] = 255
sub[sub < threshold] = 0
mean = cv2.mean(resizedDepthImage)[0]
min_threshold = 15
max_threshold = 50
edges = cv2.Canny(resizedDepthImage, min_threshold,
max_threshold)
rgb = cv2.cvtColor(edges, cv2.COLOR_GRAY2RGB)
rgb *= np.array((0, 0, 1), np.uint8)
depthImageConvertedToColor = cv2.cvtColor(
resizedDepthImage, cv2.COLOR_BGRA2BGR)
out = np.bitwise_or(depthImageConvertedToColor, rgb)
cv2.imshow('Zed2', depth_image.get_data())
cv2.waitKey(1)
except Exception as e:
print(e)
zed.close()
exit(1)
def main():
print("Running Spatial Mapping sample ... Press 'q' to quit")
# Create a Camera object
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode
init_params.coordinate_units = sl.UNIT.METER # Set coordinate units
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP # OpenGL coordinates
# If applicable, use the SVO given as parameter
# Otherwise use ZED live stream
if len(sys.argv) == 2:
filepath = sys.argv[1]
print("Using SVO file: {0}".format(filepath))
init_params.set_from_svo_file(filepath)
# Open the camera
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
# Get camera parameters
camera_parameters = zed.get_camera_information(
).camera_configuration.calibration_parameters.left_cam
pymesh = sl.Mesh() # Current incremental mesh
image = sl.Mat() # Left image from camera
pose = sl.Pose() # Camera pose tracking data
viewer = gl.GLViewer()
viewer.init(camera_parameters, pymesh)
spatial_mapping_parameters = sl.SpatialMappingParameters()
tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF
mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED
mapping_activated = False
last_call = time.time() # Timestamp of last mesh request
# Enable positional tracking
err = zed.enable_positional_tracking()
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
exit()
# Set runtime parameters
runtime = sl.RuntimeParameters()
while viewer.is_available():
# Grab an image, a RuntimeParameters object must be given to grab()
if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
zed.retrieve_image(image, sl.VIEW.LEFT)
# Update pose data (used for projection of the mesh over the current image)
tracking_state = zed.get_position(pose)
if mapping_activated:
mapping_state = zed.get_spatial_mapping_state()
# Compute elapsed time since the last call of Camera.request_spatial_map_async()
duration = time.time() - last_call
# Ask for a mesh update if 500ms elapsed since last request
if (duration > .5 and viewer.chunks_updated()):
zed.request_spatial_map_async()
last_call = time.time()
if zed.get_spatial_map_request_status_async(
) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_spatial_map_async(pymesh)
viewer.update_chunks()
change_state = viewer.update_view(image, pose.pose_data(),
tracking_state, mapping_state)
if change_state:
if not mapping_activated:
init_pose = sl.Transform()
zed.reset_positional_tracking(init_pose)
# Configure spatial mapping parameters
spatial_mapping_parameters.resolution_meter = sl.SpatialMappingParameters(
).get_resolution_preset(sl.MAPPING_RESOLUTION.MEDIUM)
spatial_mapping_parameters.use_chunk_only = True
spatial_mapping_parameters.save_texture = False # Set to True to apply texture over the created mesh
spatial_mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.MESH
# Enable spatial mapping
zed.enable_spatial_mapping()
# Clear previous mesh data
pymesh.clear()
viewer.clear_current_mesh()
# Start timer
last_call = time.time()
mapping_activated = True
else:
# Extract whole mesh
zed.extract_whole_spatial_map(pymesh)
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MEDIUM)
# Filter the extracted mesh
pymesh.filter(filter_params, True)
viewer.clear_current_mesh()
# If textures have been saved during spatial mapping, apply them to the mesh
if (spatial_mapping_parameters.save_texture):
print("Save texture set to : {}".format(
spatial_mapping_parameters.save_texture))
pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA)
# Save mesh as an obj file
filepath = "mesh_gen.obj"
status = pymesh.save(filepath)
if status:
print("Mesh saved under " + filepath)
else:
print("Failed to save the mesh under " + filepath)
mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED
mapping_activated = False
image.free(memory_type=sl.MEM.CPU)
pymesh.clear()
# Disable modules and close camera
zed.disable_spatial_mapping()
zed.disable_positional_tracking()
zed.close()
def detector():
# Initialize publisher ROS node
pub = rospy.Publisher('predictions', Predictions, queue_size=10)
pub1 = rospy.Publisher('peoplecount', Peoplecount, queue_size=10)
pub2 = rospy.Publisher('videostream', Image, queue_size=1)
rospy.init_node('detector', anonymous=True)
# Ceate sort object
mot_tracker = Sort()
# Define paths for yolo files
darknet_path = '/home/ubuntu/catkin_ws/src/jetsontx1_cvmodule/src/pyyolo/darknet' # Only './darknet' is dependent on location of rosrun command
datacfg = 'cfg/coco.data'
cfgfile = 'cfg/yolov3-tiny.cfg'
weightfile = '../yolov3-tiny.weights' #'/media/ubuntu/SDcard/yoloWeights/yolov2-tiny.weights' this also works but it loads way slower
filename = darknet_path + '/data/person.jpg'
# Image resolution parameters
(width,
height) = (1280, 720
) # Use (672,376) for VGA and (1280,720) for HD720 resolution
# Initialize visualization
fourcc = cv2.VideoWriter_fourcc(*'MJPG')
video = cv2.VideoWriter('predictionstest.avi', fourcc, 10, (width, height))
# Initialize pyyolo
pyyolo.init(darknet_path, datacfg, cfgfile, weightfile)
# Initialize face detector
face_cascade = cv2.CascadeClassifier(
'/usr/share/OpenCV/haarcascades/haarcascade_frontalface_default.xml')
smile_cascade = cv2.CascadeClassifier(
'/usr/share/OpenCV/haarcascades/haarcascade_smile.xml')
# Create a Camera object
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD1080, HD720 or VGA video mode
init_params.camera_fps = 15 # Set fps at 30
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
zed.set_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE, 50)
image = sl.Mat()
point_cloud = sl.Mat()
colours = 255 * np.random.rand(32,
3) # For drawing different colours on BB
runtime_parameters = sl.RuntimeParameters()
while not rospy.is_shutdown():
start = time.time()
# Grab an image, a RuntimeParameters object must be given to grab()
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# A new image is available if grab() returns SUCCESS
zed.retrieve_image(image, sl.VIEW.LEFT)
data = image.get_data()
gray_picture = cv2.cvtColor(
data, cv2.COLOR_BGR2GRAY
) # Make picture gray for face/smile detection
# Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
Data = data.transpose(2, 0, 1)
start5 = time.time()
Data = Data.ravel() / 255.0
end5 = time.time()
Data = np.ascontiguousarray(Data, dtype=np.float32)
start1 = time.time()
outputs = pyyolo.detect(
width, height, 4, Data, 0.5, 0.8
) #pyyolo.detect returns: [{'right':, 'bottom':, 'top':, 'class':, 'prob':, 'left':}]
end1 = time.time()
print("Section cycle time: ", end1 - start1)
dets = np.empty((0, 5), int)
count = 0
for output in outputs:
if output['class'] == 'person': #track only people
count = count + 1 # Count detected people
dets = np.append(dets, [[
output['left'], output['top'], output['right'],
output['bottom'], output['prob']
]],
axis=0)
people = Peoplecount()
people.tot_detected_people = count
pub1.publish(people)
preds = Predictions()
trackers = mot_tracker.update(
dets
) #update tracking ID. mot_tracker.update() returns: [[x1,y1,x2,y2,id]]
for d in trackers:
d = d.astype(np.int32)
# Create a dictionary to store info for publishing
detectinfo = {
'left': d[0],
'top': d[1],
'right': d[2],
'bottom': d[3],
'class': 'person',
'ID': int(d[4])
}
euclidean_distance(detectinfo, point_cloud)
relative_coordinates(detectinfo, width)
start3 = time.time()
facesmile_detect(detectinfo, gray_picture, data, face_cascade,
smile_cascade)
end3 = time.time()
#print("facesmile detect cycle time: ", end3 - start3)
#print detectinfo['smile']
pred = Prediction()
#pred.probabilities.append()
pred.classes.append(detectinfo['class'])
pred.xmin = detectinfo['left']
pred.ymin = detectinfo['top']
pred.xmax = detectinfo['right']
pred.ymax = detectinfo['bottom']
pred.id = detectinfo['ID']
pred.face = detectinfo['face']
pred.smile = detectinfo['smile']
pred.distance = detectinfo['distance']
pred.angle = detectinfo['angle']
pred.xcoord = detectinfo['x']
pred.ycoord = detectinfo['y']
preds.predictions.append(pred)
label = detectinfo['class'] + " " + str(detectinfo['ID'])
cv2.rectangle(data, (d[0], d[1]), (d[2], d[3]),
(colours[d[4] % 32, 0], colours[d[4] % 32, 1],
colours[d[4] % 32, 2]), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(data, label, (d[2], d[1] + 25), font, 1,
(0, 0, 255), 1, cv2.LINE_AA)
pub.publish(preds)
msg_frame = CvBridge().cv2_to_imgmsg(data, "8UC4")
pub2.publish(msg_frame)
#video.write(data[:,:,:3])#because data.shape is (376,672,4) and it only supports 3 channels.
cv2.imshow("image", data)
cv2.waitKey(35)
end = time.time()
print("Total cycle time: ", end - start)
# Close the camera
zed.close()
def main() :
init = sl.InitParameters()
init.coordinate_units = sl.UNIT.UNIT_METER
cam = sl.Camera()
status = cam.open(init)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD
mat1 = sl.Mat()
mat2 = sl.Mat()
# if len(sys.argv) == 1 :
# print('Please provide ZED serial number')
# exit(1)
# # Open the ZED camera
# cap = cv2.VideoCapture(0)
# if cap.isOpened() == 0:
# exit(-1)
# image_size = Resolution()
# image_size.width = 1280
# image_size.height = 720
# # Set the video resolution to HD720
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, image_size.width*2)
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, image_size.height)
# serial_number = int(sys.argv[1])
# calibration_file = download_calibration_file(serial_number)
# if calibration_file == "":
# exit(1)
# print("Calibration file found. Loading...")
# camera_matrix_left, camera_matrix_right, map_left_x, map_left_y, map_right_x, map_right_y = init_calibration(calibration_file, image_size)
while True :
# Get a new frame from camera
#retval, frame = cap.read()
# Extract left and right images from side-by-side
#left_right_image = np.split(frame, 2, axis=1)
# Display images
err = cam.grab(runtime)
cam.retrieve_image(mat1, sl.VIEW.VIEW_LEFT)
cam.retrieve_image(mat2, sl.VIEW.VIEW_RIGHT)
left_rect = mat1.get_data()
right_rect = mat2.get_data()
#cv2.imshow("left RAW", left_right_image[0])
#left_rect = cv2.remap(left_right_image[0], map_left_x, map_left_y, interpolation=cv2.INTER_LINEAR)
#right_rect = cv2.remap(left_right_image[1], map_right_x, map_right_y, interpolation=cv2.INTER_LINEAR)
cv2.imshow("left RECT", left_rect)
cv2.imshow("right RECT", right_rect)
stereo = cv2.StereoSGBM_create(minDisparity = 1,
numDisparities = 128,
blockSize = 4,
uniquenessRatio = 1,
speckleRange = 3,
speckleWindowSize = 8,
disp12MaxDiff = 200,
P1 = 600,
P2 = 2400
# SADWindowSize = 6,
# uniquenessRatio = 10,
# speckleWindowSize = 100,
# speckleRange = 32,
# disp12MaxDiff = 1,
# P1 = 8*3*3**2,
# P2 = 32*3*3**2,
# fullDP = False
)
disp = stereo.compute(left_rect, right_rect).astype(np.float32) / 16.0
cv2.imshow("disparity", (disp-1)/(128))
if cv2.waitKey(30) >= 0 :
break
exit(0)
if __name__ == "__main__":
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)
image = sl.Mat()
if zed.grab() == sl.ERROR_CODE.SUCCESS:
# zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, res)
# # point_cloud.write('C:/Users/user/Desktop/python/b')
#
# point_cloud_np = point_cloud.get_data()
# cutZed(point_cloud_np)
zed.retrieve_image(image, sl.VIEW.RIGHT) # Retrieve the left image
image.write('C:/Users/user/Desktop/python/d.png')
plt.imshow(image.get_data())
zed.close()
ZED_SVO_PATH = os.path.join(DIR, ZED_SVO)
framerate = 20
resolution = (1280, 720)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
colorVideoOut = cv2.VideoWriter('color.avi', fourcc, framerate, resolution)
depthVideoOut = cv2.VideoWriter('depth.avi', fourcc, framerate, resolution)
init_parameters = sl.InitParameters()
init_parameters.set_from_svo_file(ZED_SVO_PATH)
zed = sl.Camera()
err = zed.open(init_parameters)
runtime = sl.RuntimeParameters()
svo_image = sl.Mat(resolution[1], resolution[0], sl.MAT_TYPE.U8_C4)
depth_map = sl.Mat(resolution[1], resolution[0], sl.MAT_TYPE.U8_C4)
i = 0
for i in tqdm(range(zed.get_svo_number_of_frames())):
if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS:
i+=1
zed.retrieve_image(svo_image, sl.VIEW.LEFT, sl.MEM.CPU)
zed.retrieve_image(depth_map, sl.VIEW.DEPTH, sl.MEM.CPU)
colorNumpy = svo_image.get_data()
colorNumpy = cv2.resize(colorNumpy, resolution, cv2.INTER_AREA)
colorNumpy = cv2.cvtColor(colorNumpy, cv2.COLOR_RGBA2RGB)
colorVideoOut.write(colorNumpy)
depthNumpy = depth_map.get_data()
depthNumpy = cv2.resize(depthNumpy, resolution, cv2.INTER_AREA)
depthNumpy = cv2.cvtColor(depthNumpy, cv2.COLOR_BGR2RGB)
def main(argv):
thresh = 0.25
darknet_path = "../darknet/"
# configPath = darknet_path + "cfg/yolov3.cfg"
# weightPath = "yolov3.weights"
# metaPath = "coco.data"
V = 3.2
if (V == 1):
configPath = darknet_path + "cfg/yolov3-gripper.cfg"
weightPath = "yolov3-gripper_final.weights"
metaPath = "gripper.data"
elif (V == 3):
configPath = darknet_path + "cfg/yolov3-gripper_3.cfg"
weightPath = "yolov3-gripper_3_final.weights"
metaPath = "gripper_3.data"
elif (V == 3.2):
configPath = darknet_path + "cfg/yolov3-gripper_3_2_verified.cfg"
weightPath = "yolov3-gripper_3_2_verified_final.weights"
metaPath = "gripper_3.data"
else:
configPath = darknet_path + "cfg/yolov3-gripper.cfg"
weightPath = "yolov3-gripper_final.weights"
metaPath = "gripper.data"
#configPath = darknet_path + "cfg/yolov3-gripper.cfg"
# weightPath = "yolov3-gripper_final.weights"
# weightPath = "yolov3-gripper_3_final.weights"
# metaPath = "gripper_3.data"
#metaPath = "gripper.data"
svoPath = None
help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file>'
try:
opts, args = getopt.getopt(
argv, "hc:w:m:t:s:",
["config=", "weight=", "meta=", "threshold=", "svo_file="])
except getopt.GetoptError:
print(help_str)
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print(help_str)
sys.exit()
elif opt in ("-c", "--config"):
configPath = arg
elif opt in ("-w", "--weight"):
weightPath = arg
elif opt in ("-m", "--meta"):
metaPath = arg
elif opt in ("-t", "--threshold"):
thresh = float(arg)
elif opt in ("-s", "--svo_file"):
svoPath = arg
init = sl.InitParameters()
init.coordinate_units = sl.UNIT.UNIT_METER
if svoPath is not None:
init.svo_input_filename = svoPath
cam = sl.Camera()
if not cam.is_opened():
print("Opening ZED Camera...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
# Use STANDARD sensing mode
runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD
mat = sl.Mat()
point_cloud_mat = 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(configPath):
raise ValueError("Invalid config path `" +
os.path.abspath(configPath) + "`")
if not os.path.exists(weightPath):
raise ValueError("Invalid weight path `" +
os.path.abspath(weightPath) + "`")
if not os.path.exists(metaPath):
raise ValueError("Invalid data file path `" +
os.path.abspath(metaPath) + "`")
if netMain is None:
netMain = load_net_custom(configPath.encode("ascii"),
weightPath.encode("ascii"), 0,
1) # batch size = 1
if metaMain is None:
metaMain = load_meta(metaPath.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(metaPath) as metaFH:
metaContents = metaFH.read()
import re
match = re.search("names *= *(.*)$", metaContents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as namesFH:
namesList = namesFH.read().strip().split("\n")
altNames = [x.strip() for x in namesList]
except TypeError:
pass
except Exception:
pass
color_array = generateColor(metaPath)
print("Running...")
##CYW_RemoteControl
db = CYW_RemoteControl.global_database()
operator = CYW_RemoteControl.operator(db)
# operator.send_server_start() #no auto send. use manual publish
operator.receive_server_start()
ct = CYW_RemoteControl.controller(db, operator)
pickup_times = -1
first_loop = True
key = ''
while key != 113: # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT)
image = mat.get_data()
cam.retrieve_measure(point_cloud_mat, sl.MEASURE.MEASURE_XYZRGBA)
depth = point_cloud_mat.get_data()
# Do the detection
detections = detect(netMain, metaMain, image, thresh)
# print(chr(27) + "[2J"+"**** " +
# str(len(detections)) + " Results ****")
for detection in detections:
label = detection[0]
# if(label not in show_lables):
# break
confidence = detection[1]
# if(confidence<0.85):
# continue
pstring = label + ": " + str(np.rint(100 * confidence)) + "%"
print(pstring)
bounds = detection[2]
yExtent = int(bounds[3])
xEntent = int(bounds[2])
# Coordinates are around the center
xCoord = int(bounds[0] - bounds[2] / 2)
yCoord = int(bounds[1] - bounds[3] / 2)
boundingBox = [[xCoord, yCoord], [xCoord, yCoord + yExtent],
[xCoord + xEntent, yCoord + yExtent],
[xCoord + xEntent, yCoord]]
thickness = 1
x, y, z = getObjectDepth(depth, bounds)
if (label == "iiwa_gripper"):
# db.cam_iiwa_x,db.cam_iiwa_y,db.cam_iiwa_z=x,y+0.05,z+0.09
db.cam_iiwa_x, db.cam_iiwa_y, db.cam_iiwa_z = x + 0.02, y, z - 0.03
db.cam_iiwa_updated = 1
elif (label == "cube"):
db.cam_cube_x, db.cam_cube_y, db.cam_cube_z = x, y, z
db.cam_cube_updated = 1
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
x = "{:.2f}".format(x)
y = "{:.2f}".format(y)
z = "{:.2f}".format(z)
cv2.rectangle(image, (xCoord - thickness, yCoord - thickness),
(xCoord + xEntent + thickness, yCoord +
(18 + thickness * 4)),
color_array[detection[3]], -1)
cv2.putText(
image, label + " " + "{:.2f}".format(confidence) + " " +
(str(distance) + " m") + (" x=" + str(x)) +
(" y=" + str(y)) + (" z=" + str(z)),
(xCoord + (thickness * 4), yCoord + (10 + thickness * 4)),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
cv2.rectangle(image, (xCoord - thickness, yCoord - thickness),
(xCoord + xEntent + thickness,
yCoord + yExtent + thickness),
color_array[detection[3]], int(thickness * 2))
#control part
# print(db.cam_cube_x,db.cam_cube_y,db.cam_cube_z)
# print(db.cam_iiwa_x,db.cam_iiwa_y,db.cam_iiwa_z)
if (db.cam_cube_updated == 1 and db.cam_iiwa_updated == 1
and db.blocked == 0):
cube_z_iiwaBse_add = 0.3
pos_error_cam = [
-(db.cam_iiwa_x - db.cam_cube_x),
-(db.cam_iiwa_y - db.cam_cube_y - cube_z_iiwaBse_add),
-(db.cam_iiwa_z - db.cam_cube_z)
]
pos_error_iiwaBase = db.coordinate_cam_2_iiwaBase(
pos_error_cam)
error_allow_list = [0.02, 0.02, 0.8] #m
if (funcs.check_xyz_error_ok(error_list=pos_error_iiwaBase,
allow_list=error_allow_list)
): #at the correct place, ready to pick up
main_thread = threading.Thread(
target=ct.do_go_to_xy_then_pick,
args=(db.r_iiwa_x, db.r_iiwa_y))
main_thread.start()
pass
print("pos_error_cam", pos_error_cam)
print("pos_error_iiwaBase", pos_error_iiwaBase)
# db.iiwa_x_sp=db.r_iiwa_x+pos_error_iiwaBase[0]*100
# db.iiwa_y_sp=db.r_iiwa_y+pos_error_iiwaBase[1]*100
# if(pickup_times==0 and first_loop==False):
# db.iiwa_x_sp = db.r_iiwa_x + pos_error_iiwaBase[0] * 100
# db.iiwa_y_sp = db.r_iiwa_y + pos_error_iiwaBase[1] * 100
# ct.do_go_to_xy_then_pick(x=db.r_iiwa_x+pos_error_iiwaBase[0]*10,y=db.r_iiwa_y+pos_error_iiwaBase[1]*10)
# pickup_times=pickup_times+1
# else:
# print("already pick up")
# ct.pos_pid_control(pos_error_iiwaBase[0],pos_error_iiwaBase[1],pos_error_iiwaBase[2],P_gain=20,I_gain=0,D_gain=0)
ct.pos_pid_control(dx=pos_error_iiwaBase[0],
dy=pos_error_iiwaBase[1],
dz=0,
P_gain=160,
I_gain=0,
D_gain=0)
#for safety, no z control.
operator.publish()
cv2.imshow("ZED", image)
key = cv2.waitKey(5)
first_loop = False
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
cam.close()
print("\nFINISH")
def main():
args = cli()
# load model
model, _ = nets.factory_from_args(args)
model = model.to(args.device)
processor = decoder.factory_from_args(args, model)
# zed
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA
init.coordinate_units = sl.UNIT.UNIT_METER
init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode
img = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
last_loop = time.time()
#capture = cv2.VideoCapture(args.source)
visualizer = None
while True:
err = cam.grab(runtime_parameters)
if err == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
cam.retrieve_image(img, sl.VIEW.VIEW_LEFT)
# Retrieve depth map. Depth is aligned on the left image
cam.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
cam.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA)
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = round(img.get_width() / 2)
y = round(img.get_height() / 2)
err, point_cloud_value = point_cloud.get_value(x, y)
err, depth_value = depth.get_value(x, y)
print("depth ", depth_value)
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] +
point_cloud_value[1] * point_cloud_value[1] +
point_cloud_value[2] * point_cloud_value[2])
if not np.isnan(distance) and not np.isinf(distance):
distance = round(distance)
#print("Distance to Camera at ({0}, {1}): {2} mm\n".format(x, y, distance))
else:
print(
"Can't estimate distance at this position, move the camera\n"
)
cv2.imshow("Depth", depth.get_data())
else:
print("Err", err)
continue
image = cv2.resize(img.get_data(), None, fx=args.scale, fy=args.scale)
#print('resized image size: {}'.format(image.shape))
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
if visualizer is None:
visualizer = Visualizer(processor, args)(image)
visualizer.send(None)
start = time.time()
image_pil = PIL.Image.fromarray(image)
processed_image_cpu, _, __ = transforms.EVAL_TRANSFORM(
image_pil, [], None)
processed_image = processed_image_cpu.contiguous().to(
args.device, non_blocking=True)
#print('preprocessing time', time.time() - start)
fields = processor.fields(torch.unsqueeze(processed_image, 0))[0]
visualizer.send((image, fields))
#print('loop time = {:.3}s, FPS = {:.3}'.format(
# time.time() - last_loop, 1.0 / (time.time() - last_loop)))
last_loop = time.time()
cam.close()
def main():
print("Running...")
time_stamp = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d-%H:%M:%S')
init = sl.InitParameters()
init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080
init.camera_linux_id = 0
init.camera_fps = 10 # The framerate is lowered to avoid any USB3 bandwidth issues
cam = sl.Camera()
if not cam.is_opened():
print("Opening ZED Camera 1...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print('Camera 1: ' + repr(status))
exit()
# init.camera_linux_id = 1 # selection of the ZED ID
# cam2 = sl.Camera()
# if not cam2.is_opened():
# print("Opening ZED Camera 2...")
# status = cam2.open(init)
# if status != sl.ERROR_CODE.SUCCESS:
# print('Camera 2: ' + repr(status))
# exit()
runtime = sl.RuntimeParameters()
mat = sl.Mat()
# mat2 = sl.Mat()
print_camera_information(cam)
# print_camera_information(cam2)
fourcc = cv2.VideoWriter_fourcc(*'MJPG')
out_c1 = None
out_c2 = None
while True:
# Grab an image, a RuntimeParameters object must be given to grab()
if cam.grab(
runtime
) == sl.ERROR_CODE.SUCCESS: # and cam2.grab(runtime) == sl.ERROR_CODE.SUCCESS:
# A new image is available if grab() returns SUCCESS
cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT)
# cam2.retrieve_image(mat2, sl.VIEW.VIEW_LEFT)
if out_c1 is None: # or out_c2 is None:
print("Saving the recording at: {}".format(PATH + time_stamp +
'_cam1.avi'))
out_c1 = cv2.VideoWriter(
PATH + time_stamp + '_cam1.avi', fourcc, 20.0,
(int(mat.get_width()), int(mat.get_height())))
# out_c2 = cv2.VideoWriter(time_stamp + '_cam2.avi', fourcc, 20.0, (int(mat2.get_width()),
# int(mat2.get_height())))
c1_frame = mat.get_data()
# c2_frame = mat2.get_data()
# Save the frames as a video
if out_c1 is not None:
out_c1.write(c1_frame[:, :, :3])
# out_c2.write(c2_frame)
# Display the image frames
cv2.imshow('Camera 1 - Left', c1_frame)
# cv2.imshow('Camera 2 - Left', c2_frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cam.close()
# cam2.close()
out_c1.release()
# out_c2.release()
cv2.destroyAllWindows()
print("\nFINISH")
def main():
cam = sl.Camera()
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.ULTRA
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
if len(sys.argv) == 2:
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
init.set_from_svo_file(filepath)
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD
spatial = sl.SpatialMappingParameters()
transform = sl.Transform()
tracking = sl.PositionalTrackingParameters(transform)
cam.enable_positional_tracking(tracking)
pymesh = sl.Mesh()
pyplane = sl.Plane()
reset_tracking_floor_frame = sl.Transform()
found = 0
print("Processing...")
i = 0
calibration_params = cam.get_camera_information().calibration_parameters
# Focal length of the left eye in pixels
focal_left_x = calibration_params.left_cam.fx
focal_left_y = calibration_params.left_cam.fy
# First radial distortion coefficient
# k1 = calibration_params.left_cam.disto[0]
# Translation between left and right eye on z-axis
# tz = calibration_params.T.z
# Horizontal field of view of the left eye in degrees
# h_fov = calibration_params.left_cam.h_fov
print("fx", focal_left_x)
print("fy", focal_left_y)
print("cx", calibration_params.left_cam.cx)
print("cy", calibration_params.left_cam.cy)
print("distortion", calibration_params.left_cam.disto)
cloud = sl.Mat()
while i < 1000:
if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS :
err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame)
if err == sl.ERROR_CODE.SUCCESS:
# if i > 200 and err == sl.ERROR_CODE.SUCCESS:
found = 1
print('Floor found!')
pymesh = pyplane.extract_mesh()
#get gound plane info
print("floor normal",pyplane.get_normal()) #print normal of plane
print("floor equation", pyplane.get_plane_equation())
print("plane center", pyplane.get_center())
print("plane pose", pyplane.get_pose())
# camera_pose = sl.Pose()
# py_translation = sl.Translation()
# tracking_state = cam.get_position(camera_pose)
# # if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
# rotation = camera_pose.get_rotation_vector()
# rx = round(rotation[0], 2)
# ry = round(rotation[1], 2)
# rz = round(rotation[2], 2)
# translation = camera_pose.get_translation(py_translation)
# tx = round(translation.get()[0], 2)
# ty = round(translation.get()[1], 2)
# tz = round(translation.get()[2], 2)
# text_translation = str((tx, ty, tz))
# text_rotation = str((rx, ry, rz))
# pose_data = camera_pose.pose_data(sl.Transform())
# print("translation",text_translation)
# print("rotation",text_rotation)
cam.retrieve_measure(cloud, sl.MEASURE.XYZRGBA)
break
i+=1
if found == 0:
print('Floor was not found, please try with another SVO.')
cam.close()
exit(0)
#load image
path = "/home/hcl/Documents/ZED/pics/Explorer_HD1080_SN14932_16-24-06.png"
im = cv2.imread(path)
h, w = im.shape[0], im.shape[1]
print("orignal image shape", h,w)
im = im[:,:int(w/2),:] #get left image
# im = cv2.resize(im,(int(im.shape[1]/2),int(im.shape[0]/2)))
#warped img
warped = np.zeros((h,w,3))
roll = -80
roll *= np.pi/180
Rx = np.matrix([
[1, 0,0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
#point cloud
# i = 500
# j = 355
if False:
point_cloud = sl.Mat()
cam.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
for i in range(100):
for j in range(1000):
point3D = point_cloud.get_value(i,j)
# x = point3D[0]
# y = point3D[1]
# z = point3D[2]
color = point3D[1][3]
if ~np.isnan(color) and ~np.isinf(color):
xyz = point3D[1][:3].reshape((-1,1))
# xyz_hom = np.vstack((xyz,1))
print('point3D',point3D)
warped_index = Rx @ xyz #NEED TO USE H**O COORDINATES? divide by last column?
print('warped_index',warped_index)
RGB = im[j,i,:]
RGB = np.dstack(([255],[255],[255]))
print('RGB',RGB)
warped[int(np.round(warped_index[0]+10)),int(np.round(warped_index[1])),:] = RGB
cam.disable_positional_tracking()
# save_mesh(pymesh)
cam.close()
print("\nFINISH")
def capture_thread_func():
global image_np_global, depth_np_global, exit_signal, new_data
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
init_params.camera_fps = 60
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE
init_params.coordinate_units = sl.UNIT.UNIT_METER
init_params.svo_real_time_mode = False
# Open the camera
err = zed.open(init_params)
tracking_parameters = sl.TrackingParameters(sl.Transform())
tracking_parameters.enable_spatial_memory = True
err = zed.enable_tracking(tracking_parameters)
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()
while not exit_signal:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(image_mat,
sl.VIEW.VIEW_LEFT,
width=width,
height=height)
zed.retrieve_measure(depth_mat,
sl.MEASURE.MEASURE_XYZRGBA,
width=width,
height=height)
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()
# For spatial tracking
tracking_state = zed.get_position(
zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
if tracking_state == sl.TRACKING_STATE.TRACKING_STATE_OK:
# Getting spatial position
py_translation = sl.Translation()
tx = round(
zed_pose.get_translation(py_translation).get()[0], 2)
ty = round(
zed_pose.get_translation(py_translation).get()[1], 2)
tz = round(
zed_pose.get_translation(py_translation).get()[2], 2)
# Getting spatial orientation
py_orientation = sl.Orientation()
ox = round(
zed_pose.get_orientation(py_orientation).get()[0], 2)
oy = round(
zed_pose.get_orientation(py_orientation).get()[1], 2)
oz = round(
zed_pose.get_orientation(py_orientation).get()[2], 2)
ow = round(
zed_pose.get_orientation(py_orientation).get()[3], 2)
# Getting spatial orientation
rotation = zed_pose.get_rotation_vector()
rx = round(rotation[0], 2)
ry = round(rotation[1], 2)
rz = round(rotation[2], 2)
rx *= (180 / math.pi)
ry *= (180 / math.pi)
rz *= (180 / math.pi)
rx = round(rx, 2)
ry = round(ry, 2)
rz = round(rz, 2)
# Storing some position data
voi.coord = [tx, ty, tz]
voi.rotation = [rx, ry, rz]
voi.orientation = [ox, oy, oz, ow]
sleep(0.01)
zed.close()
zed.enable_positional_tracking()
zed.enable_object_detection(obj_param)
camera_info = zed.get_camera_information()
# Create OpenGL viewer
viewer = gl.GLViewer()
viewer.init(camera_info.calibration_parameters.left_cam)
# Configure object detection runtime parameters
obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
obj_runtime_param.detection_confidence_threshold = 50
# Create ZED objects filled in the main loop
objects = sl.Objects()
image = sl.Mat()
while viewer.is_available():
# Grab an image, a RuntimeParameters object must be given to grab()
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
zed.retrieve_image(image, sl.VIEW.LEFT)
# Retrieve objects
zed.retrieve_objects(objects, obj_runtime_param)
# Update GL view
viewer.update_view(image, objects)
viewer.exit()
image.free(memory_type=sl.MEM.CPU)
# Disable modules and close camera
def main(argv):
thresh = 0.25
darknet_path="../libdarknet/"
config_path = darknet_path + "cfg/yolov3-tiny.cfg"
weight_path = "yolov3-tiny.weights"
meta_path = "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
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()
point_cloud_mat = 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)
log.info("Running...")
key = ''
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)
image = mat.get_data()
cam.retrieve_measure(
point_cloud_mat, sl.MEASURE.XYZRGBA)
depth = point_cloud_mat.get_data()
# Do the detection
detections = detect(netMain, metaMain, image, thresh)
log.info(chr(27) + "[2J"+"**** " + str(len(detections)) + " Results ****")
for detection in detections:
label = detection[0]
confidence = detection[1]
pstring = label+": "+str(np.rint(100 * confidence))+"%"
log.info(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)
#boundingBox = [[x_coord, y_coord], [x_coord, y_coord + y_extent], [x_coord + x_extent, y_coord + y_extent], [x_coord + x_extent, y_coord]]
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, label + " " + (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))
cv2.imshow("ZED", image)
key = cv2.waitKey(5)
log.info("FPS: {}".format(1.0 / (time.time() - start_time)))
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
cam.close()
log.info("\nFINISH")
init_params.camera_resolution = sl.RESOLUTION.VGA
init_params.depth_minimum_distance = 300
# Set sensing mode in FILL
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.FILL
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
print('Error is ', err)
exit(1)
camera_height = zed.get_camera_information().camera_resolution.height
camera_width = zed.get_camera_information().camera_resolution.width
image_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.U8_C4)
depth_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.F32_C1)
image_depth_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.U8_C4)
pub = rospy.Publisher('ball_position', ball_positions)
rospy.init_node('vision_processing')
rate = rospy.Rate(60) # loops 10 times per second
timer_frames = 5
timer_count = timer_frames
start_time = time.time()
# Video capturing
while (not rospy.is_shutdown()) and zed.grab() == sl.ERROR_CODE.SUCCESS:
def main():
recordVideo = bool(False)
showImages = bool(False)
if len(sys.argv) != 2:
print("Please specify path to .svo file.")
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
# Specify SVO path parameter
# init_params = sl.InitParameters()
# init_params.set_from_svo_file(str(svo_input_path))
# init_params.svo_real_time_mode = False # Don't convert in realtime
# init_params.coordinate_units = sl.UNIT.MILLIMETER # Use milliliter units (for depth measurements)
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init_params = sl.InitParameters(input_t=input_type,
svo_real_time_mode=False)
init_params.coordinate_units = sl.UNIT.CENTIMETER # Use milliliter units (for depth measurements)
init_params.depth_mode = sl.DEPTH_MODE.ULTRA # Use ULTRA depth mode
init_params.depth_minimum_distance = 1 # Set the minimum depth perception distance to 1 m
init_params.depth_maximum_distance = 40 # Set the maximum depth perception distance to 40m
cam = sl.Camera()
status = cam.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
image = sl.Mat()
depth_map = sl.Mat()
point_cloud = sl.Mat()
## initialize image window
#define the screen resulation
screen_res = 1920, 1200
#image size
width = cam.get_camera_information().camera_resolution.width
height = cam.get_camera_information().camera_resolution.height
scale_width = screen_res[0] / width
scale_height = screen_res[1] / height
scale = min(scale_width, scale_height)
#resized window width and height
window_width = int(width * scale)
window_height = int(height * scale)
window_size = (window_width, window_height)
#cv2.WINDOW_NORMAL makes the output window resizealbe
cv2.namedWindow('result', cv2.WINDOW_NORMAL)
#resize the window according to the screen resolution
#cv2.resizeWindow("result", window_width, window_height)
key = ''
print(" Save the current image: s")
print(" Quit the video reading: q\n")
print(cam.get_svo_position())
cam.set_svo_position(10000)
print(cam.get_svo_position())
i = 0
frame_processing_time = time.time()
while (key != 113) or (i != 50): # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(image, sl.VIEW.LEFT)
# To recover data from sl.Mat to use it with opencv, use the get_data() method
# It returns a numpy array that can be used as a matrix with opencv
frame = image.get_data()
# Get the depth map
cam.retrieve_measure(depth_map, sl.MEASURE.DEPTH)
cam.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
#line detection
lane_image = np.copy(frame)
canny_image = canny(frame)
cropped_image = region_of_intrest(canny_image)
lines = cv2.HoughLinesP(cropped_image,
2,
np.pi / 180,
100,
np.array([]),
minLineLength=40,
maxLineGap=5)
averaged_lines = average_slope_intercept(lane_image, lines)
Hline_image = display_lines(lane_image, lines)
line_image = display_lines(lane_image, averaged_lines)
combo_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1)
cropped_combo_image = region_of_intrest(combo_image)
## get distance / location
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = round(image.get_width() / 2)
y = round(image.get_height() / 2)
err, point_cloud_value = point_cloud.get_value(x, y)
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] +
point_cloud_value[1] * point_cloud_value[1] +
point_cloud_value[2] * point_cloud_value[2])
point_cloud_np = point_cloud.get_data()
# point_cloud_np.dot(tr_np)
if not np.isnan(distance) and not np.isinf(distance):
# print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(x, y, distance), end="\r")
# Increment the loop
i = i + 1
else:
print("Can't estimate distance at this position.")
print(
"Your camera is probably too close to the scene, please move it backwards.\n"
)
sys.stdout.flush()
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
# x = mat.get_width() / 2
# y = mat.get_height() / 2
# point_cloud_value = point_cloud.get_value(x, y)
# distance = math.sqrt(point_cloud_value[0]*point_cloud_value[0] + point_cloud_value[1]*point_cloud_value[1] + point_cloud_value[2]*point_cloud_value[2])
# printf("Distance to Camera at (", x, y, "): ", distance, "mm")
# x1, y1, x2, y2 = lines[0].reshape(4)
# depth_value = depth_map.get_value(x2,y2)
# print("x2= ",x2," y2= ",y2," z= ",depth_value)
# point3D = point_cloud.get_value(1220,850)#1220 850
# x = point3D[0]
# y = point3D[1]
# z = point3D[2]
# #color = point3D[3]
# print("x= ",x," y= ",y," z= ",z)
if showImages:
# cv2.imshow("cropped_lane_image", cropped_lane_image)
# frame_canny = cv2.cvtColor(canny_image, cv2.COLOR_GRAY2BGR)
# frame_cropped = cv2.cvtColor(cropped_image, cv2.COLOR_GRAY2BGR)
# plt.imshow(cropped_combo_image)#canny_image)
# plt.show()
# combined_image= combine(lane_image, cropped_combo_image ,canny_image, cropped_image, combo_image , Hline_image )# , line_image
#cv2.namedWindow("combined_image", cv2.WINDOW_NORMAL)
#com_height, com_width, com_layers = combined_image.shape
#imS = cv2.resize(combined_image, window_size)
#cv2.imshow("combined_image",imS)
#cv2.imshow("result", combo_image)
# cv2.imshow("result", combined_image)
cv2.imshow("result", cropped_combo_image)
# end line detection
#cv2.imshow("ZED", mat.get_data())
key = cv2.waitKey(1)
saving_image(key, mat)
# print(int(cam.get_svo_number_of_frames()//2))
# cam.set_svo_position(10000)
else:
key = 113
else:
key = cv2.waitKey(1)
# i= i+1
#measure the time it takes to process one frame
print("--- %s seconds ---" % (time.time() - frame_processing_time))
frame_processing_time = time.time()
cv2.destroyAllWindows()
print_camera_information(cam)
cam.close()
print("\nFINISH")
