def main():
zed = sl.Camera()
point_cloud = sl.Mat()
# Set configuration parameters
input_type = sl.InputType()
input_type.set_from_svo_file('/home/ianmcvann/Documents/ZED/recent.svo')
init_params = sl.InitParameters(input_t=input_type,
svo_real_time_mode=False)
# init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.ULTRA # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.INCH # Use milliliter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.VGA
init_params.camera_fps = 100
init_params.depth_maximum_distance = 400
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(-1)
image = sl.Mat()
zed.set_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE, 15)
runtime_parameters = sl.RuntimeParameters()
while True:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
# A new image is available if grab() returns SUCCESS
zed.retrieve_image(image, sl.VIEW.RIGHT) # Retrieve the left image
frame = image.get_data()
process_pic(frame)
cv2.destroyAllWindows()
def main():
if len(sys.argv) != 2:
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
#init.depth_minimum_distance = 1
init.depth_mode = sl.DEPTH_MODE.ULTRA
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
mat = sl.Mat()
while cv2.waitKey(1) != ord("q"): # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
#cam.retrieve_image(mat)
cam.retrieve_image(mat, sl.VIEW.DEPTH)
cv2.imshow("ZED", mat.get_data())
cv2.destroyAllWindows()
cam.close()
print("\nFINISH")
def main():
if len(sys.argv) != 4:
exit()
filepath = sys.argv[1]
rgb = sys.argv[2]
depth = sys.argv[3]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
init.depth_mode = sl.DEPTH_MODE.ULTRA
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
mat_img = sl.Mat()
mat_depth = sl.Mat()
fr = 0
cont = 0
while cv2.waitKey(1) != ord("q"): # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
if cont == 2:
cam.retrieve_image(mat_img)
cam.retrieve_image(mat_depth, sl.VIEW.DEPTH)
im = mat_img.get_data()
d = mat_depth.get_data()
cv2.imshow("ZED", mat_img.get_data())
cv2.imwrite(
os.path.join(
rgb,
filepath.split('/')[-1].split('.')[0] + '-' + str(fr))
+ ".png", im)
cv2.imwrite(
os.path.join(
depth,
filepath.split('/')[-1].split('.')[0] + '-' + str(fr))
+ ".png", d)
fr = fr + 1
cont = 0
else:
cont = cont + 1
cv2.destroyAllWindows()
cam.close()
print("\nFINISH")
def main():
print("Running...")
input_type = sl.InputType()
input_type.set_from_camera_id(0)
init = sl.InitParameters()
init.input = input_type
init.camera_resolution = sl.RESOLUTION.HD720
#init.camera_linux_id = 0
init.camera_fps = 30 # 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(repr(status))
exit()
input_type.set_from_camera_id(1)
init.input = input_type
cam2 = sl.Camera()
if not cam2.is_opened():
print("Opening ZED Camera 2...")
status = cam2.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
mat = sl.Mat()
mat2 = sl.Mat()
print_camera_information(cam)
print_camera_information(cam2)
key = ''
while key != 113: # for 'q' key
# The computation could also be done in a thread, one for each camera
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat, sl.VIEW.LEFT)
cv2.imshow("ZED 1", mat.get_data())
err = cam2.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam2.retrieve_image(mat2, sl.VIEW.LEFT)
cv2.imshow("ZED 2", mat2.get_data())
key = cv2.waitKey(5)
cv2.destroyAllWindows()
cam.close()
print("\nFINISH")
def start_read_svo(filepath):
for file in os.listdir(filepath):
if not file.endswith(".svo"):
continue
file = filepath + file
print(file)
print(file.rsplit(".", 1)[0])
input_type = sl.InputType()
input_type.set_from_svo_file(file)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
init.depth_mode = sl.DEPTH_MODE.ULTRA
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
# runtime.sensing_mode = sl.SENSING_MODE.STANDARD
mat = sl.Mat()
key = ''
i = 0
print(" Save the current image: s")
print(" Quit the video reading: q\n")
while key != 113: # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat)
rgb = mat.get_data()
# cv2.imshow("RGB", rgb)
depthimg = get_depth_img(cam, mat)
# cv2.imshow("Dep", depth)
key = cv2.waitKey(1)
if i>cam.get_svo_number_of_frames()*0.1\
and i<cam.get_svo_number_of_frames()*0.75:
t = str(time.time()).split(".")[0]
cv2.imwrite(
file.rsplit(".", 1)[0] + "_rgb_" + str(i) + "_" + t +
".png", rgb)
cv2.imwrite(
file.rsplit(".", 1)[0] + "_depth_" + str(i) + "_" + t +
".jpg", depthimg)
i += 1
else:
break
cv2.destroyAllWindows()
print_camera_information(cam)
cam.close()
print("\nFINISH")
def main():
if len(sys.argv) != 2:
print("Please specify path to .svo file.")
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
init.camera_resolution = sl.RESOLUTION.HD720
init.camera_resolution = sl.RESOLUTION.HD1080
init.depth_mode = sl.DEPTH_MODE.ULTRA
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
mat = sl.Mat()
key = ''
print(" Save the current image: s")
print(" Quit the video reading: q\n")
while key != 113: # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat)
cv2.imshow("RGB", mat.get_data())
# cam.retrieve_image(mat, sl.VIEW.DEPTH)
depthimg = get_depth_img(cam, mat)
cv2.imshow("Dep", depthimg)
key = cv2.waitKey(1)
saving_image(key, mat)
if key == 48:
while True:
key = cv2.waitKey(1)
if key == 49:
break
else:
key = cv2.waitKey(1)
cv2.destroyAllWindows()
print_camera_information(cam)
cam.close()
print("\nFINISH")
def main():
zed = sl.Camera()
# Set configuration parameters
input_type = sl.InputType()
if len(sys.argv) >= 2:
input_type.set_from_svo_file(sys.argv[1])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
init.coordinate_units = sl.UNIT.METER
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
zed.close()
exit(1)
# Prepare new image size to retrieve half-resolution images
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width / 4
image_size.height = image_size.height / 4
# set video codec
codec = cv2.VideoWriter_fourcc(*'DIVX')
out = cv2.VideoWriter('selly_vison3.mp4', codec, 30.0, (480, 270))
# Declare your sl.Mat matrices
image_zed = sl.Mat()
while True:
start_time = time.time()
err = zed.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
image_frame = image_zed.get_data()[:, :, :3]
start = time.time()
cv2.imshow("Image2", image_frame)
out.write(image_frame)
if cv2.waitKey(1) & 0xFF == 32:
break
print("fps : ", round(1 / (time.time() - start_time), 2))
out.release()
cv2.destroyAllWindows()
zed.close()
print("\nFINISH")
def main():
if len(sys.argv) != 2:
print("Please specify path to .svo file.")
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
cam = sl.Camera()
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters()
init.input = input_type
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
spatial = sl.SpatialMappingParameters()
transform = sl.Transform()
tracking = sl.PositionalTrackingParameters(transform)
cam.enable_positional_tracking(tracking)
cam.enable_spatial_mapping(spatial)
pymesh = sl.Mesh()
print("Processing...")
for i in range(200):
cam.grab(runtime)
cam.request_spatial_map_async()
cam.extract_whole_spatial_map(pymesh)
cam.disable_positional_tracking()
cam.disable_spatial_mapping()
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.HIGH)
print("Filtering params : {0}.".format(pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA)
print("Applying texture : {0}.".format(apply_texture))
print_mesh_information(pymesh, apply_texture)
save_filter(filter_params)
save_mesh(pymesh)
cam.close()
print("\nFINISH")
def main():
if len(sys.argv) != 2:
print("Please specify path to .svo file.")
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
# Declare sl.Mat matrices
image_size = cam.get_camera_information().camera_resolution
image_size.width = image_size.width / 2
image_size.height = image_size.height / 2
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth_image_zed = sl.Mat(image_size.width, image_size.height,
sl.MAT_TYPE.U8_C4)
key = ''
print(" Save the current image: s")
print(" Quit the video reading: q\n")
while key != 113: # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(image_zed, resolution=image_size)
cv2.imshow("ZED Image", image_zed.get_data())
cam.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU,
image_size)
cv2.imshow("ZED Depth", depth_image_zed.get_data())
key = cv2.waitKey(1)
saving_image(key, image_zed)
else:
key = cv2.waitKey(1)
cv2.destroyAllWindows()
cam.close()
print("\nFINISH")
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 main():
if len(sys.argv) != 3:
exit()
filepath = sys.argv[1]
outputURL = sys.argv[2]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
mat = sl.Mat()
h = 720
w = 1280
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter(outputURL, fourcc, 20.0, (w, h))
while cv2.waitKey(1) != ord("q"): # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat)
im = mat.get_data()
out.write(im)
cv2.imshow("frame", im)
cam.close()
out.release()
cv2.destroyAllWindows()
print("\nFINISH")
def main():
if len(sys.argv) != 2:
print("Please specify path to .svo file.")
exit()
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
input_type = sl.InputType()
input_type.set_from_svo_file(filepath)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
runtime = sl.RuntimeParameters()
mat = sl.Mat()
key = ''
print(" Save the current image: s")
print(" Quit the video reading: q\n")
while key != 113: # for 'q' key
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat)
cv2.imshow("ZED", mat.get_data())
key = cv2.waitKey(1)
saving_image(key, mat)
else:
key = cv2.waitKey(1)
cv2.destroyAllWindows()
print_camera_information(cam)
cam.close()
print("\nFINISH")
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():
# Prepare ZED camera only
# For orther camera can ignore this section
zed = sl.Camera()
input_type = sl.InputType()
if len(sys.argv) >= 2:
input_type.set_from_svo_file(sys.argv[1])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
zed.close()
exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width / 2
image_size.height = image_size.height / 2
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
key = ' '
# End ZED setup
def edge_detect(hr):
canned = []
if 1:
for hr_i in hr:
canned.append(canny(hr_i.unsqueeze(0).half(), use_cuda=True))
canned = torch.cat(canned, dim=0).detach().float()
EDGE_SIZE = 1
canned[:, :, :EDGE_SIZE, :] = 0
canned[:, :, -EDGE_SIZE:, :] = 0
canned[:, :, :, :EDGE_SIZE] = 0
canned[:, :, :, -EDGE_SIZE:] = 0
return canned
def customTransform(input, channel=3):
im = np.array([input])
if channel != 3:
im = im[:, :, :, :channel]
im = np.transpose(im, (0, 3, 1, 2))
im = np.array(im, dtype=np.float32) / 255.0
im = torch.FloatTensor(im)
im = im.cuda()
return im
def show_img(ims, wait=0, names=['im']):
for im, name in zip(ims, names):
try:
temp = im[0].cpu().numpy()
except:
temp = im[0].cpu().detach().numpy()
temp = np.clip(np.transpose(temp, axes=(1, 2, 0)), 0, 1.0) * 255
temp = temp.astype(np.uint8)
cv2.imshow(name, temp)
cv2.waitKey(wait)
def rescale_img(img_in, scale):
img_in = img_in[:, :, :3]
frame_HR = cv2.resize(img_in, (640, 360))
frame_LR = cv2.resize(frame_HR, (320, 180))
Show_LR = cv2.resize(frame_LR, (640, 360))
return frame_HR, frame_LR, Show_LR
model = RCARN(
num_channels=3,
base_filter=256,
feat=3, #64
num_stages=3,
n_resblock=5,
nFrames=4,
scale_factor=4).to(device)
model_c = './checkpoints/EPCARN.pth'
model.load_state_dict(
torch.load(model_c, map_location=lambda storage, loc: storage))
# Update...
# .........
while key != 113:
err = zed.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
image_ocv = image_zed.get_data()
HR, LR, Show_LR = rescale_img(image_ocv, 2)
#-------------------------------------------------------------
img_send_jpeg = cv2.imencode(".jpg", LR,
[cv2.IMWRITE_JPEG_QUALITY, 100])[1]
nparr = np.asarray(img_send_jpeg, np.uint8)
LR = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
#-------------------------------------------------------------
HR = customTransform(HR)
LR = customTransform(LR)
Show_LR = customTransform(Show_LR)
canned = edge_detect(HR)
prediction = model(LR, canned)
show_img([prediction, HR, Show_LR],
wait=1,
names=['sr', 'hr', 'lr'])
key = cv2.waitKey(10)
cv2.destroyAllWindows()
zed.close()
# cv2.imshow('result',res)
# cv2.waitKey(0)
return res
else:
print("Not enough matches are found - {}/{}".format(
len(good), MIN_MATCH_COUNT))
matchesMask = None
width = 704 #1056
height = 416 #624
exit_signal = False
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
input_type = sl.InputType()
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
status = zed.open(init_params)
print(status)
time.sleep(1)
left_image_mat = sl.Mat()
# right_image_mat = sl.Mat()
def main() :
# construct the argument parser and parse command line arguments
ap = argparse.ArgumentParser()
ap.add_argument("-i", "--ip", type=str, required=True,
help="ip address of the device")
ap.add_argument("-o", "--port", type=int, required=True,
help="ephemeral port number of the server (1024 to 65535)")
ap.add_argument("-f", "--frame-count", type=int, default=32,
help="# of frames used to construct the background model")
ap.add_argument("-s", "--source", type=str, default="cam", help="using 'cam'era or 'svo'<SVO file>?")
global args
args = vars(ap.parse_args())
# start the flask app
print("running flask.")
threading.Thread(target=flaskThread).start()
print("flask app running.")
# Create a ZED camera object
zed = sl.Camera()
# Set configuration parameters
input_type = sl.InputType()
if args["source"] != "cam" :
input_type.set_from_svo_file(args["source"])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init.coordinate_units = sl.UNIT.MILLIMETER
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS :
print(repr(err))
zed.close()
exit(1)
# Create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Bind the socket to the port
server_address = ('localhost', 10000)
print('starting up on {} port {}'.format(*server_address))
sock.bind(server_address)
# Listen for incoming connections
sock.listen(1)
# Display help in console
print_help()
# Set runtime parameters after opening the camera
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
# Prepare new image size to retrieve half-resolution images
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width /2
image_size.height = image_size.height /2
# Declare your sl.Mat matrices
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth_image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth = sl.Mat()
point_cloud = sl.Mat()
key = ' '
# Wait for a connection
print('waiting for a connection')
connection, client_address = sock.accept()
print('connection from ', client_address)
while key != 113 :
# Wait for request
# print('Waiting for request...')
data = connection.recv(16)
# print('received {!r}'.format(data))
err = zed.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS :
# Retrieve the left image, depth image in the half-resolution
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
zed.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU, image_size)
zed.retrieve_measure(depth, sl.MEASURE.DEPTH, sl.MEM.CPU, image_size)
# Retrieve the RGBA point cloud in half resolution
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, image_size)
# 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
image_ocv = image_zed.get_data()
depth_image_ocv = depth_image_zed.get_data()
depth_ocv = depth.get_data()
# Detecting wood from depth_image_zed
# Minimum_value def (y)
# Get minimum depth value and circle it
# Input: y colum number
# Return: Minimum depth value
n_min = 0
n=0
depth_value_min = 2500
while n < image_size.width :
x = n;
y = image_size.height // 2
depth_value = depth_ocv[y,x]
if (math.isnan(depth_value)==0) and (depth_value < depth_value_min) and (depth_value > 0):
n_min = n
depth_value_min = depth_value
n = n + 1
# print('Min value is at: ' + str(n_min) + '. Value is: ' + str(depth_value_min) + '.')
# n_min = 100
# cv2.circle( depth_image_ocv, ( n_min, image_size.height // 2 ), \
# 32, ( 0, 0, 255 ), 1, 8 )
# Get points within a threshold and circle them
# def (minimum_value)
threshold = depth_value_min + 40
n = 0
point_depth = np.array([])
point_x = np.array([])
while n < image_size.width :
x = n
y = image_size.height // 2
depth_value =depth_ocv[y,x]
if math.isnan(depth_value)==0 and depth_value < threshold :
point_depth = np.append(point_depth, depth_value)
point_x = np.append(point_x, x)
# cv2.circle( depth_image_ocv, ( x, image_size.height // 2), \
# 16, ( 0, 0, 255 ), 1, 8 )
n = n + 1
# print(str(len(point_x)) + 'points are within threshold 40.\n')
# print('Point within threshold:\n' + str(point_depth))
girder_center = int(np.mean(point_x))
# print('Girder ceter at: ' + str(girder_center))
# Sending TCP msg
connection.sendall(girder_center.to_bytes(2,'big'))
# Draw girder center
cv2.circle( depth_image_ocv, (int(girder_center), image_size.height // \
2), 8, (255, 0, 0), 2, 8 )
# Draw view center
cv2.circle(depth_image_ocv, (image_size.width //2, image_size.height //2), \
8, (0,255,0),2,8)
# Draw Text depth_value_min
# font
font = cv2.FONT_HERSHEY_SIMPLEX
# org
org = (int(girder_center), image_size.height //2)
# fontScale
fontScale = 1
# Blue color in BGR
color = (255, 0, 0)
# Line thickness of 2 px
thickness = 2
# Using cv2.putText() method
image = cv2.putText( depth_image_ocv, str(depth_value_min), org, font,
fontScale, color, thickness, cv2.LINE_AA)
# cv2.imshow("Image", image_ocv)
# cv2.imshow("Depth", depth_image_ocv)
# Send to webserver
global outputFrame
outputFrame = depth_image_ocv.copy()
key = cv2.waitKey(10)
process_key_event(zed, key)
cv2.destroyAllWindows()
zed.close()
# Clean up the connection
connection.close()
print("\nFINISH")
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")
def main() :
#default vars
error = False
AngleValue = 0
RotationValue = 0
xOffset = 0
xRes = 1920
middleOfRes = xRes/2
#vars
#Green Tone, change based on tone of light. MAY BE TOO WIDE CURRENTLY
lowerBound=np.array([33,80,40])
upperBound=np.array([102,255,255])
zed = sl.Camera()
input_type = sl.InputType()
if len(sys.argv) >= 2 :
input_type.set_from_svo_file(sys.argv[1])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init.coordinate_units = sl.UNIT.MILLIMETER
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS :
print(repr(err))
zed.close()
exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width / 2
image_size.height = image_size.height / 2
# Declare your sl.Mat matrices
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth_image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
point_cloud = sl.Mat()
while True:
err = zed.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS :
# Retrieve the left image, depth image in the half-resolution
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
zed.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU, image_size)
# Retrieve the RGBA point cloud in half resolution
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, image_size)
#LEFT IMAGE CONVERTED TO OPENCV READABLE FORMAT
image_ocv = image_zed.get_data()
depth_image_ocv = depth_image_zed.get_data()
#uncomment if u have a display
#cv2.imshow("Image", image_ocv)
#cv2.imshow("Depth", depth_image_ocv)
ret, image = image_ocv.read()
#not sure if resize function will work
frame = cv2.resize(image, (1920, 1080))
imgHSV = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
mask=cv2.inRange(imgHSV,lowerBound,upperBound)
maskOpen=cv2.morphologyEx(mask,cv2.MORPH_OPEN,kernelOpen)
maskClose=cv2.morphologyEx(maskOpen,cv2.MORPH_CLOSE,kernelClose)
maskFinal=maskClose
_, conts, _= cv2.findContours(maskFinal.copy(),cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
#RUN THIS IF GREEN IS FOUND
for i in range(len(conts)):
c = max(conts,key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
if radius > 5:
x,y,w,h=cv2.boundingRect(conts[i])
cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255), 2)
xcenter = (int(M["m10"] / M["m00"]))
print(xcenter)
xOffset = int(xcenter - middleOfRes)
print(xOffset)
print(x)
err, point_cloud_value = point_cloud.get_value(x, y)
distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[2] * point_cloud_value[2])
print(distance)
else:
break
else:
#send network tables error, Bryon's time to pop off
error = True
#close everything down if it fails, WHICH IT WON'T
cv2.destroyAllWindows()
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.FILL
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
#depth_ocvを正規化する
detect_ocv = np.clip(depth_ocv, 0, 10000)
depth_ocv_n = (detect_ocv + 1) / 10000 * 255
print(depth_ocv_n.max())
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_n[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, 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(depth_ocv_n, 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 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")
def extract_xyz(x_pixel_M1, y_pixel_M1, x_pixel_M2, y_pixel_M2, x_pixel_M3,
y_pixel_M3, outputfilename, svofilename):
x_world_M1 = []
y_world_M1 = []
z_world_M1 = []
x_world_M2 = []
y_world_M2 = []
z_world_M2 = []
x_world_M3 = []
y_world_M3 = []
z_world_M3 = []
# 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])
err_M3, point_cloud_value_M3 = point_cloud.get_value(
x_pixel_M3[i], y_pixel_M3[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
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)
x_world_M3.append(point_cloud_value_M3[0] +
lens2cage_edge_x_mm)
y_world_M3.append(-point_cloud_value_M3[1] +
lens2cage_edge_y_mm)
z_world_M3.append(point_cloud_value_M3[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, x_world_M3, y_world_M3, z_world_M3)
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():
# Create a ZED camera object
zed = sl.Camera()
# Set configuration parameters
input_type = sl.InputType()
if len(sys.argv) >= 2:
input_type.set_from_svo_file(sys.argv[1])
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD720
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init.coordinate_units = sl.UNIT.MILLIMETER
# init.depth_minimum_distance = 0.3
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
zed.close()
exit(1)
# Display help in console
print_help()
print(count_save)
# check for existing folders and number of previously captured frames
check_dir()
# Set runtime parameters after opening the camera
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.FILL
# Prepare new image size to retrieve half-resolution images
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width / 2
image_size.height = image_size.height / 2
# Declare your sl.Mat matrices
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth_image_zed = sl.Mat(image_size.width, image_size.height,
sl.MAT_TYPE.U8_C4)
point_cloud = sl.Mat()
key = ' '
while key != 113:
err = zed.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
# Retrieve the left image, depth image in the half-resolution
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
zed.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU,
image_size)
# Retrieve the RGBA point cloud in half resolution
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU,
image_size)
# 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
image_ocv = image_zed.get_data()
depth_image_ocv = depth_image_zed.get_data()
cv2.imshow("Image", image_ocv)
cv2.imshow("Depth", depth_image_ocv)
key = cv2.waitKey(10)
process_key_event(zed, key)
cv2.destroyAllWindows()
zed.close()
print("\nFINISH")
def zedGrabber(zedFramesQueue):
svo_path = "HD1080.svo"
zed_id = 0
init_mode = 0
input_type = sl.InputType()
if init_mode == 0:
input_type.set_from_svo_file(svo_path)
else:
input_type.set_from_camera_id(zed_id)
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
init.camera_fps = 30
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP
cam = sl.Camera()
if not cam.is_opened():
print("Opening ZED Camera...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
log.error(repr(status))
exit()
# Enable positional tracking with default parameters
py_transform = sl.Transform(
) # First create a Transform object for TrackingParameters object
tracking_parameters = sl.PositionalTrackingParameters(
init_pos=py_transform)
err = cam.enable_positional_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
zed_pose = sl.Pose()
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,
while True:
start_time = time.time() # start time of the loop
err = cam.grab(runtime)
if (err != sl.ERROR_CODE.SUCCESS):
break
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()
cam.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
py_translation = sl.Translation()
tx = round(zed_pose.get_translation(py_translation).get()[0], 3)
ty = round(zed_pose.get_translation(py_translation).get()[1], 3)
tz = round(zed_pose.get_translation(py_translation).get()[2], 3)
camPosition = (tx, ty, tz)
#print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}".format(tx, ty, tz, zed_pose.timestamp.get_milliseconds()))
camOrientation = zed_pose.get_rotation_vector() * 180 / math.pi
image = rotate(image, -camOrientation[1], center=None, scale=1.0)
FPS = 1.0 / (time.time() - start_time)
# Do the detection
#rawDetections = detect(netMain, metaMain, image, thresh)
#detectionsList = detectionsAnalayzer(rawDetections, depth)
frame = zedFrame(image, depth, camOrientation, camPosition, FPS)
zedFramesQueue.put(frame)
#image = cvDrawBoxes(detectionsList, image)
#image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
#image = cv2.resize(image,(1280,720),interpolation=cv2.INTER_LINEAR)
cam.close()
def main() :
# Create a ZED camera object
zed = sl.Camera()
# Set configuration parameters
input_type = sl.InputType()
init = sl.InitParameters(input_t=input_type)
init.camera_resolution = sl.RESOLUTION.HD1080
#init.camera_resolution = sl.RESOLUTION.HD720
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init.coordinate_units = sl.UNIT.MILLIMETER
#init.coordinate_units = sl.UNIT.METER
# Open the camera
err = zed.open(init)
if err != sl.ERROR_CODE.SUCCESS :
print(repr(err))
zed.close()
exit(1)
# Display help in console
print_help()
# Creating GUI
gui = ZED_GUI()
# Set runtime parameters after opening the camera
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
# Prepare new image size to retrieve half-resolution images
image_size = zed.get_camera_information().camera_resolution
image_size.width = image_size.width /2
image_size.height = image_size.height /2
# Declare your sl.Mat matrices
image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
depth_image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
point_cloud = sl.Mat()
sensors_data = sl.SensorsData()
key = ' '
event = ' '
while key != 113:
err = zed.grab(runtime)
event, values = gui.window.read(timeout=20)
if event == "Exit" or event == sg.WIN_CLOSED:
break
if event == "Capture":
try:
height = float(values["-HEIGHT-"])
width = float(values["-WIDTH-"])
length = float(values["-LENGTH-"])
no_cajas = float(values["-NO_CAJAS-"])
no_cajas_base = float(values["-NO_CAJAS_BASE-"])
no_cajas_alto = float(values["-NO_CAJAS_ALTO-"])
sg.popup_timed('Capturando data, no mover la cámara ni el objeto', title='Capturando data', auto_close_duration=5, non_blocking=True)
capture_data(zed, height, width, length, no_cajas, no_cajas_base, no_cajas_alto)
gui.window["-HEIGHT-"].update('')
gui.window["-WIDTH-"].update('')
gui.window["-LENGTH-"].update('')
gui.window["-NO_CAJAS-"].update('1')
gui.window["-NO_CAJAS_BASE-"].update('1')
gui.window["-NO_CAJAS_ALTO-"].update('1')
sg.popup_timed('Captura guardada correctamente. Listo para continuar', title='Captura exitosa', auto_close_duration=5)
except:
sg.popup_timed('Debe ingresar valor numérico en los tres campos. Decimales con punto.', title='Valores incorrectos', auto_close_duration=5)
pass
if err == sl.ERROR_CODE.SUCCESS :
# Retrieve the left image, depth image in the half-resolution
zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size)
zed.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU, image_size)
# capture sensors data
zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.IMAGE)
# 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
image_ocv = image_zed.get_data()
depth_image_ocv = depth_image_zed.get_data()
# concatenate both images to show them side by side in the GUI
sbs_image = np.concatenate((image_ocv, depth_image_ocv), axis=1)
imgbytes = cv2.imencode(".png", sbs_image)[1].tobytes()
gui.window["-IMAGE-"].update(data=imgbytes)
# show sensors data
quaternion, linear_acceleration, angular_velocity, magnetic_field_calibrated, atmospheric_pressure =\
get_data_from_sensors(sensors_data)
gui.window["-IMU_ORIENTATION-"].update(quaternion)
gui.window["-IMU_ACCELERATION-"].update(linear_acceleration)
gui.window["-IMU_ANG_VEL-"].update(angular_velocity)
gui.window["-IMU_MAG_FIELD-"].update(magnetic_field_calibrated)
gui.window["-IMU_ATM_PRESS-"].update(atmospheric_pressure)
key = cv2.waitKey(10)
cv2.destroyAllWindows()
zed.close()
gui.window.close()
print("\nFINISH")
def Video_To_Picture(self,video_path, images_path,sample_rate,begin_number=0,type=".jpg"):
"""
将视频变换成单张图片,存放图片的名称为(数字.jpg)
如果是.svo文件,需要进行一下配置
:param video_path: 视频路径
:param images_path: 存放图片路径
:param begin_number: 图片开始名称
:param stop_number: 一共转换图片数
:param type: 保存图片类型
:return:
"""
#1:确保有视频文件
assert os.path.exists(video_path),video_path+" 路径下没有视频,不能进行视频帧的转换"
assert os.path.exists(images_path),"图片保存路径"+images_path+"不存在,因此不能够进行文件保存"
#2:初始化导入参数
i=begin_number#用于计算转换视频
all_number=0#用于跳过采样频率
read_flag=False#用于确定是导入的问题还是读完的问题
#3:导入视频
#3.1:处理.svo文件
if video_path[-4:]==".svo":
import pyzed.sl as sl#如果是zed类型的才调用pyzed
input_type = sl.InputType()
input_type.set_from_svo_file(video_path)
init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False)
cam = sl.Camera()
status = cam.open(init)
if status!=sl.ERROR_CODE.SUCCESS:
print(".svo文件导入失败,错误原因为:",repr(status))
return
runtime=sl.RuntimeParameters()
sl_image=sl.Mat()
print("正在将.svo文件转换为图片")
while True:
err=cam.grab(runtime)
if err==sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(sl_image)
image=sl_image.get_data()[:,:,:3].copy()
image=image.astype(np.uint8)
all_number=all_number+1
if all_number%sample_rate!=0:
continue
image_filename=images_path+"/"+str(i)+type
cv.imwrite(image_filename,image)
read_flag=True
else:
if read_flag:
break
else:
print("导入.svo文件出错,错误码为:",err)
sys.stdout.write("\r当前已经处理了 {} 帧视频".format(str(i-begin_number))) # 为了进行单行输出
sys.stdout.flush()
i = i + 1
print(" ")
print("完成视频解析,文件夹下共有{}帧图片,存放的路径为:{}".format(str(i), images_path))
print("这个视频一共有:{}帧".format(all_number))
#3.2:处理其他视频文件
else:
cap = cv.VideoCapture(video_path)
print("正在将视频转换为图片")
while True:
ret, frame = cap.read()
if ret:
all_number=all_number+1
if all_number%sample_rate!=0:
continue#不同采样率
image_filename = images_path + "/" + str(i) +type
cv.imwrite(image_filename, frame)
read_flag = True
#避免视频读取完毕仍然在读取,同时确保是视频
else:
if read_flag:
break
else:
print("该文件不是视频")
#用于单行输出
sys.stdout.write("\r当前已经处理了 {} 帧视频".format(str(i-begin_number))) # 为了进行单行输出
sys.stdout.flush()
i = i + 1
print(" ")
print("完成视频解析,文件夹下共有{}帧图片,存放的路径为:{}".format(str(i), images_path))
print("这个视频一共有:{}帧".format(all_number))
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")
