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.
113 lines
5.7 KiB
113 lines
5.7 KiB
# License: Apache 2.0. See LICENSE file in root directory.
|
|
# Copyright(c) 2022 Intel Corporation. All Rights Reserved.
|
|
|
|
#test:donotrun:!dds
|
|
#test:retries:gha 2
|
|
|
|
from rspy import log, test
|
|
log.nested = 'C '
|
|
|
|
import dds
|
|
import pyrealsense2 as rs
|
|
if log.is_debug_on():
|
|
rs.log_to_console( rs.log_severity.debug )
|
|
from time import sleep
|
|
|
|
context = rs.context( { 'dds': { 'enabled': True, 'domain': 123 }} )
|
|
only_sw_devices = int(rs.product_line.sw_only) | int(rs.product_line.any_intel)
|
|
|
|
import os.path
|
|
cwd = os.path.dirname(os.path.realpath(__file__))
|
|
remote_script = os.path.join( cwd, 'device-broadcaster.py' )
|
|
with test.remote( remote_script, nested_indent=" S" ) as remote:
|
|
remote.wait_until_ready()
|
|
#
|
|
#############################################################################################
|
|
#
|
|
test.start( "D435i extrinsics" )
|
|
try:
|
|
remote.run( 'instance = broadcast_device( d435i, d435i.device_info )' )
|
|
n_devs = 0
|
|
for dev in dds.wait_for_devices( context, only_sw_devices ):
|
|
n_devs += 1
|
|
test.check_equal( n_devs, 1 )
|
|
|
|
sensors = {sensor.get_info( rs.camera_info.name ) : sensor for sensor in dev.query_sensors()}
|
|
depth_profile = rs.stream_profile()
|
|
ir1_profile = rs.stream_profile()
|
|
ir2_profile = rs.stream_profile()
|
|
color_profile = rs.stream_profile()
|
|
gyro_profile = rs.stream_profile()
|
|
accel_profile = rs.stream_profile()
|
|
|
|
sensor = sensors['Stereo Module']
|
|
for profile in sensor.get_stream_profiles() :
|
|
if profile.stream_type() == rs.stream.depth :
|
|
depth_profile = profile
|
|
break
|
|
for profile in sensor.get_stream_profiles() :
|
|
if profile.stream_type() == rs.stream.infrared and profile.stream_index() == 1 : # Currently stream index does not match source
|
|
ir1_profile = profile
|
|
break
|
|
for profile in sensor.get_stream_profiles() :
|
|
if profile.stream_type() == rs.stream.infrared and profile.stream_index() == 2 :
|
|
ir2_profile = profile
|
|
break
|
|
sensor = sensors['RGB Camera']
|
|
for profile in sensor.get_stream_profiles() :
|
|
if profile.stream_type() == rs.stream.color :
|
|
color_profile = profile
|
|
break
|
|
sensor = sensors['Motion Module']
|
|
for profile in sensor.get_stream_profiles() :
|
|
if test.check_equal( profile.stream_type(), rs.stream.motion ):
|
|
gyro_profile = profile
|
|
break
|
|
|
|
depth_to_ir1_extrinsics = depth_profile.get_extrinsics_to( ir1_profile )
|
|
expected_rotation = [1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]
|
|
expected_translation = [0.0,0.0,0.0]
|
|
test.check_float_lists( depth_to_ir1_extrinsics.rotation, expected_rotation )
|
|
test.check_float_lists( depth_to_ir1_extrinsics.translation, expected_translation )
|
|
|
|
depth_to_ir2_extrinsics = depth_profile.get_extrinsics_to( ir2_profile )
|
|
expected_rotation = [1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]
|
|
expected_translation = [-0.04986396059393883,0.0,0.0]
|
|
test.check_float_lists( depth_to_ir2_extrinsics.rotation, expected_rotation )
|
|
test.check_float_lists( depth_to_ir2_extrinsics.translation, expected_translation )
|
|
|
|
depth_to_color_extrinsics = depth_profile.get_extrinsics_to( color_profile )
|
|
expected_rotation = [0.9999951720237732,-0.0004076171899214387,-0.00308464583940804,0.00040659401565790176,0.9999998807907104,-0.0003323106502648443,0.0030847808811813593,0.0003310548490844667,0.9999951720237732]
|
|
expected_translation = [0.015078714117407799,4.601718956109835e-06,0.00017121469136327505]
|
|
test.check_float_lists( depth_to_color_extrinsics.rotation, expected_rotation )
|
|
test.check_float_lists( depth_to_color_extrinsics.translation, expected_translation )
|
|
|
|
color_to_depth_extrinsics = color_profile.get_extrinsics_to( depth_profile )
|
|
expected_rotation = [0.9999951720237732,0.00040659401565790176,0.0030847808811813593,-0.0004076171899214387,0.9999998807907104,0.0003310548490844667,-0.00308464583940804,-0.0003323106502648443,0.9999951720237732]
|
|
expected_translation = [-0.015078110620379448,-1.0675736120902002e-05,-0.00021772991749458015]
|
|
test.check_float_lists( color_to_depth_extrinsics.rotation, expected_rotation )
|
|
test.check_float_lists( color_to_depth_extrinsics.translation, expected_translation )
|
|
|
|
#color_to_accel_extrinsics = color_profile.get_extrinsics_to( accel_profile )
|
|
#expected_rotation = [0.9999951720237732,0.00040659401565790176,0.0030847808811813593,-0.0004076171899214387,0.9999998807907104,0.0003310548490844667,-0.00308464583940804,-0.0003323106502648443,0.9999951720237732]
|
|
#expected_translation = [-0.02059810981154442,0.0050893244333565235,0.011522269807755947]
|
|
#test.check_float_lists( color_to_accel_extrinsics.rotation, expected_rotation )
|
|
#test.check_float_lists( color_to_accel_extrinsics.translation, expected_translation )
|
|
|
|
gyro_to_ir1_extrinsics = gyro_profile.get_extrinsics_to( ir1_profile )
|
|
expected_rotation = [1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]
|
|
expected_translation = [0.005520000122487545,-0.005100000184029341,-0.011739999987185001]
|
|
test.check_float_lists( gyro_to_ir1_extrinsics.rotation, expected_rotation )
|
|
test.check_float_lists( gyro_to_ir1_extrinsics.translation, expected_translation )
|
|
|
|
remote.run( 'close_server( instance )' )
|
|
except:
|
|
test.unexpected_exception()
|
|
dev = None
|
|
test.finish()
|
|
#
|
|
#############################################################################################
|
|
|
|
context = None
|
|
test.print_results_and_exit()
|