def __init__(self):
self.ORIGINAL_FORWARD = sl.Translation()
self.ORIGINAL_FORWARD.init_vector(0, 0, 1)
self.ORIGINAL_UP = sl.Translation()
self.ORIGINAL_UP.init_vector(0, 1, 0)
self.ORIGINAL_RIGHT = sl.Translation()
self.ORIGINAL_RIGHT.init_vector(1, 0, 0)
self.znear = 0.5
self.zfar = 100.
self.horizontalFOV = 70.
self.orientation_ = sl.Orientation()
self.position_ = sl.Translation()
self.forward_ = sl.Translation()
self.up_ = sl.Translation()
self.right_ = sl.Translation()
self.vertical_ = sl.Translation()
self.vpMatrix_ = sl.Matrix4f()
self.projection_ = sl.Matrix4f()
self.projection_.set_identity()
self.setProjection(1.78)
self.position_.init_vector(0., 5., -3.)
tmp = sl.Translation()
tmp.init_vector(0, 0, -4)
tmp2 = sl.Translation()
tmp2.init_vector(0, 1, 0)
self.setDirection(tmp, tmp2)
cam_rot = sl.Rotation()
cam_rot.set_euler_angles(-50., 180., 0., False)
self.setRotation(cam_rot)
def update(self):
if(self.mouse_button[0]):
r = sl.Rotation()
vert=self.camera.vertical_
tmp = vert.get()
vert.init_vector(tmp[0] * 1.,tmp[1] * 1., tmp[2] * 1.)
r.init_angle_translation(self.mouseMotion[0] * 0.002, vert)
self.camera.rotate(r)
r.init_angle_translation(self.mouseMotion[1] * 0.002, self.camera.right_)
self.camera.rotate(r)
if(self.mouse_button[1]):
t = sl.Translation()
tmp = self.camera.right_.get()
scale = self.mouseMotion[0] *-0.01
t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale)
self.camera.translate(t)
tmp = self.camera.up_.get()
scale = self.mouseMotion[1] * 0.01
t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale)
self.camera.translate(t)
if (self.wheelPosition != 0):
t = sl.Translation()
tmp = self.camera.forward_.get()
scale = self.wheelPosition * -0.065
t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale)
self.camera.translate(t)
self.camera.update()
self.mouseMotion = [0., 0.]
self.wheelPosition = 0
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 get_pos_dt(cams, index, cnt_r=6):
zed, zed_pose, zed_sensors = cams.zed_list[index], cams.pose_list[
index], cams.zed_sensors_list[index]
#https://github.com/stereolabs/zed-examples/blob/master/tutorials/tutorial%204%20-%20positional%20tracking/python/positional_tracking.py
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 = cams.py_translation_list[index]
py_orientation = cams.py_orientation_list[index]
# py_orientation = sl.Orientation()
tx = round(zed_pose.get_translation(py_translation).get()[0], cnt_r)
ty = round(zed_pose.get_translation(py_translation).get()[1], cnt_r)
tz = round(zed_pose.get_translation(py_translation).get()[2], cnt_r)
# print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp.get_milliseconds()))
# Display the orientation quaternion
ox = round(zed_pose.get_orientation(py_orientation).get()[0], cnt_r)
oy = round(zed_pose.get_orientation(py_orientation).get()[1], cnt_r)
oz = round(zed_pose.get_orientation(py_orientation).get()[2], cnt_r)
ow = round(zed_pose.get_orientation(py_orientation).get()[3], cnt_r)
pose_lst = [tx, ty, tz, ow, ox, oy, oz]
# get camera transform data
R = zed_pose.get_rotation_matrix(sl.Rotation()).r.T / 1000
t = zed_pose.get_translation(sl.Translation()).get()
world2cam = np.hstack((R, np.dot(-R, t).reshape(3, -1)))
K = get_camera_intrintic_info(zed)
P = np.dot(K, world2cam)
transform = np.vstack((P, [0, 0, 0, 1]))
return pose_lst, transform
def to_cv_point(self, x, z):
out = []
if isinstance(x, float) and isinstance(z, float):
out = [
int((x - self.x_min) / self.x_step),
int((z - self.z_min) / self.z_step)
]
elif isinstance(x, list) and isinstance(z, sl.Pose):
# Go to camera current pose
rotation = z.get_rotation_matrix()
rotation.inverse()
tmp = x - (z.get_translation() * rotation.get_orientation()).get()
new_position = sl.Translation()
new_position.init_vector(tmp[0], tmp[1], tmp[2])
out = [
int(((new_position.get()[0] - self.x_min) / self.x_step) +
0.5),
int(((new_position.get()[2] - self.z_min) / self.z_step) + 0.5)
]
elif isinstance(x, TrackPoint) and isinstance(z, sl.Pose):
pos = x.get_xyz()
out = self.to_cv_point(pos, z)
else:
print("Unhandled argument type")
return out
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 generate_view(self, objects, current_camera_pose, tracking_view,
tracking_enabled):
# To get position in WORLD reference
for obj in objects.object_list:
pos = obj.position
tmp_pos = sl.Translation()
tmp_pos.init_vector(pos[0], pos[1], pos[2])
new_pos = (tmp_pos * current_camera_pose.get_orientation()
).get() + current_camera_pose.get_translation().get()
obj.position = np.array([new_pos[0], new_pos[1], new_pos[2]])
# Initialize visualisation
if (not self.has_background_ready):
self.generate_background()
np.copyto(tracking_view, self.background, 'no')
if (tracking_enabled):
# First add new points and remove the ones that are too old
current_timestamp = objects.timestamp.get_seconds()
self.add_to_tracklets(objects, current_timestamp)
self.prune_old_points(current_timestamp)
# Draw all tracklets
self.draw_tracklets(tracking_view, current_camera_pose)
else:
self.draw_points(objects.object_list, tracking_view,
current_camera_pose)
def DataAcquisiton(zed, zed_pose, runtime_parameters):
while True:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
#time1 = time.time()
zed.get_position(
zed_pose,
sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) # Updating Pose
# Translation
translation = sl.Translation()
tx = round(zed_pose.get_translation(translation).get()[0], 3)
ty = round(zed_pose.get_translation(translation).get()[1], 3)
tz = round(zed_pose.get_translation(translation).get()[2], 3)
# Rotation
rotation = zed_pose.get_rotation_vector()
rx = round(rotation[0], 2)
ry = round(rotation[1], 2)
rz = round(rotation[2], 2)
with open('/media/ctuxavier/ADATASE730H/zed_recording.csv',
'a') as csv_file:
writer = csv.writer(csv_file)
writer.writerow([ty, tx, tz, rx, ry, rz, time.time()])
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 get_imu_pose(cams, index, cnt_r=10):
err = cams.zed_list[index].grab(cams.runtime_list[index])
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
zed = cams.zed_list[index]
zed_pose = cams.pose_list[index]
zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
py_translation = sl.Translation() #cams.py_translation_list[index]
tx = round(zed_pose.get_translation(py_translation).get()[0], cnt_r)
ty = round(zed_pose.get_translation(py_translation).get()[1], cnt_r)
tz = round(zed_pose.get_translation(py_translation).get()[2], cnt_r)
zed_sensors = cams.zed_sensors_list[index]
zed_imu = zed_sensors.get_imu_data()
zed_imu_pose = cams.transform_list[index] #sl.Transform()
ox = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[0], cnt_r)
oy = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[1], cnt_r)
oz = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[2], cnt_r)
ow = round(
zed_imu.get_pose(zed_imu_pose).get_orientation().get()[3], cnt_r)
imu_pose_lst = [tx, ty, tz, ow, ox, oy, oz]
return imu_pose_lst
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 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()
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 get_zed_pose(zed, verbose=False):
"""
Parameters
----------
zed: sl.Camera
The ZED camera object
verbose: bool
If true, will print the translation + orientation to the console
Returns
-------
list
Pose as [time, tx, ty, tz, ox, oy, oz, ow]
time is given in seconds.
The camera position is [tx, ty, tz]
And orientation is the quaternion [ox, oy, oz, ow]
"""
# Call zed.grab() each time before calling this function
zed_pose = sl.Pose()
# Get the pose of the camera relative to the world frame
state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
# Display 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)
time_nsec = zed_pose.timestamp.get_nanoseconds()
time_sec = float(time_nsec) / 1e9
if verbose:
print(
"Translation: tx: {0}, ty: {1}, tz: {2}, timestamp: {3}".format(
tx, ty, tz, time_sec))
# Display 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)
if verbose:
print("Orientation: ox: {0}, oy: {1}, oz: {2}, ow: {3}\n".format(
ox, oy, oz, ow))
return [time_sec, tx, ty, tz, ox, oy, oz, ow]
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 __init__(self):
self.ORIGINAL_FORWARD = sl.Translation()
self.ORIGINAL_FORWARD.init_vector(0,0,1)
self.ORIGINAL_UP = sl.Translation()
self.ORIGINAL_UP.init_vector(0,1,0)
self.ORIGINAL_RIGHT = sl.Translation()
self.ORIGINAL_RIGHT.init_vector(1,0,0)
self.znear = 0.5
self.zfar = 100.
self.horizontalFOV = 70.
self.orientation_ = sl.Orientation()
self.position_ = sl.Translation()
self.forward_ = sl.Translation()
self.up_ = sl.Translation()
self.right_ = sl.Translation()
self.vertical_ = sl.Translation()
self.vpMatrix_ = sl.Matrix4f()
self.offset_ = sl.Translation()
self.offset_.init_vector(0,0,5)
self.projection_ = sl.Matrix4f()
self.projection_.set_identity()
self.setProjection(1.78)
self.position_.init_vector(0., 0., 0.)
tmp = sl.Translation()
tmp.init_vector(0, 0, -.1)
tmp2 = sl.Translation()
tmp2.init_vector(0, 1, 0)
self.setDirection(tmp, tmp2)
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 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():
# 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 capture(imagePath):
#Create a ZED camera object
zed = sl.Camera()
# Set configuration parameters
init_params = sl.InitParameters()
init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA # Use ULTRA depth mode
init_params.coordinate_units = sl.UNIT.UNIT_METER # Use meter units (for depth measurements)
init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720
#Open the camera
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
print("[DECON] Error: Unable to open the camera")
zed.close()
exit(-1)
try:
image = sl.Mat()
depth_map = 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
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode
# Setting the depth confidence parameters
#runtime_parameters.confidence_threshold = 100
# runtime_parameters.textureness_confidence_threshold = 100
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# A new image and depth is available if grab() returns SUCCESS
zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) # Retrieve left image
img = image.get_data()
im = Image.fromarray(img).rotate(180)
# Retrieve depth map. Depth is aligned on the left image
zed.retrieve_measure(depth_map, 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)
# 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 (" + str(x) + ", " + str(y) + ") (image center): " + str(distance))
#print("Image size is: ",im.size)
(w, h) = im.size
k = 2 * distance * tan(radians(30)) / h
s = 2 * distance * tan(radians(45)) / w
print w, h, distance, s, k
#print("Vertical m/pixel:",str(k))
#print("Horizontal m/pixel:",str(s))
else:
print("Can't estimate distance at this position.")
print(
"Your camera is probably too close to the scene, please move it backwards.\n"
)
torch.cuda.empty_cache()
zed.close()
exit(-1)
sys.stdout.flush()
im.save(imagePath)
del im
except Exception as e:
print("[DECON] Error: Exception occured")
print(e)
finally:
torch.cuda.empty_cache()
zed.close()
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():
# 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():
#OPENCV setup
lowerBound = np.array([33, 80, 40])
upperBound = np.array([102, 255, 255])
red = (255, 0, 0)
distance = 1
xcenter = 1
center = 0
kernelOpen = np.ones((5, 5))
kernelClose = np.ones((20, 20))
#ZED SETUP
# 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 milliliter units (for depth measurements)
#OPEN 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
# 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)
#convert image to openCV
image_zed = sl.Mat(
zed.get_camera_information().camera_resolution.width,
zed.get_camera_information().camera_information.height,
sl.MAT_TYPE.U8_C4)
image_ocv = image_zed.get_data()
ret, image = img_ocv.read()
# 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)
if not np.isnan(distance) and not np.isinf(distance):
distance = round(distance)
print("Distance to Camera at ({0}, {1}): {2} mm\n".format(
x, y, distance))
# Increment the loop
i = i + 1
else:
print(
"Can't estimate distance at this position, move the camera\n"
)
sys.stdout.flush()
# Close the camera
zed.close()
def 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():
#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():
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 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():
global stop_signal
thread_list = []
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()
#tracing enable
py_transform = cams.transform_list[index]
tracking_parameters = sl.PositionalTrackingParameters(
init_pos=py_transform)
err = cams.zed_list[index].enable_positional_tracking(
tracking_parameters)
if err != sl.ERROR_CODE.SUCCESS:
cams.zed_list[index].close()
exit(1)
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):
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 = get_camera_intrintic_info(cams.zed_list[index])
filename = path + cams.name_list[index] + "-camera-intrinsics.txt"
df = pd.DataFrame(cam_intr)
df.to_csv(filename, sep=' ', header=None, index=None)
#*******************************************************************
index = 0
take_cam_id = 0
for cam in cameras:
get_cam_data(cams, index, take_cam_id)
index += 1
stop_signal = True
#*******************************************************************
'''
key = ' '
take_cam_id=0
while key != 113 :
index=0
image_ocv_cat=None
depth_image_ocv_cat=None
for cam in cameras:
image_ocv, depth_image_ocv=get_cam_color_depth(cams,index)
if image_ocv_cat is None:
image_ocv_cat=image_ocv
depth_image_ocv_cat=depth_image_ocv
else:
image_ocv_cat=np.hstack([image_ocv_cat,image_ocv])
depth_image_ocv_cat=np.hstack([depth_image_ocv_cat,depth_image_ocv])
index+=1
cv2.imshow("Image", image_ocv_cat)
cv2.imshow("Depth", depth_image_ocv_cat)
key = cv2.waitKey(10)
if key == 114 or key == 82:#R
index = 0
for cam in cameras:
# process_key_event(cams,key,index)
get_cam_data(cams, index,take_cam_id)
index+=1
take_cam_id=take_cam_id+1
cv2.destroyAllWindows()
'''
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.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()
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit()
tracking_params = sl.PositionalTrackingParameters()
zed.enable_positional_tracking(tracking_params)
runtime = sl.RuntimeParameters()
camera_pose = sl.Pose()
camera_info = zed.get_camera_information()
# Create OpenGL viewer
viewer = gl.GLViewer()
viewer.init(len(sys.argv), sys.argv, camera_info.camera_model)
py_translation = sl.Translation()
pose_data = sl.Transform()
text_translation = ""
text_rotation = ""
while viewer.is_available():
if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS:
tracking_state = zed.get_position(camera_pose)
if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
rotation = camera_pose.get_rotation_vector()
translation = camera_pose.get_translation(py_translation)
text_rotation = str(
(round(rotation[0], 2), round(rotation[1],
2), round(rotation[2], 2)))
text_translation = str(
