# 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()