1# Copyright 2023 The Android Open Source Project 2# 3# Licensed under the Apache License, Version 2.0 (the "License"); 4# you may not use this file except in compliance with the License. 5# You may obtain a copy of the License at 6# 7# http://www.apache.org/licenses/LICENSE-2.0 8# 9# Unless required by applicable law or agreed to in writing, software 10# distributed under the License is distributed on an "AS IS" BASIS, 11# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12# See the License for the specific language governing permissions and 13# limitations under the License. 14"""Utility functions for zoom capture. 15""" 16 17from collections.abc import Iterable 18import dataclasses 19import logging 20import math 21from typing import Optional 22import cv2 23from matplotlib import animation 24from matplotlib import ticker 25import matplotlib.pyplot as plt 26import numpy 27from PIL import Image 28 29import camera_properties_utils 30import capture_request_utils 31import image_processing_utils 32import opencv_processing_utils 33 34_CIRCLE_COLOR = 0 # [0: black, 255: white] 35_CIRCLE_AR_RTOL = 0.15 # contour width vs height (aspect ratio) 36_SMOOTH_ZOOM_OFFSET_MONOTONICITY_ATOL = 25 # number of pixels 37_PREVIEW_SMOOTH_ZOOM_OFFSET_MONOTONICITY_ATOL = 75 # number of pixels 38_CIRCLISH_RTOL = 0.05 # contour area vs ideal circle area pi*((w+h)/4)**2 39_CONTOUR_AREA_LOGGING_THRESH = 0.8 # logging tol to cut down spam in log file 40_CV2_LINE_THICKNESS = 3 # line thickness for drawing on images 41_CV2_RED = (255, 0, 0) # color in cv2 to draw lines 42_MIN_AREA_RATIO = 0.00013 # Found empirically with partners 43_MIN_CIRCLE_PTS = 25 44_MIN_FOCUS_DIST_TOL = 0.80 # allow charts a little closer than min 45_OFFSET_ATOL = 15 # number of pixels 46_OFFSET_PLOT_FPS = 2 47_OFFSET_PLOT_INTERVAL = 400 # delay between frames in milliseconds. 48_OFFSET_RTOL_MIN_FD = 0.30 49_RADIUS_RTOL_MIN_FD = 0.15 50 51DEFAULT_FOV_RATIO = 1 # ratio of sub camera's fov over logical camera's fov 52JPEG_STR = 'jpg' 53OFFSET_RTOL = 0.15 54OFFSET_RTOL_SMOOTH_ZOOM = 0.5 # generous RTOL paired with other offset checks 55OFFSET_ATOL_SMOOTH_ZOOM = 75 # generous ATOL paired with other offset checks 56PREFERRED_BASE_ZOOM_RATIO = 1 # Preferred base image for zoom data verification 57PREFERRED_BASE_ZOOM_RATIO_RTOL = 0.1 58PRV_Z_RTOL = 0.02 # 2% variation of zoom ratio between request and result 59RADIUS_RTOL = 0.15 60ZOOM_MAX_THRESH = 9.0 # TODO: b/368666244 - reduce marker size and use 10.0 61ZOOM_MIN_THRESH = 2.0 62ZOOM_RTOL = 0.01 # variation of zoom ratio due to floating point 63 64 65@dataclasses.dataclass 66class ZoomTestData: 67 """Class to store zoom-related metadata for a capture.""" 68 result_zoom: float 69 radius_tol: float 70 offset_tol: float 71 focal_length: Optional[float] = None 72 # (x, y) coordinates of ArUco marker corners in clockwise order from top left. 73 aruco_corners: Optional[Iterable[float]] = None 74 aruco_offset: Optional[float] = None 75 physical_id: int = dataclasses.field(default=None) 76 77 78def get_test_tols_and_cap_size(cam, props, chart_distance, debug): 79 """Determine the tolerance per camera based on test rig and camera params. 80 81 Cameras are pre-filtered to only include supportable cameras. 82 Supportable cameras are: YUV(RGB) 83 84 Args: 85 cam: camera object 86 props: dict; physical camera properties dictionary 87 chart_distance: float; distance to chart in cm 88 debug: boolean; log additional data 89 90 Returns: 91 dict of TOLs with camera focal length as key 92 largest common size across all cameras 93 """ 94 ids = camera_properties_utils.logical_multi_camera_physical_ids(props) 95 physical_props = {} 96 physical_ids = [] 97 for i in ids: 98 physical_props[i] = cam.get_camera_properties_by_id(i) 99 # find YUV capable physical cameras 100 if camera_properties_utils.backward_compatible(physical_props[i]): 101 physical_ids.append(i) 102 103 # find physical camera focal lengths that work well with rig 104 chart_distance_m = abs(chart_distance)/100 # convert CM to M 105 test_tols = {} 106 test_yuv_sizes = [] 107 for i in physical_ids: 108 yuv_sizes = capture_request_utils.get_available_output_sizes( 109 'yuv', physical_props[i]) 110 test_yuv_sizes.append(yuv_sizes) 111 if debug: 112 logging.debug('cam[%s] yuv sizes: %s', i, str(yuv_sizes)) 113 114 # determine if minimum focus distance is less than rig depth 115 min_fd = physical_props[i]['android.lens.info.minimumFocusDistance'] 116 for fl in physical_props[i]['android.lens.info.availableFocalLengths']: 117 logging.debug('cam[%s] min_fd: %.3f (diopters), fl: %.2f', i, min_fd, fl) 118 if (math.isclose(min_fd, 0.0, rel_tol=1E-6) or # fixed focus 119 (1.0/min_fd < chart_distance_m*_MIN_FOCUS_DIST_TOL)): 120 test_tols[fl] = (RADIUS_RTOL, OFFSET_RTOL) 121 else: 122 test_tols[fl] = (_RADIUS_RTOL_MIN_FD, _OFFSET_RTOL_MIN_FD) 123 logging.debug('loosening RTOL for cam[%s]: ' 124 'min focus distance too large.', i) 125 # find intersection of formats for max common format 126 common_sizes = list(set.intersection(*[set(list) for list in test_yuv_sizes])) 127 if debug: 128 logging.debug('common_fmt: %s', max(common_sizes)) 129 130 return test_tols, max(common_sizes) 131 132 133def find_center_circle( 134 img, img_name, size, zoom_ratio, min_zoom_ratio, 135 expected_color=_CIRCLE_COLOR, circle_ar_rtol=_CIRCLE_AR_RTOL, 136 circlish_rtol=_CIRCLISH_RTOL, min_circle_pts=_MIN_CIRCLE_PTS, 137 fov_ratio=DEFAULT_FOV_RATIO, debug=False, draw_color=_CV2_RED, 138 write_img=True): 139 """Find circle closest to image center for scene with multiple circles. 140 141 Finds all contours in the image. Rejects those too small and not enough 142 points to qualify as a circle. The remaining contours must have center 143 point of color=color and are sorted based on distance from the center 144 of the image. The contour closest to the center of the image is returned. 145 If circle is not found due to zoom ratio being larger than ZOOM_MAX_THRESH 146 or the circle being cropped, None is returned. 147 148 Note: hierarchy is not used as the hierarchy for black circles changes 149 as the zoom level changes. 150 151 Args: 152 img: numpy img array with pixel values in [0,255] 153 img_name: str file name for saved image 154 size: [width, height] of the image 155 zoom_ratio: zoom_ratio for the particular capture 156 min_zoom_ratio: min_zoom_ratio supported by the camera device 157 expected_color: int 0 --> black, 255 --> white 158 circle_ar_rtol: float aspect ratio relative tolerance 159 circlish_rtol: float contour area vs ideal circle area pi*((w+h)/4)**2 160 min_circle_pts: int minimum number of points to define a circle 161 fov_ratio: ratio of sub camera over logical camera's field of view 162 debug: bool to save extra data 163 draw_color: cv2 color in RGB to draw circle and circle center on the image 164 write_img: bool: True - save image with circle and center 165 False - don't save image. 166 167 Returns: 168 circle: [center_x, center_y, radius] 169 """ 170 171 width, height = size 172 min_area = ( 173 _MIN_AREA_RATIO * width * height * zoom_ratio * zoom_ratio * fov_ratio) 174 175 # create a copy of image to avoid modification on the original image since 176 # image_processing_utils.convert_image_to_uint8 uses mutable np array methods 177 if debug: 178 img = numpy.ndarray.copy(img) 179 180 # convert [0, 1] image to [0, 255] and cast as uint8 181 if img.dtype != numpy.uint8: 182 img = image_processing_utils.convert_image_to_uint8(img) 183 184 # gray scale & otsu threshold to binarize the image 185 gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 186 _, img_bw = cv2.threshold( 187 numpy.uint8(gray), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) 188 189 # use OpenCV to find contours (connected components) 190 contours = opencv_processing_utils.find_all_contours(255-img_bw) 191 192 # write copy of image for debug purposes 193 if debug: 194 img_copy_name = img_name.split('.')[0] + '_copy.jpg' 195 Image.fromarray((img_bw).astype(numpy.uint8)).save(img_copy_name) 196 197 # check contours and find the best circle candidates 198 circles = [] 199 img_ctr = [gray.shape[1] // 2, gray.shape[0] // 2] 200 logging.debug('img center x,y: %d, %d', img_ctr[0], img_ctr[1]) 201 logging.debug('min area: %d, min circle pts: %d', min_area, min_circle_pts) 202 logging.debug('circlish_rtol: %.3f', circlish_rtol) 203 204 for contour in contours: 205 area = cv2.contourArea(contour) 206 if area > min_area * _CONTOUR_AREA_LOGGING_THRESH: # skip tiny contours 207 logging.debug('area: %d, min_area: %d, num_pts: %d, min_circle_pts: %d', 208 area, min_area, len(contour), min_circle_pts) 209 if area > min_area and len(contour) >= min_circle_pts: 210 shape = opencv_processing_utils.component_shape(contour) 211 radius = (shape['width'] + shape['height']) / 4 212 circle_color = img_bw[shape['cty']][shape['ctx']] 213 circlish = round((math.pi * radius**2) / area, 4) 214 logging.debug('color: %s, circlish: %.2f, WxH: %dx%d', 215 circle_color, circlish, shape['width'], shape['height']) 216 if (circle_color == expected_color and 217 math.isclose(1, circlish, rel_tol=circlish_rtol) and 218 math.isclose(shape['width'], shape['height'], 219 rel_tol=circle_ar_rtol)): 220 logging.debug('circle found: r: %.2f, area: %.2f\n', radius, area) 221 circles.append([shape['ctx'], shape['cty'], radius, circlish, area]) 222 else: 223 logging.debug('circle rejected: bad color, circlish or aspect ratio\n') 224 225 if not circles: 226 zoom_ratio_value = zoom_ratio / min_zoom_ratio 227 if zoom_ratio_value >= ZOOM_MAX_THRESH: 228 logging.debug('No circle was detected, but zoom %.2f exceeds' 229 ' maximum zoom threshold', zoom_ratio_value) 230 return None 231 else: 232 raise AssertionError( 233 'No circle detected for zoom ratio <= ' 234 f'{ZOOM_MAX_THRESH}. ' 235 'Take pictures according to instructions carefully!') 236 else: 237 logging.debug('num of circles found: %s', len(circles)) 238 239 if debug: 240 logging.debug('circles [x, y, r, pi*r**2/area, area]: %s', str(circles)) 241 242 # find circle closest to center 243 circle = min( 244 circles, key=lambda x: math.hypot(x[0] - img_ctr[0], x[1] - img_ctr[1])) 245 246 # check if circle is cropped because of zoom factor 247 if opencv_processing_utils.is_circle_cropped(circle, size): 248 logging.debug('zoom %.2f is too large! Skip further captures', zoom_ratio) 249 return None 250 251 # mark image center 252 size = gray.shape 253 m_x, m_y = size[1] // 2, size[0] // 2 254 marker_size = _CV2_LINE_THICKNESS * 10 255 cv2.drawMarker(img, (m_x, m_y), draw_color, markerType=cv2.MARKER_CROSS, 256 markerSize=marker_size, thickness=_CV2_LINE_THICKNESS) 257 258 # add circle to saved image 259 center_i = (int(round(circle[0], 0)), int(round(circle[1], 0))) 260 radius_i = int(round(circle[2], 0)) 261 cv2.circle(img, center_i, radius_i, draw_color, _CV2_LINE_THICKNESS) 262 if write_img: 263 image_processing_utils.write_image(img / 255.0, img_name) 264 265 return circle 266 267 268def preview_zoom_data_to_string(test_data): 269 """Returns formatted string from test_data. 270 271 Floats are capped at 2 floating points. 272 273 Args: 274 test_data: ZoomTestData with relevant test data. 275 276 Returns: 277 Formatted String 278 """ 279 output = [] 280 for key, value in dataclasses.asdict(test_data).items(): 281 if isinstance(value, float): 282 output.append(f'{key}: {value:.2f}') 283 elif isinstance(value, list): 284 output.append( 285 f"{key}: [{', '.join([f'{item:.2f}' for item in value])}]") 286 else: 287 output.append(f'{key}: {value}') 288 289 return ', '.join(output) 290 291 292def _get_aruco_marker_x_y_offset(aruco_corners, size): 293 """Get the x and y distances from the ArUco marker to the image center. 294 295 Args: 296 aruco_corners: list of 4 Iterables, each tuple is a (x, y) coordinate of a 297 corner. 298 size: Iterable; the width and height of the images. 299 Returns: 300 The x and y distances from the ArUco marker to the center of the image. 301 """ 302 aruco_marker_x, aruco_marker_y = opencv_processing_utils.get_aruco_center( 303 aruco_corners) 304 return aruco_marker_x - size[0] // 2, aruco_marker_y - size[1] // 2 305 306 307def _get_aruco_marker_offset(aruco_corners, size): 308 """Get the distance from the chosen ArUco marker to the center of the image. 309 310 Args: 311 aruco_corners: list of 4 Iterables, each tuple is a (x, y) coordinate of a 312 corner. 313 size: Iterable; the width and height of the images. 314 Returns: 315 The distance from the ArUco marker to the center of the image. 316 """ 317 return math.hypot(*_get_aruco_marker_x_y_offset(aruco_corners, size)) 318 319 320def _get_shortest_focal_length(props): 321 """Return the first available focal length from properties.""" 322 return props['android.lens.info.availableFocalLengths'][0] 323 324 325def _get_average_offset(shared_id, aruco_ids, aruco_corners, size): 326 """Get the average offset a given marker to the image center. 327 328 Args: 329 shared_id: ID of the given marker to find the average offset. 330 aruco_ids: nested Iterables of ArUco marker IDs. 331 aruco_corners: nested Iterables of ArUco marker corners. 332 size: size of the image to calculate image center. 333 Returns: 334 The average offset from the given marker to the image center. 335 """ 336 offsets = [] 337 for ids, corners in zip(aruco_ids, aruco_corners): 338 offsets.append( 339 _get_average_offset_from_single_capture( 340 shared_id, ids, corners, size)) 341 return numpy.mean(offsets) 342 343 344def _get_average_offset_from_single_capture( 345 shared_id, ids, corners, size): 346 """Get the average offset a given marker to a known image's center. 347 348 Args: 349 shared_id: ID of the given marker to find the average offset. 350 ids: Iterable of ArUco marker IDs for single capture test data. 351 corners: Iterable of ArUco marker corners for single capture test data. 352 size: size of the image to calculate image center. 353 Returns: 354 The average offset from the given marker to the image center. 355 """ 356 corresponding_corners = corners[numpy.where(ids == shared_id)[0][0]] 357 return _get_aruco_marker_offset(corresponding_corners, size) 358 359 360def _are_values_non_decreasing(values, abs_tol=0): 361 """Returns True if any values are not decreasing with absolute tolerance.""" 362 return all(x < y + abs_tol for x, y in zip(values, values[1:])) 363 364 365def _are_values_non_increasing(values, abs_tol=0): 366 """Returns True if any values are not increasing with absolute tolerance.""" 367 return all(x > y - abs_tol for x, y in zip(values, values[1:])) 368 369 370def _verify_offset_monotonicity(offsets, monotonicity_atol): 371 """Returns if values continuously increase or decrease with tolerance.""" 372 return ( 373 _are_values_non_decreasing( 374 offsets, monotonicity_atol) or 375 _are_values_non_increasing( 376 offsets, monotonicity_atol) 377 ) 378 379 380def update_zoom_test_data_with_shared_aruco_marker( 381 test_data, aruco_ids, aruco_corners, size): 382 """Update test_data in place with a shared ArUco marker if available. 383 384 Iterates through the list of aruco_ids and aruco_corners to find the shared 385 ArUco marker that is closest to the center across all captures. If found, 386 updates the test_data with the shared marker and its offset from the 387 image center. 388 389 Args: 390 test_data: list of ZoomTestData. 391 aruco_ids: nested Iterables of ArUco marker IDs. 392 aruco_corners: nested Iterables of ArUco marker corners. 393 size: Iterable; the width and height of the images. 394 """ 395 shared_ids = set(list(aruco_ids[0])) 396 for ids in aruco_ids[1:]: 397 shared_ids.intersection_update(list(ids)) 398 # Choose closest shared marker to center of transition image if possible. 399 if shared_ids: 400 for i, (ids, corners) in enumerate(zip(aruco_ids, aruco_corners)): 401 if test_data[i].physical_id != test_data[0].physical_id: 402 transition_aruco_ids = ids 403 transition_aruco_corners = corners 404 shared_id = min( 405 shared_ids, 406 key=lambda i: _get_average_offset_from_single_capture( 407 i, transition_aruco_ids, transition_aruco_corners, size) 408 ) 409 break 410 else: 411 shared_id = min( 412 shared_ids, 413 key=lambda i: _get_average_offset(i, aruco_ids, aruco_corners, size) 414 ) 415 else: 416 raise AssertionError('No shared ArUco marker found across all captures.') 417 logging.debug('Using shared aruco ID %d', shared_id) 418 for i, (ids, corners) in enumerate(zip(aruco_ids, aruco_corners)): 419 index = numpy.where(ids == shared_id)[0][0] 420 corresponding_corners = corners[index] 421 logging.debug('Corners of shared ID: %s', corresponding_corners) 422 test_data[i].aruco_corners = corresponding_corners 423 test_data[i].aruco_offset = ( 424 _get_aruco_marker_offset( 425 corresponding_corners, size 426 ) 427 ) 428 429 430def verify_zoom_results(test_data, size, z_max, z_min, 431 offset_plot_name_stem=None): 432 """Verify that the output images' zoom level reflects the correct zoom ratios. 433 434 This test verifies that the center and radius of the circles in the output 435 images reflects the zoom ratios being set. The larger the zoom ratio, the 436 larger the circle. And the distance from the center of the circle to the 437 center of the image is proportional to the zoom ratio as well. 438 439 Args: 440 test_data: Iterable[ZoomTestData] 441 size: array; the width and height of the images 442 z_max: float; the maximum zoom ratio being tested 443 z_min: float; the minimum zoom ratio being tested 444 offset_plot_name_stem: Optional[str]; log path and name of the offset plot 445 446 Returns: 447 Boolean whether the test passes (True) or not (False) 448 """ 449 # assert some range is tested before circles get too big 450 test_success = True 451 452 zoom_max_thresh = ZOOM_MAX_THRESH 453 z_max_ratio = z_max / z_min 454 if z_max_ratio < ZOOM_MAX_THRESH: 455 zoom_max_thresh = z_max_ratio 456 457 # handle capture orders like [1, 0.5, 1.5, 2...] 458 test_data_zoom_values = [v.result_zoom for v in test_data] 459 test_data_max_z = max(test_data_zoom_values) / min(test_data_zoom_values) 460 logging.debug('test zoom ratio max: %.2f vs threshold %.2f', 461 test_data_max_z, zoom_max_thresh) 462 if not math.isclose( 463 test_data_max_z, zoom_max_thresh, rel_tol=ZOOM_RTOL): 464 test_success = False 465 e_msg = (f'Max zoom ratio tested: {test_data_max_z:.4f}, ' 466 f'range advertised min: {z_min}, max: {z_max} ' 467 f'THRESH: {zoom_max_thresh + ZOOM_RTOL}') 468 logging.error(e_msg) 469 return test_success and verify_zoom_data( 470 test_data, size, offset_plot_name_stem=offset_plot_name_stem) 471 472 473def verify_zoom_data(test_data, size, plot_name_stem=None, 474 offset_plot_name_stem=None, 475 monotonicity_atol=_SMOOTH_ZOOM_OFFSET_MONOTONICITY_ATOL, 476 number_of_cameras_to_test=0): 477 """Verify output images' zoom level reflects the correct zoom ratios. 478 479 This test ensures accurate zoom functionality by verifying that ArUco marker 480 dimensions and positions in the output images scale correctly with applied 481 zoom ratios. Specifically, the ArUco marker side must increase 482 proportionally to the zoom. The marker's center offset from the image center 483 should either scale proportionally with the zoom or converge towards the 484 offset of the initial capture from the physical camera, particularly after 485 a camera switch. 486 487 Args: 488 test_data: Iterable[ZoomTestData] 489 size: array; the width and height of the images 490 plot_name_stem: Optional[str]; log path and name of the plot 491 offset_plot_name_stem: Optional[str]; log path and name of the offset plot 492 monotonicity_atol: Optional[float]; absolute tolerance for offset 493 monotonicity 494 number_of_cameras_to_test: [Optional][int]; minimum cameras in ZoomTestData 495 496 Returns: 497 Boolean whether the test passes (True) or not (False) 498 """ 499 range_success = True 500 side_success = True 501 offset_success = True 502 used_smooth_offset = False 503 504 # assert that multiple cameras were tested where applicable 505 ids_tested = set([v.physical_id for v in test_data]) 506 if len(ids_tested) < number_of_cameras_to_test: 507 range_success = False 508 logging.error('Expected at least %d physical cameras tested, ' 509 'found IDs: %s', number_of_cameras_to_test, ids_tested) 510 511 # initialize relative size w/ zoom[0] for diff zoom ratio checks 512 side_0 = opencv_processing_utils.get_aruco_marker_side_length( 513 test_data[0].aruco_corners) 514 z_0 = float(test_data[0].result_zoom) 515 516 # use 1x ~ 1.1x data as base image if available 517 if z_0 < PREFERRED_BASE_ZOOM_RATIO: 518 for data in test_data: 519 if (data.result_zoom >= PREFERRED_BASE_ZOOM_RATIO and 520 math.isclose(data.result_zoom, PREFERRED_BASE_ZOOM_RATIO, 521 rel_tol=PREFERRED_BASE_ZOOM_RATIO_RTOL)): 522 side_0 = opencv_processing_utils.get_aruco_marker_side_length( 523 data.aruco_corners) 524 z_0 = float(data.result_zoom) 525 break 526 logging.debug('Initial zoom: %.3f, Aruco marker length: %.3f', z_0, side_0) 527 if plot_name_stem: 528 frame_numbers = [] 529 z_variations = [] 530 rel_variations = [] 531 radius_tols = [] 532 max_rel_variation = None 533 max_rel_variation_zoom = None 534 offset_x_values = [] 535 offset_y_values = [] 536 hypots = [] 537 538 id_to_next_offset_and_zoom = {} 539 offsets_while_transitioning = [] 540 previous_id = test_data[0].physical_id 541 # First pass to get transition points 542 for i, data in enumerate(test_data): 543 if i == 0: 544 continue 545 if test_data[i-1].physical_id != data.physical_id: 546 id_to_next_offset_and_zoom[previous_id] = ( 547 data.aruco_offset, data.result_zoom 548 ) 549 previous_id = data.physical_id 550 551 initial_offset = test_data[0].aruco_offset 552 initial_zoom = test_data[0].result_zoom 553 # Second pass to check offset correctness 554 for i, data in enumerate(test_data): 555 logging.debug(' ') # add blank line between frames 556 logging.debug('Frame # %d: {%s}', i, preview_zoom_data_to_string(data)) 557 logging.debug('Zoom: %.2f, physical ID: %s', 558 data.result_zoom, data.physical_id) 559 offset_x, offset_y = _get_aruco_marker_x_y_offset(data.aruco_corners, size) 560 offset_x_values.append(offset_x) 561 offset_y_values.append(offset_y) 562 z_ratio = data.result_zoom / z_0 563 logged_data = False 564 565 # check relative size against zoom[0] 566 current_side = opencv_processing_utils.get_aruco_marker_side_length( 567 data.aruco_corners) 568 side_ratio = current_side / side_0 569 570 # Calculate variations 571 z_variation = z_ratio - side_ratio 572 relative_variation = abs(z_variation) / max(abs(z_ratio), abs(side_ratio)) 573 574 # Store values for plotting 575 if plot_name_stem: 576 frame_numbers.append(i) 577 z_variations.append(z_variation) 578 rel_variations.append(relative_variation) 579 radius_tols.append(data.radius_tol) 580 if max_rel_variation is None or relative_variation > max_rel_variation: 581 max_rel_variation = relative_variation 582 max_rel_variation_zoom = data.result_zoom 583 584 logging.debug('r ratio req: %.3f, measured: %.3f', 585 z_ratio, side_ratio) 586 msg = ( 587 f'Marker side ratio: result({data.result_zoom:.3f}/{z_0:.3f}):' 588 f' {z_ratio:.3f}, marker({current_side:.3f}/{side_0:.3f}):' 589 f' {side_ratio:.3f}, RTOL: {data.radius_tol}' 590 ) 591 if not math.isclose(z_ratio, side_ratio, rel_tol=data.radius_tol): 592 side_success = False 593 logging.error(msg) 594 else: 595 logging.debug(msg) 596 597 # check relative offset against init vals w/ no focal length change 598 # set init values for first capture or change in physical cam focal length 599 hypots.append(data.aruco_offset) 600 if i == 0: 601 continue 602 if test_data[i-1].physical_id != data.physical_id: 603 initial_zoom = float(data.result_zoom) 604 initial_offset = data.aruco_offset 605 d_msg = (f'-- init {i} zoom: {data.result_zoom:.2f}, ' 606 f'Initial offset: {initial_offset:.1f}, ' 607 f'Zoom: {z_ratio:.1f} ') 608 logging.debug(d_msg) 609 if offsets_while_transitioning: 610 logging.debug('Offsets while transitioning: %s', 611 offsets_while_transitioning) 612 if used_smooth_offset and not _verify_offset_monotonicity( 613 offsets_while_transitioning, monotonicity_atol): 614 logging.error('Offsets %s are not monotonic', 615 offsets_while_transitioning) 616 offset_success = False 617 offsets_while_transitioning.clear() 618 else: 619 offsets_while_transitioning.append(data.aruco_offset) 620 z_ratio = data.result_zoom / initial_zoom 621 # Expected offset based on the current zoom ratio and initial offset 622 offset_hypot_rel = data.aruco_offset / z_ratio 623 rel_tol = data.offset_tol 624 msg = (f'Frame # {i} zoom: {data.result_zoom:.2f}, ' 625 f'Baseline offset value: {initial_offset:.4f}, ' 626 f'Expected offset: {offset_hypot_rel:.4f}, ' 627 f'Zoom: {z_ratio:.1f}, ' 628 f'RTOL: {rel_tol}, ATOL: {_OFFSET_ATOL}') 629 if not math.isclose(initial_offset, offset_hypot_rel, 630 rel_tol=rel_tol, abs_tol=_OFFSET_ATOL): 631 logging.warning('Offset check failed. %s', msg) 632 used_smooth_offset = True 633 if data.physical_id not in id_to_next_offset_and_zoom: 634 offset_success = False 635 logging.error('No physical camera is available to explain ' 636 'offset changes!') 637 else: 638 next_initial_offset, next_initial_zoom = ( 639 id_to_next_offset_and_zoom[data.physical_id] 640 ) 641 next_offset_scaled_by_next_zoom = ( 642 next_initial_offset / next_initial_zoom 643 ) 644 absolutely_close = ( 645 math.isclose(next_initial_offset, data.aruco_offset, 646 rel_tol=OFFSET_RTOL_SMOOTH_ZOOM, 647 abs_tol=OFFSET_ATOL_SMOOTH_ZOOM) 648 ) 649 relatively_close = ( 650 math.isclose(next_offset_scaled_by_next_zoom, offset_hypot_rel, 651 rel_tol=OFFSET_RTOL_SMOOTH_ZOOM, 652 abs_tol=OFFSET_ATOL_SMOOTH_ZOOM) 653 ) 654 if not absolutely_close and not relatively_close: 655 offset_success = False 656 e_msg = ('Current offset did not match upcoming physical camera! ' 657 f'{i} zoom: {data.result_zoom:.2f}, ' 658 f'next initial offset: {next_initial_offset:.1f}, ' 659 f'current offset: {data.aruco_offset:.1f}, ' 660 f'current scaled offset: {offset_hypot_rel:.1f}, ' 661 'next offset scaled according to next zoom: ' 662 f'{next_offset_scaled_by_next_zoom:.1f}, ' 663 f'RTOL: {OFFSET_RTOL_SMOOTH_ZOOM}, ' 664 f'ATOL: {OFFSET_ATOL_SMOOTH_ZOOM}') 665 logging.error(e_msg) 666 else: 667 logging.debug('Successfully matched current offset with upcoming ' 668 'physical camera offset') 669 if not logged_data: 670 logging.debug(msg) 671 672 if plot_name_stem: 673 plot_name = plot_name_stem.split('/')[-1].split('.')[0] 674 # Don't change print to logging. Used for KPI. 675 print(f'{plot_name}_max_rel_variation: ', max_rel_variation) 676 print(f'{plot_name}_max_rel_variation_zoom: ', max_rel_variation_zoom) 677 678 # Calculate RMS values 679 rms_z_variations = numpy.sqrt(numpy.mean(numpy.square(z_variations))) 680 rms_rel_variations = numpy.sqrt(numpy.mean(numpy.square(rel_variations))) 681 682 # Print RMS values 683 print(f'{plot_name}_rms_z_variations: ', rms_z_variations) 684 print(f'{plot_name}_rms_rel_variations: ', rms_rel_variations) 685 686 plot_variation(frame_numbers, z_variations, None, 687 f'{plot_name_stem}_variations.png', 'Zoom Variation') 688 plot_variation(frame_numbers, rel_variations, radius_tols, 689 f'{plot_name_stem}_relative.png', 'Relative Variation') 690 691 if offset_plot_name_stem: 692 plot_offset_trajectory( 693 [d.result_zoom for d in test_data], 694 offset_x_values, 695 offset_y_values, 696 hypots, 697 f'{offset_plot_name_stem}_offset_trajectory.gif' # GIF animation 698 ) 699 700 return range_success and side_success and offset_success 701 702 703def verify_preview_zoom_results(test_data, size, z_max, z_min, z_step_size, 704 plot_name_stem, number_of_cameras_to_test=0): 705 """Verify that the output images' zoom level reflects the correct zoom ratios. 706 707 This test verifies that the center and radius of the circles in the output 708 images reflects the zoom ratios being set. The larger the zoom ratio, the 709 larger the circle. And the distance from the center of the circle to the 710 center of the image is proportional to the zoom ratio as well. Verifies 711 that circles are detected throughout the zoom range. 712 713 Args: 714 test_data: Iterable[ZoomTestData] 715 size: array; the width and height of the images 716 z_max: float; the maximum zoom ratio being tested 717 z_min: float; the minimum zoom ratio being tested 718 z_step_size: float; zoom step size to zoom from z_min to z_max 719 plot_name_stem: str; log path and name of the plot 720 number_of_cameras_to_test: [Optional][int]; minimum cameras in ZoomTestData 721 722 Returns: 723 Boolean whether the test passes (True) or not (False) 724 """ 725 test_success = True 726 727 test_data_zoom_values = [v.result_zoom for v in test_data] 728 results_z_max = max(test_data_zoom_values) 729 results_z_min = min(test_data_zoom_values) 730 logging.debug('Capture result: min zoom: %.2f vs max zoom: %.2f', 731 results_z_min, results_z_max) 732 733 # check if max zoom in capture result close to requested zoom range 734 if not (math.isclose(results_z_max, z_max, rel_tol=PRV_Z_RTOL) or 735 math.isclose(results_z_max, z_max - z_step_size, rel_tol=PRV_Z_RTOL)): 736 test_success = False 737 e_msg = (f'Max zoom ratio {results_z_max:.4f} in capture results ' 738 f'is not close to requested zoom ratio {z_max:.2f} or ' 739 f'close to max zoom ratio subtract zoom step ' 740 f'{z_max - z_step_size:.2f} within {PRV_Z_RTOL:.2f}% tolerance.') 741 logging.error(e_msg) 742 else: 743 d_msg = (f'Max zoom ratio in capture results {results_z_max:.2f} is within' 744 f' tolerance of requested max zoom ratio {z_max:.2f}.') 745 logging.debug(d_msg) 746 747 if not math.isclose(results_z_min, z_min, rel_tol=PRV_Z_RTOL): 748 test_success = False 749 e_msg = (f'Min zoom ratio {results_z_min:.4f} in capture results is not ' 750 f'close to requested min zoom {z_min:.2f} within {PRV_Z_RTOL:.2f}%' 751 f' tolerance.') 752 logging.error(e_msg) 753 else: 754 d_msg = (f'Min zoom ratio in capture results {results_z_min:.2f} is within' 755 f' tolerance of requested min zoom ratio {z_min:.2f}.') 756 logging.debug(d_msg) 757 758 return test_success and verify_zoom_data( 759 test_data, size, plot_name_stem=plot_name_stem, 760 monotonicity_atol=_PREVIEW_SMOOTH_ZOOM_OFFSET_MONOTONICITY_ATOL, 761 number_of_cameras_to_test=number_of_cameras_to_test) 762 763 764def get_preview_zoom_params(zoom_range, steps): 765 """Returns zoom min, max, step_size based on zoom range and steps. 766 767 Determine zoom min, max, step_size based on zoom range, steps. 768 Zoom max is capped due to current ITS box size limitation. 769 770 Args: 771 zoom_range: [float,float]; Camera's zoom range 772 steps: int; number of steps 773 774 Returns: 775 zoom_min: minimum zoom 776 zoom_max: maximum zoom 777 zoom_step_size: size of zoom steps 778 """ 779 # Determine test zoom range 780 logging.debug('z_range = %s', str(zoom_range)) 781 zoom_min, zoom_max = float(zoom_range[0]), float(zoom_range[1]) 782 zoom_max = min(zoom_max, ZOOM_MAX_THRESH * zoom_min) 783 784 zoom_step_size = (zoom_max-zoom_min) / (steps-1) 785 logging.debug('zoomRatioRange = %s z_min = %f z_max = %f z_stepSize = %f', 786 str(zoom_range), zoom_min, zoom_max, zoom_step_size) 787 788 return zoom_min, zoom_max, zoom_step_size 789 790 791def plot_variation(frame_numbers, variations, tolerances, plot_name, ylabel): 792 """Plots a variation against frame numbers with corresponding tolerances. 793 794 Args: 795 frame_numbers: List of frame numbers. 796 variations: List of variations. 797 tolerances: List of tolerances corresponding to each variation. 798 plot_name: Name for the plot file. 799 ylabel: Label for the y-axis. 800 """ 801 802 plt.figure(figsize=(40, 10)) 803 804 plt.scatter(frame_numbers, variations, marker='o', linestyle='-', 805 color='blue', label=ylabel) 806 807 if tolerances: 808 plt.plot(frame_numbers, tolerances, linestyle='--', color='red', 809 label='Tolerance') 810 811 plt.xlabel('Frame Number', fontsize=12) 812 plt.ylabel(ylabel, fontsize=12) 813 plt.title(f'{ylabel} vs. Frame Number', fontsize=14) 814 815 plt.legend() 816 817 plt.grid(axis='y', linestyle='--') 818 plt.savefig(plot_name) 819 plt.close() 820 821 822def plot_offset_trajectory( 823 zooms, x_offsets, y_offsets, hypots, plot_name): 824 """Plot an animation describing offset drift for each zoom ratio. 825 826 Args: 827 zooms: Iterable[float]; zoom ratios corresponding to each offset. 828 x_offsets: Iterable[float]; x-axis offsets. 829 y_offsets: Iterable[float]; y-axis offsets. 830 hypots: Iterable[float]; offset hypotenuses (distances from image center). 831 plot_name: Plot name with path to save the plot. 832 """ 833 fig, (ax1, ax2) = plt.subplots(1, 2, constrained_layout=True) 834 fig.suptitle('Zoom Offset Trajectory') 835 scatter = ax1.scatter([], [], c='blue', marker='o') 836 line, = ax1.plot([], [], c='blue', linestyle='dashed') 837 838 # Preset axes limits, since data is added frame by frame (no initial data). 839 ax1.set_xlim(min(x_offsets), max(x_offsets), auto=True) 840 ax1.set_ylim(min(y_offsets), max(y_offsets), auto=True) 841 842 ax1.set_title('Offset (x, y) by Zoom Ratio') 843 ax1.set_xlabel('x') 844 ax1.set_ylabel('y') 845 846 # Function to animate each frame. Each frame corresponds to a capture/zoom. 847 def animate(i): 848 scatter.set_offsets((x_offsets[i], y_offsets[i])) 849 line.set_data(x_offsets[:i+1], y_offsets[:i+1]) 850 ax1.set_title(f'Zoom: {zooms[i]:.3f}') 851 return scatter, line 852 853 ani = animation.FuncAnimation( 854 fig, animate, repeat=True, frames=len(hypots), 855 interval=_OFFSET_PLOT_INTERVAL 856 ) 857 858 ax2.xaxis.set_major_locator(ticker.MultipleLocator(1)) # ticker every 1.0x. 859 ax2.plot(zooms, hypots, '-bo') 860 ax2.set_title('Offset Distance vs. Zoom Ratio') 861 ax2.set_xlabel('Zoom Ratio') 862 ax2.set_ylabel('Offset (pixels)') 863 864 writer = animation.PillowWriter(fps=_OFFSET_PLOT_FPS) 865 ani.save(plot_name, writer=writer) 866