You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
192 lines
6.6 KiB
192 lines
6.6 KiB
# License: Apache 2.0. See LICENSE file in root directory.
|
|
# Copyright(c) 2023 Intel Corporation. All Rights Reserved.
|
|
|
|
# test:device D400*
|
|
|
|
import pyrealsense2 as rs
|
|
from rspy import test, log
|
|
from rspy import tests_wrapper as tw
|
|
import os
|
|
import math
|
|
|
|
# Start depth + color streams and go through the frame to make sure it is showing a depth image
|
|
# Color stream is only used to display the way the camera is facing
|
|
# Verify that the frame does indeed have variance - therefore it is showing a depth image
|
|
|
|
# In debug mode, an image with the frames found will be displayed and saved
|
|
# Make sure you have the required packages installed
|
|
DEBUG_MODE = False
|
|
|
|
# Defines how far in cm do pixels have to be, to be considered in a different distance
|
|
# for example, 10 for 10cm, will define the range 100-109 cm as one (as 100)
|
|
DETAIL_LEVEL = 10
|
|
FRAMES_TO_CHECK = 30
|
|
|
|
dev = test.find_first_device_or_exit()
|
|
tw.start_wrapper( dev )
|
|
|
|
cfg = rs.config()
|
|
cfg.enable_stream(rs.stream.depth, rs.format.z16, 30)
|
|
if DEBUG_MODE:
|
|
cfg.enable_stream(rs.stream.color, rs.format.bgr8, 30)
|
|
|
|
|
|
def display_image(img):
|
|
"""
|
|
Display a given image and exits when the x button or esc key are pressed
|
|
"""
|
|
import cv2
|
|
|
|
window_title = "Output Stream"
|
|
cv2.imshow(window_title, img)
|
|
while cv2.getWindowProperty(window_title, cv2.WND_PROP_VISIBLE) > 0:
|
|
k = cv2.waitKey(33)
|
|
if k == 27: # Esc key to stop
|
|
cv2.destroyAllWindows()
|
|
break
|
|
elif k == -1: # normally -1 returned
|
|
pass
|
|
|
|
|
|
def frames_to_image(depth, color, save, display, laser_enabled):
|
|
"""
|
|
This function gets depth and color frames, transforms them to an image (numpy array)
|
|
and then save and/or display
|
|
|
|
If color frame is given, it will also concatenate it with the depth frame before doing the given action
|
|
"""
|
|
import numpy as np
|
|
import cv2
|
|
|
|
colorizer = rs.colorizer()
|
|
depth_image = np.asanyarray(colorizer.colorize(depth).get_data())
|
|
img = depth_image
|
|
|
|
if color: # if color frame was successfully captured, merge it and the depth frame
|
|
from scipy.ndimage import zoom
|
|
color_image = np.asanyarray(color.get_data())
|
|
depth_rows, _, _ = depth_image.shape
|
|
color_rows, _, _ = color_image.shape
|
|
# resize the image with the higher resolution to look like the smaller one
|
|
if depth_rows < color_rows:
|
|
color_image = zoom(color_image, (depth_rows / color_rows, depth_rows / color_rows, 1))
|
|
elif color_rows < depth_rows:
|
|
depth_image = zoom(depth_image, (color_rows / depth_rows, color_rows / depth_rows, 1))
|
|
img = np.concatenate((depth_image, color_image), axis=1)
|
|
|
|
if save:
|
|
file_name = f"output_stream{'_laser_on' if laser_enabled else '_laser_off'}.png"
|
|
log.i("Saved image in", os.getcwd() + "\\" + file_name)
|
|
cv2.imwrite(file_name, img)
|
|
if display:
|
|
display_image(img)
|
|
|
|
|
|
def get_frames(config, laser_enabled):
|
|
pipeline = rs.pipeline()
|
|
pipeline_profile = pipeline.start(config)
|
|
|
|
sensor = pipeline_profile.get_device().first_depth_sensor()
|
|
if laser_enabled and sensor.supports(rs.option.laser_power):
|
|
sensor.set_option(rs.option.laser_power, sensor.get_option_range(rs.option.laser_power).max)
|
|
sensor.set_option(rs.option.emitter_enabled, 1 if laser_enabled else 0)
|
|
|
|
# to get a proper image, we sometimes need to wait a few frames, like when the camera is facing a light source
|
|
frames = pipeline.wait_for_frames()
|
|
for i in range(30):
|
|
frames = pipeline.wait_for_frames()
|
|
depth = frames.get_depth_frame()
|
|
color = frames.get_color_frame()
|
|
pipeline.stop()
|
|
return depth, color
|
|
|
|
|
|
def round_to_units(num):
|
|
"""
|
|
Input: Distance of a certain point, in meters
|
|
Output: Distance according to the given detail level, in cm
|
|
"""
|
|
in_cm = round(num, 2) * 100 # convert to cm
|
|
return math.floor(in_cm / DETAIL_LEVEL) * DETAIL_LEVEL # rounds the distance according to the given unit
|
|
|
|
|
|
def sort_dict(my_dict):
|
|
my_keys = list(my_dict.keys())
|
|
my_keys.sort()
|
|
sorted_dict = {i: my_dict[i] for i in my_keys}
|
|
return sorted_dict
|
|
|
|
|
|
def get_distances(depth):
|
|
MAX_METERS = 10 # max distance that can be detected, in meters
|
|
dists = {}
|
|
total = 0
|
|
for y in range(depth.get_height()):
|
|
for x in range(depth.get_width()):
|
|
dist = depth.get_distance(x, y)
|
|
if dist >= MAX_METERS: # out of bounds, assuming it is a junk value
|
|
continue
|
|
|
|
dist = round_to_units(dist) # round according to DETAIL_LEVEL
|
|
|
|
if dists.get(dist) is not None:
|
|
dists[dist] += 1
|
|
else:
|
|
dists[dist] = 1
|
|
total += 1
|
|
|
|
dists = sort_dict(dists) # for debug convenience
|
|
log.d("Distances detected in frame are:", dists)
|
|
return dists, total
|
|
|
|
|
|
def is_depth_meaningful(config, laser_enabled=True, save_image=False, show_image=False):
|
|
"""
|
|
Checks if the camera is showing a frame with a meaningful depth.
|
|
DETAIL_LEVEL is setting how close distances need to be, to be considered the same
|
|
|
|
returns true if frame shows meaningful depth
|
|
"""
|
|
|
|
depth, color = get_frames(config, laser_enabled)
|
|
|
|
if not depth:
|
|
log.f("Error getting depth frame")
|
|
return False
|
|
if DEBUG_MODE and not color:
|
|
log.e("Error getting color frame")
|
|
|
|
dists, total = get_distances(depth)
|
|
|
|
# save or display image (only possible through manual debugging)
|
|
if save_image or show_image:
|
|
frames_to_image(depth, color, save_image, show_image, laser_enabled)
|
|
|
|
# Goes over the distances found, and checks if any distance is the same on more than 90% of the pixels
|
|
meaningful_depth = True
|
|
for key in dists:
|
|
if dists[key] > total*0.9:
|
|
meaningful_depth = False
|
|
break
|
|
num_blank_pixels = dists[0]
|
|
return meaningful_depth, num_blank_pixels
|
|
|
|
|
|
################################################################################################
|
|
|
|
test.start("Testing depth frame - laser ON -", dev.get_info(rs.camera_info.name))
|
|
is_there_depth = False
|
|
max_black_pixels = float('inf')
|
|
|
|
# we perform the check on a few different frames to make sure we get the best indication if we have depth
|
|
for frame_num in range(FRAMES_TO_CHECK):
|
|
result, laser_black_pixels = is_depth_meaningful(cfg, laser_enabled=True, save_image=DEBUG_MODE, show_image=DEBUG_MODE)
|
|
is_there_depth = is_there_depth or result # we check if we found depth at any frame checked
|
|
max_black_pixels = min(max_black_pixels, laser_black_pixels)
|
|
|
|
test.check(is_there_depth is True)
|
|
test.finish()
|
|
|
|
tw.stop_wrapper( dev )
|
|
test.print_results_and_exit()
|