From c2db23d0f0b22a496912454dfaabbdb467d1e7c1 Mon Sep 17 00:00:00 2001
From: Eran <librealsense.eran@gmail.com>
Date: Wed, 20 Mar 2024 12:43:55 +0200
Subject: [PATCH] add scaled-intrinsics test

---
 unit-tests/dds/test-librs-intrinsics.py | 58 ++++++++++++++++++++-----
 1 file changed, 47 insertions(+), 11 deletions(-)

diff --git a/unit-tests/dds/test-librs-intrinsics.py b/unit-tests/dds/test-librs-intrinsics.py
index 1fe35546ee7..7c72a2ce833 100644
--- a/unit-tests/dds/test-librs-intrinsics.py
+++ b/unit-tests/dds/test-librs-intrinsics.py
@@ -11,8 +11,6 @@
 
         import pyrealdds as dds
         import d435i
-        import d405
-        import d455
 
         dds.debug( log.is_debug_on(), log.nested )
 
@@ -22,16 +20,11 @@
         # These are the servers currently broadcast
         servers = dict()
 
-        def broadcast_device( camera, device_info ):
-            """
-            E.g.:
-                instance = broadcast_device( d435i, d435i.device_info )
-            """
+        def broadcast_device( server, device_info ):
             global servers
             instance = device_info.serial
             if not instance:
                 raise RuntimeError( "serial-number must be filled out" )
-            server = camera.build( participant )
             servers[instance] = {
                 'info' : device_info,
                 'server' : server
@@ -39,6 +32,27 @@ def broadcast_device( camera, device_info ):
             server.broadcast( device_info )
             return instance
 
+        def build_scaled_intrinsics_server():
+            color = dds.color_stream_server( 'Color', 'RGB Camera' )
+            color.init_profiles( d435i.color_stream_profiles(), 8 )
+            color.init_options( [] )
+
+            # Add only a single set of intrinsics for 1920x1080, from which we expect to scale to all resolutions:
+            i = dds.video_intrinsics();
+            i.width = 1920
+            i.height = 1080
+            i.principal_point.x = 970.4506225585938
+            i.principal_point.y = 542.8473510742188
+            i.focal_length.x = 1362.133056640625
+            i.focal_length.y = 1362.629638671875
+            i.distortion.model = dds.distortion_model.inverse_brown
+            i.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0]
+            color.set_intrinsics( set( [i] ) )
+
+            dev = dds.device_server( participant, d435i.device_info.topic_root )
+            dev.init( [color], [], {} )
+            return dev
+
         def close_server( instance ):
             """
             Close the instance returned by broadcast_device()
@@ -65,7 +79,7 @@ def close_server( instance ):
     #
     with test.closure( "D435i intrinsics" ):
 
-        remote.run( 'instance = broadcast_device( d435i, d435i.device_info )' )
+        remote.run( 'instance = broadcast_device( d435i.build( participant ), d435i.device_info )' )
         dev = rs.wait_for_devices( context, rs.only_sw_devices, n=1. )
 
         sensors = {sensor.get_info( rs.camera_info.name ) : sensor for sensor in dev.query_sensors()}
@@ -151,8 +165,30 @@ def close_server( instance ):
 
         remote.run( 'close_server( instance )' )
         dev = None
-    #
-    #############################################################################################
+
+    with test.closure( 'Scaled intrinsics' ):
+        remote.run( 'instance = broadcast_device( build_scaled_intrinsics_server(), d435i.device_info )' )
+        dev = rs.wait_for_devices( context, rs.only_sw_devices, n=1. )
+        sensors = dev.query_sensors()
+        test.check_equal( len(sensors), 1 )
+        color = sensors[0]
+        profiles = color.profiles
+        resolutions = set()
+        for p in profiles:
+            vp = p.as_video_stream_profile()
+            if not vp:
+                continue
+            if vp.fps() != 30:
+                continue
+            if vp.format() != rs.format.yuyv:
+                continue
+            res = ( vp.width(), vp.height() )
+            if res not in resolutions:
+                log.out( f'resolution: {res} -> {vp.get_intrinsics()}' )  # throws if there's no intrinsics
+                resolutions.add( res )
+        test.check( len(resolutions) > 5 )
+
+        remote.run( 'close_server( instance )' )
 
 context = None
 test.print_results()