3、深度感知
如何检索场景的深度和点云,并在终端打印给定点的距离。
循环直到捕获50帧。
(1)创建打开并配置ZED
在HD720模式下将ZED设置为60fps,并在PERFORMANCE模式下启用深度。
ZED SDK提供了不同的深度模式:PERDORMANCE,MEDIUM,QUALITY
# Create a ZED camera
zed = sl.Camera()
# Create configuration parameters
init_params = sl.InitParameters()
init_params.sdk_verbose = True # Enable the verbose mode
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Set the depth mode to performance (fastest)
# Open the camera
err = zed.open(init_params)
if (err!=sl.ERROR_CODE.SUCCESS):
exit(-1)
深度模式的默认参数是DEPTH_MODE.PERFORMANCE。没有必要在Initparameters中设置深度模式。
(2)捕获数据
循环直至成功捕获50张图像。
检索深度图和检索图像一样简单:创建Mat存储深度图;调用retrieve_measure()获取深度图。
# Capture 50 images and depth, then stop
i = 0
image = sl.Mat()
depth = sl.Mat()
while (i < 50) :
# Grab an image
if (zed.grab() == sl.ERROR_CODE.SUCCESS) :
# A new image is available if grab() returns SUCCESS
zed.retrieve_image(image, sl.VIEW.LEFT) # Get the left image
zed.retrieve_measure(depth, sl.MEASURE.DEPTH) # Retrieve depth Mat. Depth is aligned on the left image
i = i + 1
}
}
获取特定像素处的深度。在本程序中提取图像中心点的距离(宽度/2,高度/2)。
# Get and print distance value in mm at the center of the image
# We measure the distance camera - object using Euclidean distance
x = image.get_width() / 2
y = image.get_height() / 2
point_cloud_value = point_cloud.get_value(x, y)
distance = math.sqrt(point_cloud_value[0]*point_cloud_value[0] + point_cloud_value[1]*point_cloud_value[1] + point_cloud_value[2]*point_cloud_value[2])
printf("Distance to Camera at (", x, y, "): ", distance, "mm")
(3)关闭相机
# Close the camera
zed.close()
完整程序
import pyzed.sl as sl
import math
import numpy as np
import sys
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.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.confidence_threshold = 100
runtime_parameters.texture_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 i < 150:
# 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)
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
zed.close()
if __name__ == "__main__":
main()