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 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():
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 get_ground_plane(self):
ground = sl.Plane()
transform = sl.Transform()
result = self.zed.find_floor_plane(ground, transform)
return [result, ground, transform]
def main():
cam = sl.Camera()
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.ULTRA
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
if len(sys.argv) == 2:
filepath = sys.argv[1]
print("Reading SVO file: {0}".format(filepath))
init.set_from_svo_file(filepath)
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
print(repr(status))
exit(1)
runtime = sl.RuntimeParameters()
runtime.sensing_mode = sl.SENSING_MODE.STANDARD
runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD
spatial = sl.SpatialMappingParameters()
transform = sl.Transform()
tracking = sl.PositionalTrackingParameters(transform)
cam.enable_positional_tracking(tracking)
pymesh = sl.Mesh()
pyplane = sl.Plane()
reset_tracking_floor_frame = sl.Transform()
found = 0
print("Processing...")
i = 0
calibration_params = cam.get_camera_information().calibration_parameters
# Focal length of the left eye in pixels
focal_left_x = calibration_params.left_cam.fx
focal_left_y = calibration_params.left_cam.fy
# First radial distortion coefficient
# k1 = calibration_params.left_cam.disto[0]
# Translation between left and right eye on z-axis
# tz = calibration_params.T.z
# Horizontal field of view of the left eye in degrees
# h_fov = calibration_params.left_cam.h_fov
print("fx", focal_left_x)
print("fy", focal_left_y)
print("cx", calibration_params.left_cam.cx)
print("cy", calibration_params.left_cam.cy)
print("distortion", calibration_params.left_cam.disto)
cloud = sl.Mat()
while i < 1000:
if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS :
err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame)
if err == sl.ERROR_CODE.SUCCESS:
# if i > 200 and err == sl.ERROR_CODE.SUCCESS:
found = 1
print('Floor found!')
pymesh = pyplane.extract_mesh()
#get gound plane info
print("floor normal",pyplane.get_normal()) #print normal of plane
print("floor equation", pyplane.get_plane_equation())
print("plane center", pyplane.get_center())
print("plane pose", pyplane.get_pose())
# camera_pose = sl.Pose()
# py_translation = sl.Translation()
# tracking_state = cam.get_position(camera_pose)
# # if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
# rotation = camera_pose.get_rotation_vector()
# rx = round(rotation[0], 2)
# ry = round(rotation[1], 2)
# rz = round(rotation[2], 2)
# translation = camera_pose.get_translation(py_translation)
# tx = round(translation.get()[0], 2)
# ty = round(translation.get()[1], 2)
# tz = round(translation.get()[2], 2)
# text_translation = str((tx, ty, tz))
# text_rotation = str((rx, ry, rz))
# pose_data = camera_pose.pose_data(sl.Transform())
# print("translation",text_translation)
# print("rotation",text_rotation)
cam.retrieve_measure(cloud, sl.MEASURE.XYZRGBA)
break
i+=1
if found == 0:
print('Floor was not found, please try with another SVO.')
cam.close()
exit(0)
#load image
path = "/home/hcl/Documents/ZED/pics/Explorer_HD1080_SN14932_16-24-06.png"
im = cv2.imread(path)
h, w = im.shape[0], im.shape[1]
print("orignal image shape", h,w)
im = im[:,:int(w/2),:] #get left image
# im = cv2.resize(im,(int(im.shape[1]/2),int(im.shape[0]/2)))
#warped img
warped = np.zeros((h,w,3))
roll = -80
roll *= np.pi/180
Rx = np.matrix([
[1, 0,0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
#point cloud
# i = 500
# j = 355
if False:
point_cloud = sl.Mat()
cam.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
for i in range(100):
for j in range(1000):
point3D = point_cloud.get_value(i,j)
# x = point3D[0]
# y = point3D[1]
# z = point3D[2]
color = point3D[1][3]
if ~np.isnan(color) and ~np.isinf(color):
xyz = point3D[1][:3].reshape((-1,1))
# xyz_hom = np.vstack((xyz,1))
print('point3D',point3D)
warped_index = Rx @ xyz #NEED TO USE H**O COORDINATES? divide by last column?
print('warped_index',warped_index)
RGB = im[j,i,:]
RGB = np.dstack(([255],[255],[255]))
print('RGB',RGB)
warped[int(np.round(warped_index[0]+10)),int(np.round(warped_index[1])),:] = RGB
cam.disable_positional_tracking()
# save_mesh(pymesh)
cam.close()
print("\nFINISH")