def main():
# Create a ZEDCamera object
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60)
# Use a right-handed Y-up coordinate system
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Enable positional tracking with default parameters.
# Positional tracking needs to be enabled before using spatial mapping
py_transform = sl.Transform()
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
err = zed.enable_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Enable spatial mapping
mapping_parameters = sl.SpatialMappingParameters(map_type=sl.SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD)
err = zed.enable_spatial_mapping(mapping_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Grab data during 3000 frames
i = 0
py_fpc = sl.FusedPointCloud() # Create a Mesh object
runtime_parameters = sl.RuntimeParameters()
while i < 3000:
# For each new grab, mesh data is updated
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh
mapping_state = zed.get_spatial_mapping_state()
print("\rImages captured: {0} / 3000 || {1}".format(i, mapping_state))
i = i + 1
print("\n")
# Extract, filter and save the mesh in an obj file
print("Extracting Point Cloud...\n")
err = zed.extract_whole_spatial_map(py_fpc)
print(repr(err))
#print("Filtering Mesh...\n")
#py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces)
print("Saving Point Cloud...\n")
py_fpc.save("fpc.obj")
# Disable tracking and mapping and close the camera
zed.disable_spatial_mapping()
zed.disable_tracking()
zed.close()
def main():
init = sl.InitParameters(camera_resolution=sl.RESOLUTION.RESOLUTION_HD720,
depth_mode=sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE,
coordinate_units=sl.UNIT.UNIT_METER,
coordinate_system=sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP,
sdk_verbose=True)
cam = sl.Camera()
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
transform = sl.Transform()
tracking_params = sl.TrackingParameters(transform)
cam.enable_tracking(tracking_params)
runtime = sl.RuntimeParameters()
camera_pose = sl.Pose()
viewer = tv.PyTrackingViewer()
viewer.init()
py_translation = sl.Translation()
start_zed(cam, runtime, camera_pose, viewer, py_translation)
viewer.exit()
glutMainLoop()
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()
#init = sl.InitParameters(svo_input_filename=filepath)
init = sl.InitParameters()
#new
init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default
# Use a right-handed Y-up coordinate system
init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
init.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
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.TrackingParameters(transform)
cam.enable_tracking(tracking)
cam.enable_spatial_mapping(spatial)
pymesh = sl.Mesh()
print("Processing...")
#for i in range(200):
while True:
try:
cam.grab(runtime)
cam.request_mesh_async()
except KeyboardInterrupt:
cam.extract_whole_mesh(pymesh)
cam.disable_tracking()
cam.disable_spatial_mapping()
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH)
print("Filtering params : {0}.".format(
pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(
sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_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")
raise
def main():
cam = sl.Camera()
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
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.SENSING_MODE_STANDARD
runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD
spatial = sl.SpatialMappingParameters()
transform = sl.Transform()
tracking = sl.TrackingParameters(transform)
cam.enable_tracking(tracking)
pymesh = sl.Mesh()
pyplane = sl.Plane()
reset_tracking_floor_frame = sl.Transform()
found = 0
print("Processing...")
i = 0
while i < 1000:
if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS:
err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame)
if i > 200 and err == sl.ERROR_CODE.SUCCESS:
found = 1
print('Floor found!')
pymesh = pyplane.extract_mesh()
break
i += 1
if found == 0:
print('Floor was not found, please try with another SVO.')
cam.close()
exit(0)
cam.disable_tracking()
save_mesh(pymesh)
cam.close()
print("\nFINISH")
def test():
zed = sl.Camera()
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60)
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP # Use a right-handed Y-up coordinate system
init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
zed.open(init_params)
# Configure spatial mapping parameters
mapping_parameters = sl.SpatialMappingParameters(
sl.MAPPING_RESOLUTION.MAPPING_RESOLUTION_LOW,
sl.MAPPING_RANGE.MAPPING_RANGE_FAR)
mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_MESH
mapping_parameters.save_texture = True
filter_params = sl.MeshFilterParameters(
) # not available for fused point cloud
filter_params.set(
sl.MESH_FILTER.MESH_FILTER_LOW) # not available for fused point cloud
# Enable tracking and mapping
py_transform = sl.Transform()
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
zed.enable_tracking(tracking_parameters)
zed.enable_spatial_mapping(mapping_parameters)
mesh = sl.Mesh() # Create a mesh object
timer = 0
runtime_parameters = sl.RuntimeParameters()
print("Getting Frames...")
# Grab 500 frames and stop
while timer < 500:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# When grab() = SUCCESS, a new image, depth and pose is available.
# Spatial mapping automatically ingests the new data to build the mesh.
timer += 1
print("Saving...")
# Retrieve the spatial map
zed.extract_whole_spatial_map(mesh)
# Filter the mesh
mesh.filter(filter_params) # not available for fused point cloud
# Apply the texture
mesh.apply_texture() # not available for fused point cloud
# Save the mesh in .obj format
mesh.save("mesh.obj")
print("Done")
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()
init = sl.InitParameters(svo_input_filename=filepath)
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.TrackingParameters(transform)
cam.enable_tracking(tracking)
cam.enable_spatial_mapping(spatial)
pymesh = sl.Mesh()
print("Processing...")
for i in range(200):
cam.grab(runtime)
cam.request_mesh_async()
cam.extract_whole_mesh(pymesh)
cam.disable_tracking()
cam.disable_spatial_mapping()
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH)
print("Filtering params : {0}.".format(pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(
sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_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 __init__(self):
self.maps = list()
#self.map = []
self.mesh = sl.Mesh()
print("Finding ZED...")
self.zed = sl.Camera()
self.translation = [0, 0, 0]
self.orientation = [0, 0, 0, 0]
self.init_params = sl.InitParameters()
py_transform = sl.Transform()
trackparms = sl.TrackingParameters(init_pos=py_transform)
trackparms.enable_pose_smoothing = True
self.tracking_params = trackparms
self.mapping_params = sl.SpatialMappingParameters(
resolution=sl.MAPPING_RESOLUTION.MAPPING_RESOLUTION_LOW,
mapping_range=sl.MAPPING_RANGE.MAPPING_RANGE_FAR,
save_texture=True)
self.filter_params = sl.MeshFilterParameters()
self.runtime_params = sl.RuntimeParameters()
print("Starting ZED...")
self.start_camera()
self.update_pose()
self.has_requested_map = False
self.last_update_time = time.time()
def main():
# 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.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60)
# Use a right-handed Y-up coordinate system
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Enable positional tracking with default parameters
py_transform = sl.Transform() # First create a Transform object for TrackingParameters object
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
err = zed.enable_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Track the camera position during 1000 frames
i = 0
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
#added!
path = '/media/nvidia/SD1/position.csv'
position_file = open(path,'w')
while i < 1000:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Get the pose of the left eye of the camera with reference to the world frame
zed.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
zed.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE)
# Display the translation and timestamp
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)
position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp))
# Display the orientation quaternion
py_orientation = sl.Orientation()
ox = round(zed_pose.get_orientation(py_orientation).get()[0], 3)
oy = round(zed_pose.get_orientation(py_orientation).get()[1], 3)
oz = round(zed_pose.get_orientation(py_orientation).get()[2], 3)
ow = round(zed_pose.get_orientation(py_orientation).get()[3], 3)
position_file.write("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow))
# Display the Rotation Matrix
py_rotationMatrix = zed_pose.get_rotation_matrix()
position_file.write("Got Rotation Matrix, but did not print\n")
# Display the Rotation Vector
py_rotationVector = zed_pose.get_rotation_vector()
rx = round(py_rotationVector[0], 3)
ry = round(py_rotationVector[1], 3)
rz = round(py_rotationVector[2], 3)
position_file.write("Rotation Vector: Rx: {0}, Ry: {1}, Rz {2}, Timestamp: {3}\n".format(rx, ry, rz, zed_pose.timestamp))
# Display the Euler Angles
py_eulerAngles = zed_pose.get_euler_angles()
ex = round(py_eulerAngles[0], 3)
ey = round(py_eulerAngles[1], 3)
ez = round(py_eulerAngles[2], 3)
position_file.write("EulerAngles: EAx: {0}, EAy: {1}, EAz {2}, Timestamp: {3}\n".format(ex, ey, ez, zed_pose.timestamp))
i = i + 1
# Close the camera
zed.close()
# Close file
position_file.close()
def main():
imu = PX4Data()
# Create a Camera object
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_VGA # Use HD1080 video mode
init_params.camera_fps = 120 # Set fps at 60
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD
init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Enable positional tracking with default parameters
py_transform = sl.Transform() # First create a Transform object for TrackingParameters object
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
err = zed.enable_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Capture 50 frames and stop
i = 0
image = sl.Mat()
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode
prevTimeStamp = 0
file = open('data/data.txt', 'w')
key = 0
depth = sl.Mat()
point_cloud = sl.Mat()
pcList = []
while key != 113:
# 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
timestamp = zed.get_timestamp(sl.TIME_REFERENCE.TIME_REFERENCE_CURRENT) # Get the timestamp at the time the image was
dt = (timestamp - prevTimeStamp) * 1.0 / 10 ** 9
if dt > 0.03:
# Get the pose of the left eye of the camera with reference to the world frame
zed.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
# Display the translation and timestamp
py_translation = sl.Translation()
gnd_pos = zed_pose.get_translation(py_translation).get()
tx = round(gnd_pos[0], 3)
ty = round(gnd_pos[1], 3)
tz = round(gnd_pos[2], 3)
print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp))
# Display the orientation quaternion
py_orientation = sl.Orientation()
quat = zed_pose.get_orientation(py_orientation).get()
ox = round(quat[0], 3)
oy = round(quat[1], 3)
oz = round(quat[2], 3)
ow = round(quat[3], 3)
print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow))
zed.retrieve_image(image, sl.VIEW.VIEW_LEFT)
img = image.get_data()
cv2.imwrite('data/images/' + str(timestamp) + '.png', img)
zed.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA)
print(point_cloud.get_data().shape)
pc = np.reshape(point_cloud.get_data(), (1, 376, 672, 4))
pcList.append(pc)
cv2.imshow("ZED", img)
key = cv2.waitKey(1)
prevTimeStamp = timestamp
print(dt)
print("Image resolution: {0} x {1} || Image timestamp: {2}\n".format(image.get_width(), image.get_height(), timestamp))
file.write('%d '
'%.4f %.4f %.4f '
'%.4f %.4f %.4f %.4f '
'%.4f %.4f %.4f '
'%.4f %.4f %.4f '
'%.4f %.4f %.4f '
'%.4f %.4f %.4f '
'%.4f %.4f %.4f %.4f \n' % (timestamp, tx, ty, tz, ox, oy, oz, ow,
imu.acc.x, imu.acc.y, imu.acc.z,
imu.gyr.x, imu.gyr.y, imu.gyr.z,
imu.gps.x, imu.gps.y, imu.gps.z,
imu.vel.x, imu.vel.y, imu.vel.z,
imu.quat.x, imu.quat.y, imu.quat.z, imu.quat.w))
i = i + 1
# Close the camera
pc = np.concatenate(pcList, axis=0)
np.save('pc', pc)
zed.close()
file.close()
imu.close()
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()
#init = sl.InitParameters(svo_input_filename=filepath)
init = sl.InitParameters()
#new
init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default
# Use a right-handed Y-up coordinate system
init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
init.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
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.TrackingParameters(transform)
cam.enable_tracking(tracking)
cam.enable_spatial_mapping(spatial)
#from Positional Tracking:
# Track the camera position until Keyboard Interupt (ctrl-C)
#i = 0
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
path = '/media/nvidia/SD1/translation.csv'
position_file = open(path, 'w')
#END from positional tracking
pymesh = sl.Mesh()
print("Processing...")
#for i in range(200):
while True:
try:
cam.grab(runtime)
cam.request_mesh_async()
# Get the pose of the left eye of the camera with reference to the world frame
cam.get_position(zed_pose,
sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE)
# Display the translation and timestamp
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)
#position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp))
position_file.write("{0},{1},{2},{3}\n".format(
tx, ty, tz, zed_pose.timestamp))
except KeyboardInterrupt:
cam.extract_whole_mesh(pymesh)
cam.disable_tracking()
cam.disable_spatial_mapping()
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH)
print("Filtering params : {0}.".format(
pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(
sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA)
print("Applying texture : {0}.".format(apply_texture))
#print_mesh_information(pymesh, apply_texture)
#save_filter(filter_params)
#save_mesh(pymesh)
cam.close()
position_file.close()
#save_position(path)
save_all(filter_params, pymesh, path)
print("\nFINISH")
raise
def initProcessing(plane, transform):
print("Init spatial mapping and camera pose")
spatial_parameters = sl.SpatialMappingParameters()
zed.enable_spatial_mapping(spatial_parameters)
tracking_parameters = sl.TrackingParameters(transform)
zed.enable_tracking(tracking_parameters)
def configure_zed_camera(img_capture=True, svo_file=None):
'''
Configure zed camera according to the capturing mode.
- When capturing images, it will be configured in 2k@15fps + DEPTH_MODE_ULTRA.
Also position tracking/spatial mapping will be enabled to store camera's parameters.
- When capturing videos, it will be configured in 1080HD@30fps + DEPTH_MODE_NONE.
:param img_capture: Flag of the image capturing mode. Default is True. When set False, it will configured as video capturing mode.
:param svo_file: When given as a .svo file, it will configured as reading file mode.
:return: configured zed camera object, runtime parameters
'''
# Create a Camera object
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER
# init_params.coordinate_system=sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP,
init_params.camera_disable_self_calib = False
init_params.depth_minimum_distance = 0.5 #in meter
init_params.enable_right_side_measure = True
init_params.sdk_verbose = True
if not img_capture:
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080
init_params.camera_fps = 30
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE
else:
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD2K
init_params.camera_fps = 15
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE
if svo_file is not None:
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA
init_params.svo_input_filename = str(svo_file)
init_params.svo_real_time_mode = False # Don't convert in realtime
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
if svo_file is not None:
# Positional tracking needs to be enabled before using spatial mapping
transform = sl.Transform()
tracking_parameters = sl.TrackingParameters(transform)
err = zed.enable_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(-1)
# Enable spatial mapping
mapping_parameters = sl.SpatialMappingParameters()
err = zed.enable_spatial_mapping(mapping_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(-1)
# Get camera information (ZED serial number)
zed_serial = zed.get_camera_information().serial_number
print("Hello! This is my serial number: {0}".format(zed_serial))
# Set runtime parameters after opening the camera
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD
runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD
return zed, runtime
def high_speed(args, viewer):
config = load_config(args)
dataset_type = config.get('dataset', 'type')
detection_thresh = config.getfloat('predict', 'detection_thresh')
min_num_keypoints = config.getint('predict', 'min_num_keypoints')
model = create_model(args, config)
svo_file_path = None #"/home/adujardin/Downloads/5m.svo" #config.get('zed', 'svo_file_path')
init_cap_params = sl.InitParameters()
if svo_file_path:
print("Loading SVO file " + svo_file_path)
init_cap_params.svo_input_filename = svo_file_path
init_cap_params.svo_real_time_mode = True
init_cap_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720
init_cap_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA
init_cap_params.coordinate_units = sl.UNIT.UNIT_METER
init_cap_params.depth_stabilization = True
init_cap_params.coordinate_system = sl.COORDINATE_SYSTEM.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()
py_transform = sl.Transform()
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
cap.enable_tracking(tracking_parameters)
capture = Capture(cap, model.insize)
predictor = Predictor(model=model, cap=capture)
capture.start()
predictor.start()
fps_time = 0
main_event = threading.Event()
viewer.edges = model.edges
try:
while not main_event.is_set() and cap.is_opened():
try:
image, feature_map, depth = predictor.get()
humans = get_humans_by_feature(model, feature_map,
detection_thresh,
min_num_keypoints)
humans_3d = get_humans3d(humans, depth, model)
except queue.Empty:
continue
except Exception as e:
print(e)
break
pilImg = Image.fromarray(image)
pilImg = draw_humans(
model.keypoint_names,
model.edges,
pilImg,
humans,
None,
visbbox=config.getboolean('predict', 'visbbox'),
)
img_with_humans = cv2.cvtColor(np.asarray(pilImg),
cv2.COLOR_RGB2BGR)
img_with_humans = cv2.resize(
img_with_humans,
(700, 400)) #(3 * model.insize[0], 3 * model.insize[1]))
msg = 'GPU ON' if chainer.backends.cuda.available else 'GPU OFF'
msg += ' ' + config.get('model_param', 'model_name')
fps_display = 'FPS: %f' % (1.0 / (time.time() - fps_time))
str_to_dsplay = msg + " " + fps_display
cv2.putText(img_with_humans, fps_display, (10, 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.imshow('Pose Proposal Network' + msg, img_with_humans)
viewer.update_text(str_to_dsplay)
viewer.update_humans(humans_3d)
fps_time = time.time()
key = cv2.waitKey(1)
# press Esc to exit
if key == 27:
exit
main_event.set()
except Exception as e:
print(e)
except KeyboardInterrupt:
main_event.set()
capture.stop()
predictor.stop()
capture.join()
predictor.join()
cap.close()
def main():
if len(sys.argv) != 3:
print(
"Please specify collection time (seconds), and path to save files")
exit()
max_time = sys.argv[1]
print(max_time)
path = sys.argv[2]
print(path)
#delay program 60 sec, so that user can get to start location
print(
"\nYou have 60 seconds to get to start location before program will begin"
)
time.sleep(60)
print("\nInitializing camera")
cam = sl.Camera()
init = sl.InitParameters()
# Use HD720 video mode (default Use a right-handed Y-up coordinate system)
init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720
init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
# Set units in meters
init.coordinate_units = sl.UNIT.UNIT_METER
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.TrackingParameters(transform)
cam.enable_tracking(tracking)
cam.enable_spatial_mapping(spatial)
#for Positional Tracking:
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
#temporary file to save translation data to, until you know the filename (given from the user)
#path = '/media/nvidia/SD1/translation.csv'
position_file = open((path + ".csv"), 'w+')
pymesh = sl.Mesh()
print("Camera setup")
#get start time
start_time = time.time()
print("Starting to collect data")
while (time.time() - start_time) < float(max_time):
cam.grab(runtime)
cam.request_mesh_async()
# Get the pose of the left eye of the camera with reference to the world frame
cam.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE)
# Display the translation and timestamp
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)
#position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp))
position_file.write("{0},{1},{2},{3}\n".format(tx, ty, tz,
zed_pose.timestamp))
print(
"Finished collecting data, extracting mesh, saving and shutting down camera"
)
cam.extract_whole_mesh(pymesh)
cam.disable_tracking()
cam.disable_spatial_mapping()
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH)
print("Filtering params : {0}.".format(pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(
sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA)
print("Applying texture : {0}.".format(apply_texture))
#print_mesh_information(pymesh, apply_texture)
#save_filter(filter_params)
#save_mesh(pymesh)
cam.close()
position_file.close()
#save_position(path)
save_all_path_arg(filter_params, pymesh, path)
print("\nFINISH")
def main(abs_k=None, depth=True, throttle_SP=None, steer_SP=None):
# Create a Camera object
zed = sl.Camera()
# Initilialize Camera Parameters
init_params = init_zed(depth)
# Open the camera
zed_cam = zed.open(init_params)
if zed_cam != sl.ERROR_CODE.SUCCESS:
print("Error: Zed Camera could not be opened.")
exit()
print("")
print("Zed Camera succesfully opened.")
# Enable positional tracking
if depth:
transform = sl.Transform()
tracking_parameters = sl.TrackingParameters(init_pos=transform)
tracking = zed.enable_tracking(tracking_parameters)
if tracking != sl.ERROR_CODE.SUCCESS:
exit()
print("")
print("Tracking succesfully enabled.")
runtime_parameters = sl.RuntimeParameters()
zed_pose = sl.Pose() # Getting Pose
mat = sl.Mat()
with open('/media/ctuxavier/ADATASE730H/zed_recording.csv',
mode='w') as csv_file: # Open csv file
print("Start Recording")
print("Press Ctrl+C to stop")
fieldnames = ['Tx', 'Ty', 'Tz', 'Pitch', 'Roll', 'Yaw', 'Timestamp']
writer = csv.DictWriter(csv_file, fieldnames=fieldnames)
writer.writeheader()
with open('/media/ctuxavier/ADATASE730H/img_data.txt', 'w') as data_file:
data_file.write("grab_timestamp, write_timestamp\n")
threads = []
# Init and start image acquisition thread
ImageAcquisition_t = Thread(target=ImageAcquisition,
args=(zed, mat, runtime_parameters))
ImageAcquisition_t.daemon = True
threads.append(ImageAcquisition_t)
ImageAcquisition_t.start()
if depth:
# Init and start data acquisition thread
DataAcquisiton_t = Thread(target=DataAcquisiton,
args=(zed, zed_pose, runtime_parameters))
DataAcquisiton_t.daemon = True
threads.append(DataAcquisiton_t)
DataAcquisiton_t.start()
# start car detection thread by Pavel Jahoda - for autonomous car chasing
if carChase:
car_detection_t = Thread(target=CarDetection,
args=(throttle_SP, steer_SP))
threads.append(car_detection_t)
car_detection_t.start()
# Init and start image segmentation thread
image_segmentation_t = Thread(target=image_segmentation, args=(abs_k, ))
image_segmentation_t.daemon = True
threads.append(image_segmentation)
image_segmentation_t.start()
try:
for thread in threads:
thread.join()
except (KeyboardInterrupt, SystemExit):
print("Zed data and images threads closed!")
def main():
# 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.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60)
# Use a right-handed Y-up coordinate system
init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP
init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Enable positional tracking with default parameters
py_transform = sl.Transform(
) # First create a Transform object for TrackingParameters object
tracking_parameters = sl.TrackingParameters(init_pos=py_transform)
err = zed.enable_tracking(tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Track the camera position during 1000 frames
i = 0
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
while i < 1000:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Get the pose of the left eye of the camera with reference to the world frame
zed.get_position(zed_pose,
sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
zed.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE)
# Display the translation and timestamp
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)
print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".
format(tx, ty, tz, zed_pose.timestamp))
# Display the orientation quaternion
py_orientation = sl.Orientation()
ox = round(zed_pose.get_orientation(py_orientation).get()[0], 3)
oy = round(zed_pose.get_orientation(py_orientation).get()[1], 3)
oz = round(zed_pose.get_orientation(py_orientation).get()[2], 3)
ow = round(zed_pose.get_orientation(py_orientation).get()[3], 3)
print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(
ox, oy, oz, ow))
#Display the IMU acceleratoin
acceleration = [0, 0, 0]
zed_imu.get_linear_acceleration(acceleration)
ax = round(acceleration[0], 3)
ay = round(acceleration[1], 3)
az = round(acceleration[2], 3)
print("IMU Acceleration: Ax: {0}, Ay: {1}, Az {2}\n".format(
ax, ay, az))
#Display the IMU angular velocity
a_velocity = [0, 0, 0]
zed_imu.get_angular_velocity(a_velocity)
vx = round(a_velocity[0], 3)
vy = round(a_velocity[1], 3)
vz = round(a_velocity[2], 3)
print("IMU Angular Velocity: Vx: {0}, Vy: {1}, Vz {2}\n".format(
vx, vy, vz))
# Display the IMU orientation quaternion
imu_orientation = sl.Orientation()
ox = round(zed_imu.get_orientation(imu_orientation).get()[0], 3)
oy = round(zed_imu.get_orientation(imu_orientation).get()[1], 3)
oz = round(zed_imu.get_orientation(imu_orientation).get()[2], 3)
ow = round(zed_imu.get_orientation(imu_orientation).get()[3], 3)
print(
"IMU Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(
ox, oy, oz, ow))
i = i + 1
# Close the camera
zed.close()
def main():
if not sys.argv or len(sys.argv) != 2:
print(
"Only the path of the output SVO file should be passed as argument."
)
exit(1)
cam = sl.Camera()
init = sl.InitParameters()
init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default
init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP # Use a right-handed Y-up coordinate system
init.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters
# new for SVO
init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE
##
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.TrackingParameters(transform)
cam.enable_tracking(tracking)
cam.enable_spatial_mapping(spatial)
#from Positional Tracking:
# Track the camera position until Keyboard Interupt (ctrl-C)
zed_pose = sl.Pose()
zed_imu = sl.IMUData()
runtime_parameters = sl.RuntimeParameters()
path = '/media/nvidia/SD1/translation.csv'
position_file = open(path, 'w')
#END from positional tracking
pymesh = sl.Mesh()
print("Processing...")
#new for SVO
path_output = sys.argv[1]
err = cam.enable_recording(
path_output, sl.SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_AVCHD)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit(1)
print("SVO is Recording, use Ctrl-C to stop.")
frames_recorded = 0
##
while True:
try:
cam.grab(runtime)
# new for SVO
state = cam.record()
if state["status"]:
frames_recorded += 1
print("Frame count: " + str(frames_recorded), end="\r")
##
cam.request_mesh_async()
# Get the pose of the left eye of the camera with reference to the world frame
cam.get_position(zed_pose,
sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD)
cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE)
# Display the translation and timestamp
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)
position_file.write("{0},{1},{2},{3}\n".format(
tx, ty, tz, zed_pose.timestamp))
except KeyboardInterrupt:
cam.extract_whole_mesh(pymesh)
cam.disable_tracking()
cam.disable_spatial_mapping()
# new for .svo
cam.disable_recording()
##
filter_params = sl.MeshFilterParameters()
filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH)
print("Filtering params : {0}.".format(
pymesh.filter(filter_params)))
apply_texture = pymesh.apply_texture(
sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA)
print("Applying texture : {0}.".format(apply_texture))
print_mesh_information(pymesh, apply_texture)
save_filter(filter_params)
save_mesh(pymesh)
cam.close()
position_file.close()
save_position(path)
print("\nFINISH")
raise
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()
