def main():
cam = sl.Camera()
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.ULTRA
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
if len(sys.argv) == 2:
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
init.set_from_svo_file(filepath)
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD
spatial = sl.SpatialMappingParameters()
transform = sl.Transform()
tracking = sl.PositionalTrackingParameters(transform)
cam.enable_positional_tracking(tracking)
pymesh = sl.Mesh()
pyplane = sl.Plane()
reset_tracking_floor_frame = sl.Transform()
found = 0
print("Processing...")
i = 0
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_positional_tracking()
save_mesh(pymesh)
cam.close()
print("\nFINISH")
def run(cam, runtime, camera_pose, viewer, py_translation):
while True:
if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS:
tracking_state = cam.get_position(camera_pose)
text_translation = ""
text_rotation = ""
if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
rotation = camera_pose.get_rotation_vector()
rx = round(rotation[0], 2)
ry = round(rotation[1], 2)
rz = round(rotation[2], 2)
translation = camera_pose.get_translation(py_translation)
tx = round(translation.get()[0], 2)
ty = round(translation.get()[1], 2)
tz = round(translation.get()[2], 2)
text_translation = str((tx, ty, tz))
text_rotation = str((rx, ry, rz))
pose_data = camera_pose.pose_data(sl.Transform())
viewer.update_zed_position(pose_data)
viewer.update_text(text_translation, text_rotation, tracking_state)
else:
sl.c_sleep_ms(1)
def main():
init = sl.InitParameters(camera_resolution=sl.RESOLUTION.HD720,
depth_mode=sl.DEPTH_MODE.PERFORMANCE,
coordinate_units=sl.UNIT.METER,
coordinate_system=sl.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.PositionalTrackingParameters(transform)
cam.enable_positional_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 zed_init():
'''
##作者:左家乐
##日期：2020-08-01
##功能：Init the ZED
##IN-para : no
##return ： err()
'''
camera_settings = sl.VIDEO_SETTINGS.BRIGHTNESS
str_camera_settings = "BRIGHTNESS"
step_camera_settings = 1
print("3.Detected the ZED...")
global cam
cam = sl.Camera()
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.METER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.HD720
err = cam.open(init_params) #if failed to open zed ,return 0
global runtime_parameters
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # Use STANDARD sensing mode
runtime_parameters.confidence_threshold = 100
runtime_parameters.textureness_confidence_threshold = 100
global mat
mat = sl.Mat()
global depth
depth = sl.Mat()
global point_cloud
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75, 4.0, 0))
tr_np = mirror_ref.m
return err
def init_params(menu, cam_id=0):
global save_dir
zed = EasyDict({})
zed.cam = sl.Camera()
zed.mat = EasyDict({
'pose': sl.Pose(),
'translation': sl.Translation(),
'transform': sl.Transform(),
'image': sl.Mat(), # image_map
'depth': sl.Mat(), # depth_map
'point_cloud': sl.Mat(),
'sensors': sl.SensorsData() # sensors_data
})
zed.param = EasyDict({
'init':
sl.InitParameters(
camera_resolution=mode.resolution[menu.cam.resolution],
depth_mode=mode.depth[menu.mode.depth],
coordinate_units=mode.unit[menu.unit],
coordinate_system=mode.coordinate_system[menu.coordinate_system],
depth_minimum_distance=menu.depth_range.min,
depth_maximum_distance=menu.depth_range.max,
sdk_verbose=verbose),
'runtime':
sl.RuntimeParameters(sensing_mode=mode.sensing[menu.mode.sensing]),
'tracking':
sl.PositionalTrackingParameters(zed.mat.transform)
})
#######
zed.param.init.set_from_camera_id(cam_id)
save_dir = save_dir_fmt.format(cam_id)
return zed
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():
# Create a Camera object
zed = sl.Camera()
#init
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.MILLIMETER # Use milliliter units (for depth measurements)
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # Use STANDARD sensing mode
# Capture 50 images and depth, then stop
i = 0
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75, 4.0, 0))
tr_np = mirror_ref.m
while i < 50:
# 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.LEFT)
zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
x = round(image.get_width() / 2)
y = round(image.get_height() / 2)
print("width: ", x)
print("height: ", y)
err, point_cloud_value = point_cloud.get_value(x, y)
#distance formula
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):
distance = round(distance)
print("Distance to Camera at ({0}, {1}): {2} mm\n".format(
x, y, distance))
# Increment the loop
i = i + 1
else:
print("invalid\n")
sys.stdout.flush()
zed.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)
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 cal():
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.METER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.HD720
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # Use STANDARD sensing mode
# Setting the depth confidence parameters
runtime_parameters.confidence_threshold = 100
runtime_parameters.textureness_confidence_threshold = 100
# Capture 150 images and depth, then stop
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75,4.0,0))
tr_np = mirror_ref.m
while True:
# A new image is available if grab() returns SUCCESS
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(image, sl.VIEW.LEFT)
zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
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")
zed.close()
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()
def update(self):
dot_ = sl.Translation.dot_translation(self.vertical_, self.up_)
if (dot_ < 0.):
tmp = self.vertical_.get()
self.vertical_.init_vector(tmp[0] * -1., tmp[1] * -1.,
tmp[2] * -1.)
transformation = sl.Transform()
transformation.init_orientation_translation(self.orientation_,
self.position_)
transformation.inverse()
self.vpMatrix_ = self.projection_ * transformation
def __init__(self):
self.available = False
self.mutex = Lock()
self.camera = CameraGL()
self.wheelPosition = 0.
self.mouse_button = [False, False]
self.mouseCurrentPosition = [0., 0.]
self.previousMouseMotion = [0., 0.]
self.mouseMotion = [0., 0.]
self.pose = sl.Transform()
self.trackState = sl.POSITIONAL_TRACKING_STATE
self.txtT = ""
self.txtR = ""
def update(self):
dot_ = sl.Translation.dot_translation(self.vertical_, self.up_)
if(dot_ < 0.):
tmp = self.vertical_.get()
self.vertical_.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.)
transformation = sl.Transform()
tmp_position = self.position_.get()
tmp = (self.offset_ * self.orientation_).get()
new_position = sl.Translation()
new_position.init_vector(tmp_position[0] + tmp[0], tmp_position[1] + tmp[1], tmp_position[2] + tmp[2])
transformation.init_orientation_translation(self.orientation_, new_position)
transformation.inverse()
self.vpMatrix_ = self.projection_ * transformation
def main():
rospy.init_node("rosnode_zed")
signalRecieved = True
while not signalRecieved:
pass
plane = sl.Plane()
data = np.array([0, 0, 0, 0])
trasnform_matrix = sl.Matrix4f(data)
transform = sl.Transform(trasnform_matrix)
initProcessing(plane, transform)
while True:
grabFrames()
time.sleep(0.1)
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 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 __init__(self):
self.available = False
self.mutex = Lock()
self.draw_mesh = False
self.new_chunks = False
self.chunks_pushed = False
self.change_state = False
self.projection = sl.Matrix4f()
self.projection.set_identity()
self.znear = 0.5
self.zfar = 100.
self.image_handler = ImageHandler()
self.sub_maps = []
self.pose = sl.Transform().set_identity()
self.tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF
self.mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED
def __init__(self):
self.available = False
self.mutex = Lock()
self.projection = sl.Matrix4f()
self.projection.set_identity()
self.znear = 0.1
self.zfar = 100.
self.image_handler = ImageHandler()
self.pose = sl.Transform().set_identity()
self.tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF
self.mesh_object = MeshObject()
self.user_action = UserAction()
self.new_data = False
self.wnd_w = 0
self.wnd_h = 0
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 open_zed(self, event):
self.zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.QUALITY # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.METER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.VGA # 此处修改图像尺寸
init_params.camera_fps = 100
# Open the camera
err = self.zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # Use STANDARD sensing mode
# Setting the depth confidence parameters
runtime_parameters.confidence_threshold = 100
runtime_parameters.textureness_confidence_threshold = 100
# Capture 150 images and depth, then stop
i = 0
self.image = sl.Mat()
# cv2.imwrite("i1.jpg", MatrixToImage(image))
self.depth = sl.Mat()
# cv2.imwrite("d1.jpg", MatrixToImage(depth))
self.point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75, 4.0, 0))
tr_np = mirror_ref.m
while True:
# A new image is available if grab() returns SUCCESS
if self.zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
self.zed.retrieve_image(self.image, sl.VIEW.LEFT)
# Retrieve depth map. Depth is aligned on the left image
self.zed.retrieve_measure(self.depth, sl.MEASURE.DEPTH)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
self.zed.retrieve_measure(self.point_cloud, sl.MEASURE.XYZRGBA)
id = self.image.get_data()
# dd = self.depth.get_data()
im = img.fromarray(id)
imi = im.convert('RGB')
# im = img.fromarray(dd)
# imd = im.convert('RGB')
self.frame = imi
size = imi.size
image1 = cv2.cvtColor(np.asarray(imi), cv2.COLOR_BGR2RGB)
pic = Bitmap.FromBuffer(size[0], size[1], image1)
self.bmp.SetBitmap(pic)
self.grid_bag_sizer.Fit(self)
# im.save("test2.jpg")
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = round(self.image.get_width() / 2)
y = round(self.image.get_height() / 2)
err, point_cloud_value = self.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 = self.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()
# Close the camera
self.zed.close()
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.HD720 # Use HD720 video mode (default fps: 60)
# Use a right-handed Y-up coordinate system
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
init_params.coordinate_units = sl.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.PositionalTrackingParameters(
init_pos=py_transform)
err = zed.enable_positional_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_sensors = sl.SensorsData()
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.WORLD)
zed.get_sensors_data(zed_sensors, sl.TIME_REFERENCE.IMAGE)
zed_imu = zed_sensors.get_imu_data()
# 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.get_milliseconds()))
# 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
zed_imu_pose = sl.Transform()
ox = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[0], 3)
oy = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[1], 3)
oz = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[2], 3)
ow = round(
zed_imu.get_pose(zed_imu_pose).get_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():
#ZED自带程序，照搬过来
print("Running...")
#init = zcam.PyInitParameters()
#zed = zcam.PyZEDCamera()
'''new'''
# 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.PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.MILLIMETER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.HD2K
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
'''new'''
'''
#获取深度，点云等数据
runtime = zcam.PyRuntimeParameters()
mat = core.PyMat()
depth = core.PyMat()
point_cloud = core.PyMat()
'''
'''new'''
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # Use STANDARD sensing mode
# Setting the depth confidence parameters
runtime_parameters.confidence_threshold = 100
runtime_parameters.textureness_confidence_threshold = 100
# Capture 150 images and depth, then stop
i = 0
mat = sl.Mat() #image -> mat
depth = sl.Mat()
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75, 4.0, 0))
tr_np = mirror_ref.m
'''new'''
'''
#ZED的参数设置
init_params = zcam.PyInitParameters()
init_params.depth_mode = sl.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.PyUNIT.PyUNIT_MILLIMETER # Use milliliter units (for depth measurements)
#改变相机的模式，VGA分辨率低但是速度会快很多
init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_VGA
init_params.camera_fps = 100
'''
key = ''
while key != 113: # for 'q' key
#err = zed.grab(runtime)
#if err == tp.PyERROR_CODE.PySUCCESS:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed.retrieve_image(mat, sl.VIEW.LEFT)
zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
frame = mat.get_data()
t1 = cv2.getTickCount()
#frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#因为ZED是双目相机，所以这里识别部分只使用左镜头的图像
frame = cv2.resize(
frame, (int(mat.get_width() / 2), int(mat.get_height() / 2)),
interpolation=cv2.INTER_AREA)
# SURF detect
KP, Desc = surf.detectAndCompute(frame, None)
matches = flann.knnMatch(Desc, trainDesc, k=2)
goodMatch = []
for m, n in matches:
if (m.distance < 0.8 * n.distance):
goodMatch.append(m)
if (len(goodMatch) > MIN_MATCH_COUNT):
yp = []
qp = []
for m in goodMatch:
yp.append(trainKP[m.trainIdx].pt)
qp.append(KP[m.queryIdx].pt)
yp, qp = np.float32((yp, qp))
H, status = cv2.findHomography(yp, qp, cv2.RANSAC, 3.0)
h, w = trainImg.shape
trainBorder = np.float32([[[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]])
imgBorder = cv2.perspectiveTransform(trainBorder, H)
cv2.polylines(frame, [np.int32(imgBorder)], True, (0, 255, 0),
3) #在imshow的图像上显示出识别的框
# get coordinate 获得目标物体bbox的坐标并计算出中心点坐标
c1 = imgBorder[0, 0]
c2 = imgBorder[0, 1]
c3 = imgBorder[0, 2]
c4 = imgBorder[0, 3]
xmin = min(c1[0], c2[0])
xmax = max(c3[0], c4[0])
ymin = min(c1[1], c4[1])
ymax = max(c2[1], c3[1])
#distance_point_cloud
x = round(xmin + xmax)
y = round(ymin + ymax)
if (x < 0 or y < 0):
x = 0
y = 0
#计算出的中心点坐标后，来获取点云数据
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])
#把距离打在屏幕里
if not np.isnan(distance) and not np.isinf(distance):
distance = round(distance)
print("Distance to Camera at ({0}, {1}): {2} mm\n".format(
x, y, distance))
Z = "distance:{} mm".format(distance)
cv2.putText(frame, Z, (xmax, ymax), font, 0.7,
(255, 255, 255), 2, cv2.LINE_AA)
# Increment the loop
else:
print(
"Can't estimate distance at this position, move the camera\n"
)
else:
print("Not Eough match")
sys.stdout.flush()
t2 = cv2.getTickCount()
fps = cv2.getTickFrequency() / (t2 - t1)
fps = "Camera FPS: {0}.".format(fps)
cv2.putText(frame, fps, (25, 25), font, 0.5, (255, 255, 255), 2,
cv2.LINE_AA)
cv2.imshow("ZED", frame)
key = cv2.waitKey(5)
else:
key = cv2.waitKey(5)
cv2.destroyAllWindows()
zed.close()
print("\nFINISH")
def main():
# global stop_signal
# signal.signal(signal.SIGINT, signal_handler)
# List and open cameras
cameras = sl.Camera.get_device_list()
index = 0
cams = EasyDict({})
cams.pose_list = []
cams.zed_sensors_list = []
cams.zed_list = []
cams.left_list = []
cams.depth_list = []
cams.pointcloud_list = []
cams.timestamp_list = []
cams.image_size_list = []
cams.image_zed_list = []
cams.depth_image_zed_list = []
cams.name_list = []
cams.name_list = []
cams.py_translation_list = []
cams.py_orientation_list = []
cams.transform_list = []
cams.runtime_list = []
# Set configuration parameters
init = sl.InitParameters(
camera_resolution=sl.RESOLUTION.HD2K,
coordinate_units=sl.UNIT.METER,
#coordinate_units=sl.UNIT.MILLIMETER,#■
depth_mode=sl.DEPTH_MODE.PERFORMANCE,
coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP)
for cam in cameras:
init.set_from_serial_number(cam.serial_number)
cams.name_list.append("ZED_{}".format(cam.serial_number))
print("Opening {}".format(cams.name_list[index]))
# Create a ZED camera object
cams.zed_list.append(sl.Camera())
cams.left_list.append(sl.Mat())
cams.depth_list.append(sl.Mat())
cams.pointcloud_list.append(sl.Mat())
cams.pose_list.append(sl.Pose())
cams.zed_sensors_list.append(sl.SensorsData())
cams.timestamp_list.append(0)
cams.py_translation_list.append(sl.Translation())
cams.transform_list.append(sl.Transform())
cams.py_orientation_list.append(sl.Orientation())
# Open the camera
status = cams.zed_list[index].open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
cams.zed_list[index].close()
exit(1)
#tracing enable
py_transform = cams.transform_list[index]
print("PositionalTrackingParameters start")
tracking_parameters = sl.PositionalTrackingParameters(
init_pos=py_transform)
err = cams.zed_list[index].enable_positional_tracking(
tracking_parameters)
print("PositionalTrackingParameters end")
if err != sl.ERROR_CODE.SUCCESS:
cams.zed_list[index].close()
exit(1)
runtime = sl.RuntimeParameters()
cams.runtime_list.append(runtime)
index = index + 1
#Start camera threads
# for index in range(0, len(cams.zed_list)):
# if cams.zed_list[index].is_opened():
# thread_list.append(threading.Thread(target=grab_run, args=(cams,index,)))
# thread_list[index].start()
#https://github.com/stereolabs/zed-examples/blob/master/tutorials/tutorial%204%20-%20positional%20tracking/python/positional_tracking.py
# py_translation = sl.Translation()
# Display help in console
print_help()
# Prepare new image size to retrieve half-resolution images
for index, cam in enumerate(cameras):
fd_cam = f'{basePath}/{cams.name_list[index]}'
os.makedirs(fd_cam, exist_ok=True)
image_size = cams.zed_list[index].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 = cams.left_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
#depth_image_zed = cams.depth_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4)
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)
cams.image_size_list.append(image_size)
cams.image_zed_list.append(image_zed)
cams.depth_image_zed_list.append(depth_image_zed)
########
cam_intr, distortion = get_camera_intrintic_info(cams.zed_list[index])
filename = f'{fd_cam}/camera-intrinsics.csv'
np.savetxt(filename, cam_intr)
filename = f'{fd_cam}/camera-distortion.csv'
np.savetxt(filename, distortion)
#*******************************************************************
take_by_keyinput(cameras, cams)
# take_by_keyinput_camera_view(cameras, cams)
#*******************************************************************
index = 0
for cam in cameras:
cams.zed_list[index].close()
index += 1
print("\nFINISH")
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 get_ground_plane(self):
ground = sl.Plane()
transform = sl.Transform()
result = self.zed.find_floor_plane(ground, transform)
return [result, ground, transform]
def __init__(self):
self.body_io = []
self.path_mem = []
self.path = sl.Transform()
self.set_path(self.path, self.path_mem)
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 main():
# 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.ULTRA # Use PERFORMANCE depth mode
init_params.coordinate_units = sl.UNIT.METER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.HD720
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
# Create and set RuntimeParameters after opening the camera
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.FILL # Use STANDARD sensing mode
# Setting the depth confidence parameters
runtime_parameters.confidence_threshold = 50
runtime_parameters.textureness_confidence_threshold = 100
# Capture 150 images and depth, then stop
i = 0
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75, 4.0, 0))
tr_np = mirror_ref.m
while True:
# 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.LEFT)
# Retrieve depth map. Depth is aligned on the left image
zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = round(image.get_width() / 2)
y = round(image.get_height() / 2)
err, point_cloud_value = point_cloud.get_value(x, y)
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)
# Use get_data() to get the numpy array
image_ocv = image.get_data()
# image_ocv = cv.cvtColor(image_ocv, cv.COLOR_BGR2GRAY)
# image_ocv = cv.bitwise_not(image_ocv)
image_ocv = cv.circle(image_ocv, (x, y),
radius=4,
color=(0, 255, 0),
thickness=2)
if not np.isnan(distance) and not np.isinf(distance):
text = "Distance to Camera at ({}, {}) (image center): {:1.3} m".format(
x, y, distance)
print(text, end="\r")
cv.putText(image_ocv, text, (10, 30), cv.FONT_HERSHEY_SIMPLEX,
0.5, (0, 255, 0), 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"
)
# Display the left image from the numpy array
cv.imshow("Image", image_ocv)
key = cv.waitKey(1)
if key == 27: # esc
break
sys.stdout.flush()
# Close the camera
zed.close()
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.HD720 # Use HD720 video mode
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE
init_params.coordinate_units = sl.UNIT.METER
init_params.sdk_verbose = True
# Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
obj_param = sl.ObjectDetectionParameters()
obj_param.enable_tracking=True
obj_param.image_sync=True
obj_param.enable_mask_output=True
camera_infos = zed.get_camera_information()
if obj_param.enable_tracking :
positional_tracking_param = sl.PositionalTrackingParameters()
#positional_tracking_param.set_as_static = True
positional_tracking_param.set_floor_as_origin = True
zed.enable_positional_tracking(positional_tracking_param)
print("Object Detection: Loading Module...")
err = zed.enable_object_detection(obj_param)
if err != sl.ERROR_CODE.SUCCESS :
print (repr(err))
zed.close()
exit(1)
objects = sl.Objects()
obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
obj_runtime_param.detection_confidence_threshold = 40
while zed.grab() == sl.ERROR_CODE.SUCCESS:
err = zed.retrieve_objects(objects, obj_runtime_param)
start = timeit.default_timer()
if objects.is_new :
obj_array = objects.object_list
if len(obj_array) > 0 :
first_object = obj_array[0]
print("First object attributes:")
print(" Label '"+repr(first_object.label)+"' (conf. "+str(int(first_object.confidence))+"/100)")
position = first_object.position
dimensions = first_object.dimensions
print(" 3D position: [{0},{1},{2}]\n 3D dimentions: [{3},{4},{5}]".format(position[0],position[1],position[2],dimensions[0],dimensions[1],dimensions[2]))
######################
image = sl.Mat()
depth = sl.Mat()
point_cloud = sl.Mat()
mirror_ref = sl.Transform()
mirror_ref.set_translation(sl.Translation(2.75,4.0,0))
tr_np = mirror_ref.m
zed.retrieve_image(image, sl.VIEW.LEFT)
# Retrieve depth map. Depth is aligned on the left image
zed.retrieve_measure(depth, sl.MEASURE.DEPTH)
# Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
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")
else:
pass
print("\n Bounding Box 3D ")
bounding_box = first_object.bounding_box
stop = timeit.default_timer()
print("\n FPS:", stop - start)
# Close the camera
zed.close()
def main():
print("Running Plane Detection sample ... Press 'q' to quit")
# Create a camera object
zed = sl.Camera()
# Set configuration parameters
init = sl.InitParameters()
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP # OpenGL coordinate system
# If applicable, use the SVO given as parameter
# Otherwise use ZED live stream
if len(sys.argv) == 2:
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
init.set_from_svo_file(filepath)
# Open the camera
status = zed.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit(1)
# Get camera info and check if IMU data is available
camera_infos = zed.get_camera_information()
has_imu = camera_infos.sensors_configuration.gyroscope_parameters.is_available
# Initialize OpenGL viewer
viewer = gl.GLViewer()
viewer.init(
camera_infos.camera_configuration.calibration_parameters.left_cam,
has_imu)
image = sl.Mat() # current left image
pose = sl.Pose() # positional tracking data
plane = sl.Plane() # detected plane
mesh = sl.Mesh() # plane mesh
find_plane_status = sl.ERROR_CODE.SUCCESS
tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF
# Timestamp of the last mesh request
last_call = time.time()
user_action = gl.UserAction()
user_action.clear()
# Enable positional tracking before starting spatial mapping
zed.enable_positional_tracking()
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD
while viewer.is_available():
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# Retrieve left image
zed.retrieve_image(image, sl.VIEW.LEFT)
# Update pose data (used for projection of the mesh over the current image)
tracking_state = zed.get_position(pose)
if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
# Compute elapse time since the last call of plane detection
duration = time.time() - last_call
# Ask for a mesh update on mouse click
if user_action.hit:
image_click = [
user_action.hit_coord[0] * camera_infos.
camera_configuration.camera_resolution.width,
user_action.hit_coord[1] * camera_infos.
camera_configuration.camera_resolution.height
]
find_plane_status = zed.find_plane_at_hit(
image_click, plane)
# Check if 500 ms have elapsed since last mesh request
if duration > .5 and user_action.press_space:
# Update pose data (used for projection of the mesh over the current image)
reset_tracking_floor_frame = sl.Transform()
find_plane_status = zed.find_floor_plane(
plane, reset_tracking_floor_frame)
last_call = time.time()
if find_plane_status == sl.ERROR_CODE.SUCCESS:
mesh = plane.extract_mesh()
viewer.update_mesh(mesh, plane.type)
user_action = viewer.update_view(image, pose.pose_data(),
tracking_state)
viewer.exit()
image.free(sl.MEM.CPU)
mesh.clear()
# Disable modules and close camera
zed.disable_positional_tracking()
zed.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()
