1# Copyright 2016 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"""Image processing utilities using openCV.""" 15 16 17import logging 18import math 19import os 20import pathlib 21import cv2 22import numpy 23 24import capture_request_utils 25import error_util 26import image_processing_utils 27 28ANGLE_CHECK_TOL = 1 # degrees 29ANGLE_NUM_MIN = 10 # Minimum number of angles for find_angle() to be valid 30 31 32TEST_IMG_DIR = os.path.join(os.environ['CAMERA_ITS_TOP'], 'test_images') 33CHART_FILE = os.path.join(TEST_IMG_DIR, 'ISO12233.png') 34CHART_HEIGHT_RFOV = 13.5 # cm 35CHART_HEIGHT_WFOV = 9.5 # cm 36CHART_DISTANCE_RFOV = 31.0 # cm 37CHART_DISTANCE_WFOV = 22.0 # cm 38CHART_SCALE_RTOL = 0.1 39CHART_SCALE_START = 0.65 40CHART_SCALE_STOP = 1.35 41CHART_SCALE_STEP = 0.025 42 43CIRCLE_AR_ATOL = 0.1 # circle aspect ratio tolerance 44CIRCLISH_ATOL = 0.10 # contour area vs ideal circle area & aspect ratio TOL 45CIRCLISH_LOW_RES_ATOL = 0.15 # loosen for low res images 46CIRCLE_MIN_PTS = 20 47CIRCLE_RADIUS_NUMPTS_THRESH = 2 # contour num_pts/radius: empirically ~3x 48CIRCLE_COLOR_ATOL = 0.05 # circle color fill tolerance 49CIRCLE_LOCATION_VARIATION_RTOL = 0.05 # tolerance to remove similar circles 50 51CV2_LINE_THICKNESS = 3 # line thickness for drawing on images 52CV2_RED = (255, 0, 0) # color in cv2 to draw lines 53CV2_THRESHOLD_BLOCK_SIZE = 11 54CV2_THRESHOLD_CONSTANT = 2 55 56CV2_HOME_DIRECTORY = os.path.dirname(cv2.__file__) 57CV2_ALTERNATE_DIRECTORY = pathlib.Path(CV2_HOME_DIRECTORY).parents[3] 58HAARCASCADE_FILE_NAME = 'haarcascade_frontalface_default.xml' 59 60FOV_THRESH_TELE25 = 25 61FOV_THRESH_TELE40 = 40 62FOV_THRESH_TELE = 60 63FOV_THRESH_WFOV = 90 64 65LOW_RES_IMG_THRESH = 320 * 240 66 67RGB_GRAY_WEIGHTS = (0.299, 0.587, 0.114) # RGB to Gray conversion matrix 68 69SCALE_RFOV_IN_WFOV_BOX = 0.67 70SCALE_TELE_IN_WFOV_BOX = 0.5 71SCALE_TELE_IN_RFOV_BOX = 0.67 72SCALE_TELE40_IN_WFOV_BOX = 0.33 73SCALE_TELE40_IN_RFOV_BOX = 0.5 74SCALE_TELE25_IN_RFOV_BOX = 0.33 75 76SQUARE_AREA_MIN_REL = 0.05 # Minimum size for square relative to image area 77SQUARE_CROP_MARGIN = 0 # Set to aid detection of QR codes 78SQUARE_TOL = 0.05 # Square W vs H mismatch RTOL 79SQUARISH_RTOL = 0.10 80SQUARISH_AR_RTOL = 0.10 81 82VGA_HEIGHT = 480 83VGA_WIDTH = 640 84 85 86def convert_to_gray(img): 87 """Returns openCV grayscale image. 88 89 Args: 90 img: A numpy image. 91 Returns: 92 An openCV image converted to grayscale. 93 """ 94 return numpy.dot(img[..., :3], RGB_GRAY_WEIGHTS) 95 96 97def convert_to_y(img): 98 """Returns a Y image from a BGR image. 99 100 Args: 101 img: An openCV image. 102 Returns: 103 An openCV image converted to Y. 104 """ 105 y, _, _ = cv2.split(cv2.cvtColor(img, cv2.COLOR_BGR2YUV)) 106 return y 107 108 109def binarize_image(img_gray): 110 """Returns a binarized image based on cv2 thresholds. 111 112 Args: 113 img_gray: A grayscale openCV image. 114 Returns: 115 An openCV image binarized to 0 (black) and 255 (white). 116 """ 117 _, img_bw = cv2.threshold(numpy.uint8(img_gray), 0, 255, 118 cv2.THRESH_BINARY + cv2.THRESH_OTSU) 119 return img_bw 120 121 122def _load_opencv_haarcascade_file(): 123 """Return Haar Cascade file for face detection.""" 124 for cv2_directory in (CV2_HOME_DIRECTORY, CV2_ALTERNATE_DIRECTORY,): 125 for path, _, files in os.walk(cv2_directory): 126 if HAARCASCADE_FILE_NAME in files: 127 haarcascade_file = os.path.join(path, HAARCASCADE_FILE_NAME) 128 logging.debug('Haar Cascade file location: %s', haarcascade_file) 129 return haarcascade_file 130 raise error_util.CameraItsError('haarcascade_frontalface_default.xml was ' 131 f'not found in {CV2_HOME_DIRECTORY} ' 132 f'or {CV2_ALTERNATE_DIRECTORY}') 133 134 135def find_opencv_faces(img, scale_factor, min_neighbors): 136 """Finds face rectangles with openCV. 137 138 Args: 139 img: numpy array; 3-D RBG image with [0,1] values 140 scale_factor: float, specifies how much image size is reduced at each scale 141 min_neighbors: int, specifies minimum number of neighbors to keep rectangle 142 Returns: 143 List of rectangles with faces 144 """ 145 # prep opencv 146 opencv_haarcascade_file = _load_opencv_haarcascade_file() 147 face_cascade = cv2.CascadeClassifier(opencv_haarcascade_file) 148 img_255 = img * 255 149 img_gray = cv2.cvtColor(img_255.astype(numpy.uint8), cv2.COLOR_RGB2GRAY) 150 151 # find face rectangles with opencv 152 faces_opencv = face_cascade.detectMultiScale( 153 img_gray, scale_factor, min_neighbors) 154 logging.debug('%s', str(faces_opencv)) 155 return faces_opencv 156 157 158def find_all_contours(img): 159 cv2_version = cv2.__version__ 160 logging.debug('cv2_version: %s', cv2_version) 161 if cv2_version.startswith('3.'): # OpenCV 3.x 162 _, contours, _ = cv2.findContours(img, cv2.RETR_TREE, 163 cv2.CHAIN_APPROX_SIMPLE) 164 else: # OpenCV 2.x and 4.x 165 contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 166 return contours 167 168 169def calc_chart_scaling(chart_distance, camera_fov): 170 """Returns charts scaling factor. 171 172 Args: 173 chart_distance: float; distance in cm from camera of displayed chart 174 camera_fov: float; camera field of view. 175 176 Returns: 177 chart_scaling: float; scaling factor for chart 178 """ 179 chart_scaling = 1.0 180 camera_fov = float(camera_fov) 181 if (FOV_THRESH_TELE < camera_fov < FOV_THRESH_WFOV and 182 math.isclose( 183 chart_distance, CHART_DISTANCE_WFOV, rel_tol=CHART_SCALE_RTOL)): 184 chart_scaling = SCALE_RFOV_IN_WFOV_BOX 185 elif (FOV_THRESH_TELE40 < camera_fov <= FOV_THRESH_TELE and 186 math.isclose( 187 chart_distance, CHART_DISTANCE_WFOV, rel_tol=CHART_SCALE_RTOL)): 188 chart_scaling = SCALE_TELE_IN_WFOV_BOX 189 elif (camera_fov <= FOV_THRESH_TELE40 and 190 math.isclose(chart_distance, CHART_DISTANCE_WFOV, rel_tol=CHART_SCALE_RTOL)): 191 chart_scaling = SCALE_TELE40_IN_WFOV_BOX 192 elif (camera_fov <= FOV_THRESH_TELE25 and 193 (math.isclose( 194 chart_distance, CHART_DISTANCE_RFOV, rel_tol=CHART_SCALE_RTOL) or 195 chart_distance > CHART_DISTANCE_RFOV)): 196 chart_scaling = SCALE_TELE25_IN_RFOV_BOX 197 elif (camera_fov <= FOV_THRESH_TELE40 and 198 math.isclose( 199 chart_distance, CHART_DISTANCE_RFOV, rel_tol=CHART_SCALE_RTOL)): 200 chart_scaling = SCALE_TELE40_IN_RFOV_BOX 201 elif (camera_fov <= FOV_THRESH_TELE and 202 math.isclose( 203 chart_distance, CHART_DISTANCE_RFOV, rel_tol=CHART_SCALE_RTOL)): 204 chart_scaling = SCALE_TELE_IN_RFOV_BOX 205 return chart_scaling 206 207 208def scale_img(img, scale=1.0): 209 """Scale image based on a real number scale factor.""" 210 dim = (int(img.shape[1] * scale), int(img.shape[0] * scale)) 211 return cv2.resize(img.copy(), dim, interpolation=cv2.INTER_AREA) 212 213 214def gray_scale_img(img): 215 """Return gray scale version of image.""" 216 if len(img.shape) == 2: 217 img_gray = img.copy() 218 elif len(img.shape) == 3: 219 if img.shape[2] == 1: 220 img_gray = img[:, :, 0].copy() 221 else: 222 img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) 223 return img_gray 224 225 226class Chart(object): 227 """Definition for chart object. 228 229 Defines PNG reference file, chart, size, distance and scaling range. 230 """ 231 232 def __init__( 233 self, 234 cam, 235 props, 236 log_path, 237 chart_file=None, 238 height=None, 239 distance=None, 240 scale_start=None, 241 scale_stop=None, 242 scale_step=None, 243 rotation=None): 244 """Initial constructor for class. 245 246 Args: 247 cam: open ITS session 248 props: camera properties object 249 log_path: log path to store the captured images. 250 chart_file: str; absolute path to png file of chart 251 height: float; height in cm of displayed chart 252 distance: float; distance in cm from camera of displayed chart 253 scale_start: float; start value for scaling for chart search 254 scale_stop: float; stop value for scaling for chart search 255 scale_step: float; step value for scaling for chart search 256 rotation: clockwise rotation in degrees (multiple of 90) or None 257 """ 258 self._file = chart_file or CHART_FILE 259 if math.isclose( 260 distance, CHART_DISTANCE_RFOV, rel_tol=CHART_SCALE_RTOL): 261 self._height = height or CHART_HEIGHT_RFOV 262 self._distance = distance 263 else: 264 self._height = height or CHART_HEIGHT_WFOV 265 self._distance = CHART_DISTANCE_WFOV 266 self._scale_start = scale_start or CHART_SCALE_START 267 self._scale_stop = scale_stop or CHART_SCALE_STOP 268 self._scale_step = scale_step or CHART_SCALE_STEP 269 self.opt_val = None 270 self.locate(cam, props, log_path, rotation) 271 272 def _set_scale_factors_to_one(self): 273 """Set scale factors to 1.0 for skipped tests.""" 274 self.wnorm = 1.0 275 self.hnorm = 1.0 276 self.xnorm = 0.0 277 self.ynorm = 0.0 278 self.scale = 1.0 279 280 def _calc_scale_factors(self, cam, props, fmt, log_path, rotation): 281 """Take an image with s, e, & fd to find the chart location. 282 283 Args: 284 cam: An open its session. 285 props: Properties of cam 286 fmt: Image format for the capture 287 log_path: log path to save the captured images. 288 rotation: clockwise rotation of template in degrees (multiple of 90) or 289 None 290 291 Returns: 292 template: numpy array; chart template for locator 293 img_3a: numpy array; RGB image for chart location 294 scale_factor: float; scaling factor for chart search 295 """ 296 req = capture_request_utils.auto_capture_request() 297 cap_chart = image_processing_utils.stationary_lens_cap(cam, req, fmt) 298 img_3a = image_processing_utils.convert_capture_to_rgb_image( 299 cap_chart, props) 300 img_3a = image_processing_utils.rotate_img_per_argv(img_3a) 301 af_scene_name = os.path.join(log_path, 'af_scene.jpg') 302 image_processing_utils.write_image(img_3a, af_scene_name) 303 template = cv2.imread(self._file, cv2.IMREAD_ANYDEPTH) 304 if rotation is not None: 305 logging.debug('Rotating template by %d degrees', rotation) 306 template = numpy.rot90(template, k=rotation / 90) 307 focal_l = cap_chart['metadata']['android.lens.focalLength'] 308 pixel_pitch = ( 309 props['android.sensor.info.physicalSize']['height'] / img_3a.shape[0]) 310 logging.debug('Chart distance: %.2fcm', self._distance) 311 logging.debug('Chart height: %.2fcm', self._height) 312 logging.debug('Focal length: %.2fmm', focal_l) 313 logging.debug('Pixel pitch: %.2fum', pixel_pitch * 1E3) 314 logging.debug('Template width: %dpixels', template.shape[1]) 315 logging.debug('Template height: %dpixels', template.shape[0]) 316 chart_pixel_h = self._height * focal_l / (self._distance * pixel_pitch) 317 scale_factor = template.shape[0] / chart_pixel_h 318 if rotation == 90 or rotation == 270: 319 # With the landscape to portrait override turned on, the width and height 320 # of the active array, normally w x h, will be h x (w * (h/w)^2). Reduce 321 # the applied scaling by the same factor to compensate for this, because 322 # the chart will take up more of the scene. Assume w > h, since this is 323 # meant for landscape sensors. 324 rotate_physical_aspect = ( 325 props['android.sensor.info.physicalSize']['height'] / 326 props['android.sensor.info.physicalSize']['width']) 327 scale_factor *= rotate_physical_aspect ** 2 328 logging.debug('Chart/image scale factor = %.2f', scale_factor) 329 return template, img_3a, scale_factor 330 331 def locate(self, cam, props, log_path, rotation): 332 """Find the chart in the image, and append location to chart object. 333 334 Args: 335 cam: Open its session. 336 props: Camera properties object. 337 log_path: log path to store the captured images. 338 rotation: clockwise rotation of template in degrees (multiple of 90) or 339 None 340 341 The values appended are: 342 xnorm: float; [0, 1] left loc of chart in scene 343 ynorm: float; [0, 1] top loc of chart in scene 344 wnorm: float; [0, 1] width of chart in scene 345 hnorm: float; [0, 1] height of chart in scene 346 scale: float; scale factor to extract chart 347 opt_val: float; The normalized match optimization value [0, 1] 348 """ 349 fmt = {'format': 'yuv', 'width': VGA_WIDTH, 'height': VGA_HEIGHT} 350 cam.do_3a() 351 chart, scene, s_factor = self._calc_scale_factors(cam, props, fmt, log_path, 352 rotation) 353 scale_start = self._scale_start * s_factor 354 scale_stop = self._scale_stop * s_factor 355 scale_step = self._scale_step * s_factor 356 offset = scale_step / 2 357 self.scale = s_factor 358 logging.debug('scale start: %.3f, stop: %.3f, step: %.3f', 359 scale_start, scale_stop, scale_step) 360 logging.debug('Used offset of %.3f to include stop value.', offset) 361 max_match = [] 362 # check for normalized image 363 if numpy.amax(scene) <= 1.0: 364 scene = (scene * 255.0).astype(numpy.uint8) 365 scene_gray = gray_scale_img(scene) 366 logging.debug('Finding chart in scene...') 367 for scale in numpy.arange(scale_start, scale_stop + offset, scale_step): 368 scene_scaled = scale_img(scene_gray, scale) 369 if (scene_scaled.shape[0] < chart.shape[0] or 370 scene_scaled.shape[1] < chart.shape[1]): 371 logging.debug( 372 'Skipped scale %.3f. scene_scaled shape: %s, chart shape: %s', 373 scale, scene_scaled.shape, chart.shape) 374 continue 375 result = cv2.matchTemplate(scene_scaled, chart, cv2.TM_CCOEFF_NORMED) 376 _, opt_val, _, top_left_scaled = cv2.minMaxLoc(result) 377 logging.debug(' scale factor: %.3f, opt val: %.3f', scale, opt_val) 378 max_match.append((opt_val, scale, top_left_scaled)) 379 380 # determine if optimization results are valid 381 opt_values = [x[0] for x in max_match] 382 if not opt_values or (2.0 * min(opt_values) > max(opt_values)): 383 estring = ('Warning: unable to find chart in scene!\n' 384 'Check camera distance and self-reported ' 385 'pixel pitch, focal length and hyperfocal distance.') 386 logging.warning(estring) 387 self._set_scale_factors_to_one() 388 else: 389 if (max(opt_values) == opt_values[0] or 390 max(opt_values) == opt_values[len(opt_values) - 1]): 391 estring = ('Warning: Chart is at extreme range of locator.') 392 logging.warning(estring) 393 # find max and draw bbox 394 matched_scale_and_loc = max(max_match, key=lambda x: x[0]) 395 self.opt_val = matched_scale_and_loc[0] 396 self.scale = matched_scale_and_loc[1] 397 logging.debug('Optimum scale factor: %.3f', self.scale) 398 logging.debug('Opt val: %.3f', self.opt_val) 399 top_left_scaled = matched_scale_and_loc[2] 400 logging.debug('top_left_scaled: %d, %d', top_left_scaled[0], 401 top_left_scaled[1]) 402 h, w = chart.shape 403 bottom_right_scaled = (top_left_scaled[0] + w, top_left_scaled[1] + h) 404 logging.debug('bottom_right_scaled: %d, %d', bottom_right_scaled[0], 405 bottom_right_scaled[1]) 406 top_left = ((top_left_scaled[0] // self.scale), 407 (top_left_scaled[1] // self.scale)) 408 bottom_right = ((bottom_right_scaled[0] // self.scale), 409 (bottom_right_scaled[1] // self.scale)) 410 self.wnorm = ((bottom_right[0]) - top_left[0]) / scene.shape[1] 411 self.hnorm = ((bottom_right[1]) - top_left[1]) / scene.shape[0] 412 self.xnorm = (top_left[0]) / scene.shape[1] 413 self.ynorm = (top_left[1]) / scene.shape[0] 414 patch = image_processing_utils.get_image_patch(scene, self.xnorm, 415 self.ynorm, self.wnorm, 416 self.hnorm) 417 template_scene_name = os.path.join(log_path, 'template_scene.jpg') 418 image_processing_utils.write_image(patch, template_scene_name) 419 420 421def component_shape(contour): 422 """Measure the shape of a connected component. 423 424 Args: 425 contour: return from cv2.findContours. A list of pixel coordinates of 426 the contour. 427 428 Returns: 429 The most left, right, top, bottom pixel location, height, width, and 430 the center pixel location of the contour. 431 """ 432 shape = {'left': numpy.inf, 'right': 0, 'top': numpy.inf, 'bottom': 0, 433 'width': 0, 'height': 0, 'ctx': 0, 'cty': 0} 434 for pt in contour: 435 if pt[0][0] < shape['left']: 436 shape['left'] = pt[0][0] 437 if pt[0][0] > shape['right']: 438 shape['right'] = pt[0][0] 439 if pt[0][1] < shape['top']: 440 shape['top'] = pt[0][1] 441 if pt[0][1] > shape['bottom']: 442 shape['bottom'] = pt[0][1] 443 shape['width'] = shape['right'] - shape['left'] + 1 444 shape['height'] = shape['bottom'] - shape['top'] + 1 445 shape['ctx'] = (shape['left'] + shape['right']) // 2 446 shape['cty'] = (shape['top'] + shape['bottom']) // 2 447 return shape 448 449 450def find_circle_fill_metric(shape, img_bw, color): 451 """Find the proportion of points matching a desired color on a shape's axes. 452 453 Args: 454 shape: dictionary returned by component_shape(...) 455 img_bw: binarized numpy image array 456 color: int of [0 or 255] 0 is black, 255 is white 457 Returns: 458 float: number of x, y axis points matching color / total x, y axis points 459 """ 460 matching = 0 461 total = 0 462 for y in range(shape['top'], shape['bottom']): 463 total += 1 464 matching += 1 if img_bw[y][shape['ctx']] == color else 0 465 for x in range(shape['left'], shape['right']): 466 total += 1 467 matching += 1 if img_bw[shape['cty']][x] == color else 0 468 logging.debug('Found %d matching points out of %d', matching, total) 469 return matching / total 470 471 472def find_circle(img, img_name, min_area, color, use_adaptive_threshold=False): 473 """Find the circle in the test image. 474 475 Args: 476 img: numpy image array in RGB, with pixel values in [0,255]. 477 img_name: string with image info of format and size. 478 min_area: float of minimum area of circle to find 479 color: int of [0 or 255] 0 is black, 255 is white 480 use_adaptive_threshold: True if binarization should use adaptive threshold. 481 482 Returns: 483 circle = {'x', 'y', 'r', 'w', 'h', 'x_offset', 'y_offset'} 484 """ 485 circle = {} 486 img_size = img.shape 487 if img_size[0]*img_size[1] >= LOW_RES_IMG_THRESH: 488 circlish_atol = CIRCLISH_ATOL 489 else: 490 circlish_atol = CIRCLISH_LOW_RES_ATOL 491 492 # convert to gray-scale image and binarize using adaptive/global threshold 493 if use_adaptive_threshold: 494 img_gray = cv2.cvtColor(img.astype(numpy.uint8), cv2.COLOR_BGR2GRAY) 495 img_bw = cv2.adaptiveThreshold(img_gray, 255, cv2.ADAPTIVE_THRESH_MEAN_C, 496 cv2.THRESH_BINARY, CV2_THRESHOLD_BLOCK_SIZE, 497 CV2_THRESHOLD_CONSTANT) 498 else: 499 img_gray = convert_to_gray(img) 500 img_bw = binarize_image(img_gray) 501 502 # find contours 503 contours = find_all_contours(255-img_bw) 504 505 # Check each contour and find the circle bigger than min_area 506 num_circles = 0 507 circle_contours = [] 508 logging.debug('Initial number of contours: %d', len(contours)) 509 for contour in contours: 510 area = cv2.contourArea(contour) 511 num_pts = len(contour) 512 if (area > img_size[0]*img_size[1]*min_area and 513 num_pts >= CIRCLE_MIN_PTS): 514 shape = component_shape(contour) 515 radius = (shape['width'] + shape['height']) / 4 516 colour = img_bw[shape['cty']][shape['ctx']] 517 circlish = (math.pi * radius**2) / area 518 aspect_ratio = shape['width'] / shape['height'] 519 fill = find_circle_fill_metric(shape, img_bw, color) 520 logging.debug('Potential circle found. radius: %.2f, color: %d, ' 521 'circlish: %.3f, ar: %.3f, pts: %d, fill metric: %.3f', 522 radius, colour, circlish, aspect_ratio, num_pts, fill) 523 if (colour == color and 524 math.isclose(1.0, circlish, abs_tol=circlish_atol) and 525 math.isclose(1.0, aspect_ratio, abs_tol=CIRCLE_AR_ATOL) and 526 num_pts/radius >= CIRCLE_RADIUS_NUMPTS_THRESH and 527 math.isclose(1.0, fill, abs_tol=CIRCLE_COLOR_ATOL)): 528 radii = [ 529 image_processing_utils.distance( 530 (shape['ctx'], shape['cty']), numpy.squeeze(point)) 531 for point in contour 532 ] 533 minimum_radius, maximum_radius = min(radii), max(radii) 534 logging.debug('Minimum radius: %.2f, maximum radius: %.2f', 535 minimum_radius, maximum_radius) 536 if circle: 537 old_circle_center = (circle['x'], circle['y']) 538 new_circle_center = (shape['ctx'], shape['cty']) 539 # Based on image height 540 center_distance_atol = img_size[0]*CIRCLE_LOCATION_VARIATION_RTOL 541 if math.isclose( 542 image_processing_utils.distance( 543 old_circle_center, new_circle_center), 544 0, 545 abs_tol=center_distance_atol 546 ) and maximum_radius - minimum_radius < circle['radius_spread']: 547 logging.debug('Replacing the previously found circle. ' 548 'Circle located at %s has a smaller radius spread ' 549 'than the previously found circle at %s. ' 550 'Current radius spread: %.2f, ' 551 'previous radius spread: %.2f', 552 new_circle_center, old_circle_center, 553 maximum_radius - minimum_radius, 554 circle['radius_spread']) 555 circle_contours.pop() 556 circle = {} 557 num_circles -= 1 558 circle_contours.append(contour) 559 560 # Populate circle dictionary 561 circle['x'] = shape['ctx'] 562 circle['y'] = shape['cty'] 563 circle['r'] = (shape['width'] + shape['height']) / 4 564 circle['w'] = float(shape['width']) 565 circle['h'] = float(shape['height']) 566 circle['x_offset'] = (shape['ctx'] - img_size[1]//2) / circle['w'] 567 circle['y_offset'] = (shape['cty'] - img_size[0]//2) / circle['h'] 568 circle['radius_spread'] = maximum_radius - minimum_radius 569 logging.debug('Num pts: %d', num_pts) 570 logging.debug('Aspect ratio: %.3f', aspect_ratio) 571 logging.debug('Circlish value: %.3f', circlish) 572 logging.debug('Location: %.1f x %.1f', circle['x'], circle['y']) 573 logging.debug('Radius: %.3f', circle['r']) 574 logging.debug('Circle center position wrt to image center:%.3fx%.3f', 575 circle['x_offset'], circle['y_offset']) 576 num_circles += 1 577 # if more than one circle found, break 578 if num_circles == 2: 579 break 580 581 if num_circles == 0: 582 image_processing_utils.write_image(img/255, img_name, True) 583 if not use_adaptive_threshold: 584 return find_circle( 585 img, img_name, min_area, color, use_adaptive_threshold=True) 586 else: 587 raise AssertionError('No circle detected. ' 588 'Please take pictures according to instructions.') 589 590 if num_circles > 1: 591 image_processing_utils.write_image(img/255, img_name, True) 592 cv2.drawContours(img, circle_contours, -1, CV2_RED, 593 CV2_LINE_THICKNESS) 594 img_name_parts = img_name.split('.') 595 image_processing_utils.write_image( 596 img/255, f'{img_name_parts[0]}_contours.{img_name_parts[1]}', True) 597 if not use_adaptive_threshold: 598 return find_circle( 599 img, img_name, min_area, color, use_adaptive_threshold=True) 600 raise AssertionError('More than 1 circle detected. ' 601 'Background of scene may be too complex.') 602 603 return circle 604 605 606def find_center_circle(img, img_name, color, circle_ar_rtol, circlish_rtol, 607 min_circle_pts, min_area, debug): 608 """Find circle closest to image center for scene with multiple circles. 609 610 Finds all contours in the image. Rejects those too small and not enough 611 points to qualify as a circle. The remaining contours must have center 612 point of color=color and are sorted based on distance from the center 613 of the image. The contour closest to the center of the image is returned. 614 615 Note: hierarchy is not used as the hierarchy for black circles changes 616 as the zoom level changes. 617 618 Args: 619 img: numpy img array with pixel values in [0,255]. 620 img_name: str file name for saved image 621 color: int 0 --> black, 255 --> white 622 circle_ar_rtol: float aspect ratio relative tolerance 623 circlish_rtol: float contour area vs ideal circle area pi*((w+h)/4)**2 624 min_circle_pts: int minimum number of points to define a circle 625 min_area: int minimum area of circles to screen out 626 debug: bool to save extra data 627 628 Returns: 629 circle: [center_x, center_y, radius] 630 """ 631 632 # gray scale & otsu threshold to binarize the image 633 gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 634 _, img_bw = cv2.threshold( 635 numpy.uint8(gray), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) 636 637 # use OpenCV to find contours (connected components) 638 contours = find_all_contours(255-img_bw) 639 640 # check contours and find the best circle candidates 641 circles = [] 642 img_ctr = [gray.shape[1] // 2, gray.shape[0] // 2] 643 for contour in contours: 644 area = cv2.contourArea(contour) 645 if area > min_area and len(contour) >= min_circle_pts: 646 shape = component_shape(contour) 647 radius = (shape['width'] + shape['height']) / 4 648 colour = img_bw[shape['cty']][shape['ctx']] 649 circlish = round((math.pi * radius**2) / area, 4) 650 if (colour == color and 651 math.isclose(1, circlish, rel_tol=circlish_rtol) and 652 math.isclose(shape['width'], shape['height'], 653 rel_tol=circle_ar_rtol)): 654 circles.append([shape['ctx'], shape['cty'], radius, circlish, area]) 655 656 if not circles: 657 raise AssertionError('No circle was detected. Please take pictures ' 658 'according to instructions carefully!') 659 660 if debug: 661 logging.debug('circles [x, y, r, pi*r**2/area, area]: %s', str(circles)) 662 663 # find circle closest to center 664 circles.sort(key=lambda x: math.hypot(x[0] - img_ctr[0], x[1] - img_ctr[1])) 665 circle = circles[0] 666 667 # mark image center 668 size = gray.shape 669 m_x, m_y = size[1] // 2, size[0] // 2 670 marker_size = CV2_LINE_THICKNESS * 10 671 cv2.drawMarker(img, (m_x, m_y), CV2_RED, markerType=cv2.MARKER_CROSS, 672 markerSize=marker_size, thickness=CV2_LINE_THICKNESS) 673 674 # add circle to saved image 675 center_i = (int(round(circle[0], 0)), int(round(circle[1], 0))) 676 radius_i = int(round(circle[2], 0)) 677 cv2.circle(img, center_i, radius_i, CV2_RED, CV2_LINE_THICKNESS) 678 image_processing_utils.write_image(img / 255.0, img_name) 679 680 return [circle[0], circle[1], circle[2]] 681 682 683def append_circle_center_to_img(circle, img, img_name): 684 """Append circle center and image center to image and save image. 685 686 Draws line from circle center to image center and then labels end-points. 687 Adjusts text positioning depending on circle center wrt image center. 688 Moves text position left/right half of up/down movement for visual aesthetics. 689 690 Args: 691 circle: dict with circle location vals. 692 img: numpy float image array in RGB, with pixel values in [0,255]. 693 img_name: string with image info of format and size. 694 """ 695 line_width_scaling_factor = 500 696 text_move_scaling_factor = 3 697 img_size = img.shape 698 img_center_x = img_size[1]//2 699 img_center_y = img_size[0]//2 700 701 # draw line from circle to image center 702 line_width = int(max(1, max(img_size)//line_width_scaling_factor)) 703 font_size = line_width // 2 704 move_text_dist = line_width * text_move_scaling_factor 705 cv2.line(img, (circle['x'], circle['y']), (img_center_x, img_center_y), 706 CV2_RED, line_width) 707 708 # adjust text location 709 move_text_right_circle = -1 710 move_text_right_image = 2 711 if circle['x'] > img_center_x: 712 move_text_right_circle = 2 713 move_text_right_image = -1 714 715 move_text_down_circle = -1 716 move_text_down_image = 4 717 if circle['y'] > img_center_y: 718 move_text_down_circle = 4 719 move_text_down_image = -1 720 721 # add circles to end points and label 722 radius_pt = line_width * 2 # makes a dot 2x line width 723 filled_pt = -1 # cv2 value for a filled circle 724 # circle center 725 cv2.circle(img, (circle['x'], circle['y']), radius_pt, CV2_RED, filled_pt) 726 text_circle_x = move_text_dist * move_text_right_circle + circle['x'] 727 text_circle_y = move_text_dist * move_text_down_circle + circle['y'] 728 cv2.putText(img, 'circle center', (text_circle_x, text_circle_y), 729 cv2.FONT_HERSHEY_SIMPLEX, font_size, CV2_RED, line_width) 730 # image center 731 cv2.circle(img, (img_center_x, img_center_y), radius_pt, CV2_RED, filled_pt) 732 text_imgct_x = move_text_dist * move_text_right_image + img_center_x 733 text_imgct_y = move_text_dist * move_text_down_image + img_center_y 734 cv2.putText(img, 'image center', (text_imgct_x, text_imgct_y), 735 cv2.FONT_HERSHEY_SIMPLEX, font_size, CV2_RED, line_width) 736 image_processing_utils.write_image(img/255, img_name, True) # [0, 1] values 737 738 739def is_circle_cropped(circle, size): 740 """Determine if a circle is cropped by edge of image. 741 742 Args: 743 circle: list [x, y, radius] of circle 744 size: tuple (x, y) of size of img 745 746 Returns: 747 Boolean True if selected circle is cropped 748 """ 749 750 cropped = False 751 circle_x, circle_y = circle[0], circle[1] 752 circle_r = circle[2] 753 x_min, x_max = circle_x - circle_r, circle_x + circle_r 754 y_min, y_max = circle_y - circle_r, circle_y + circle_r 755 if x_min < 0 or y_min < 0 or x_max > size[0] or y_max > size[1]: 756 cropped = True 757 return cropped 758 759 760def find_white_square(img, min_area): 761 """Find the white square in the test image. 762 763 Args: 764 img: numpy image array in RGB, with pixel values in [0,255]. 765 min_area: float of minimum area of circle to find 766 767 Returns: 768 square = {'left', 'right', 'top', 'bottom', 'width', 'height'} 769 """ 770 square = {} 771 num_squares = 0 772 img_size = img.shape 773 774 # convert to gray-scale image 775 img_gray = convert_to_gray(img) 776 777 # otsu threshold to binarize the image 778 img_bw = binarize_image(img_gray) 779 780 # find contours 781 contours = find_all_contours(img_bw) 782 783 # Check each contour and find the square bigger than min_area 784 logging.debug('Initial number of contours: %d', len(contours)) 785 min_area = img_size[0]*img_size[1]*min_area 786 logging.debug('min_area: %.3f', min_area) 787 for contour in contours: 788 area = cv2.contourArea(contour) 789 num_pts = len(contour) 790 if (area > min_area and num_pts >= 4): 791 shape = component_shape(contour) 792 squarish = (shape['width'] * shape['height']) / area 793 aspect_ratio = shape['width'] / shape['height'] 794 logging.debug('Potential square found. squarish: %.3f, ar: %.3f, pts: %d', 795 squarish, aspect_ratio, num_pts) 796 if (math.isclose(1.0, squarish, abs_tol=SQUARISH_RTOL) and 797 math.isclose(1.0, aspect_ratio, abs_tol=SQUARISH_AR_RTOL)): 798 # Populate square dictionary 799 angle = cv2.minAreaRect(contour)[-1] 800 if angle < -45: 801 angle += 90 802 square['angle'] = angle 803 square['left'] = shape['left'] - SQUARE_CROP_MARGIN 804 square['right'] = shape['right'] + SQUARE_CROP_MARGIN 805 square['top'] = shape['top'] - SQUARE_CROP_MARGIN 806 square['bottom'] = shape['bottom'] + SQUARE_CROP_MARGIN 807 square['w'] = shape['width'] + 2*SQUARE_CROP_MARGIN 808 square['h'] = shape['height'] + 2*SQUARE_CROP_MARGIN 809 num_squares += 1 810 811 if num_squares == 0: 812 raise AssertionError('No white square detected. ' 813 'Please take pictures according to instructions.') 814 if num_squares > 1: 815 raise AssertionError('More than 1 white square detected. ' 816 'Background of scene may be too complex.') 817 return square 818 819 820def get_angle(input_img): 821 """Computes anglular inclination of chessboard in input_img. 822 823 Args: 824 input_img (2D numpy.ndarray): Grayscale image stored as a 2D numpy array. 825 Returns: 826 Median angle of squares in degrees identified in the image. 827 828 Angle estimation algorithm description: 829 Input: 2D grayscale image of chessboard. 830 Output: Angle of rotation of chessboard perpendicular to 831 chessboard. Assumes chessboard and camera are parallel to 832 each other. 833 834 1) Use adaptive threshold to make image binary 835 2) Find countours 836 3) Filter out small contours 837 4) Filter out all non-square contours 838 5) Compute most common square shape. 839 The assumption here is that the most common square instances are the 840 chessboard squares. We've shown that with our current tuning, we can 841 robustly identify the squares on the sensor fusion chessboard. 842 6) Return median angle of most common square shape. 843 844 USAGE NOTE: This function has been tuned to work for the chessboard used in 845 the sensor_fusion tests. See images in test_images/rotated_chessboard/ for 846 sample captures. If this function is used with other chessboards, it may not 847 work as expected. 848 """ 849 # Tuning parameters 850 square_area_min = (float)(input_img.shape[1] * SQUARE_AREA_MIN_REL) 851 852 # Creates copy of image to avoid modifying original. 853 img = numpy.array(input_img, copy=True) 854 855 # Scale pixel values from 0-1 to 0-255 856 img *= 255 857 img = img.astype(numpy.uint8) 858 img_thresh = cv2.adaptiveThreshold( 859 img, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 201, 2) 860 861 # Find all contours. 862 contours = find_all_contours(img_thresh) 863 864 # Filter contours to squares only. 865 square_contours = [] 866 for contour in contours: 867 rect = cv2.minAreaRect(contour) 868 _, (width, height), angle = rect 869 870 # Skip non-squares 871 if not math.isclose(width, height, rel_tol=SQUARE_TOL): 872 continue 873 874 # Remove very small contours: usually just tiny dots due to noise. 875 area = cv2.contourArea(contour) 876 if area < square_area_min: 877 continue 878 879 square_contours.append(contour) 880 881 areas = [] 882 for contour in square_contours: 883 area = cv2.contourArea(contour) 884 areas.append(area) 885 886 median_area = numpy.median(areas) 887 888 filtered_squares = [] 889 filtered_angles = [] 890 for square in square_contours: 891 area = cv2.contourArea(square) 892 if not math.isclose(area, median_area, rel_tol=SQUARE_TOL): 893 continue 894 895 filtered_squares.append(square) 896 _, (width, height), angle = cv2.minAreaRect(square) 897 filtered_angles.append(angle) 898 899 if len(filtered_angles) < ANGLE_NUM_MIN: 900 logging.debug( 901 'A frame had too few angles to be processed. ' 902 'Num of angles: %d, MIN: %d', len(filtered_angles), ANGLE_NUM_MIN) 903 return None 904 905 return numpy.median(filtered_angles) 906