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 unittest
21
22import numpy
23
24
25import cv2
26import capture_request_utils
27import image_processing_utils
28
29ANGLE_CHECK_TOL = 1  # degrees
30ANGLE_NUM_MIN = 10  # Minimum number of angles for find_angle() to be valid
31
32
33TEST_IMG_DIR = os.path.join(os.environ['CAMERA_ITS_TOP'], 'test_images')
34CHART_FILE = os.path.join(TEST_IMG_DIR, 'ISO12233.png')
35CHART_HEIGHT = 13.5  # cm
36CHART_DISTANCE_RFOV = 31.0  # cm
37CHART_DISTANCE_WFOV = 22.0  # cm
38CHART_SCALE_START = 0.65
39CHART_SCALE_STOP = 1.35
40CHART_SCALE_STEP = 0.025
41
42CIRCLE_AR_ATOL = 0.1  # circle aspect ratio tolerance
43CIRCLISH_ATOL = 0.10  # contour area vs ideal circle area & aspect ratio TOL
44CIRCLISH_LOW_RES_ATOL = 0.15  # loosen for low res images
45CIRCLE_MIN_PTS = 20
46CIRCLE_RADIUS_NUMPTS_THRESH = 2  # contour num_pts/radius: empirically ~3x
47
48CV2_RED = (255, 0, 0)  # color in cv2 to draw lines
49
50FOV_THRESH_TELE25 = 25
51FOV_THRESH_TELE40 = 40
52FOV_THRESH_TELE = 60
53FOV_THRESH_WFOV = 90
54
55LOW_RES_IMG_THRESH = 320 * 240
56
57RGB_GRAY_WEIGHTS = (0.299, 0.587, 0.114)  # RGB to Gray conversion matrix
58
59SCALE_RFOV_IN_WFOV_BOX = 0.67
60SCALE_TELE_IN_WFOV_BOX = 0.5
61SCALE_TELE_IN_RFOV_BOX = 0.67
62SCALE_TELE40_IN_RFOV_BOX = 0.5
63SCALE_TELE25_IN_RFOV_BOX = 0.33
64
65SQUARE_AREA_MIN_REL = 0.05  # Minimum size for square relative to image area
66SQUARE_TOL = 0.1  # Square W vs H mismatch RTOL
67
68VGA_HEIGHT = 480
69VGA_WIDTH = 640
70
71
72def find_all_contours(img):
73  cv2_version = cv2.__version__
74  logging.debug('cv2_version: %s', cv2_version)
75  if cv2_version.startswith('3.'):  # OpenCV 3.x
76    _, contours, _ = cv2.findContours(img, cv2.RETR_TREE,
77                                      cv2.CHAIN_APPROX_SIMPLE)
78  else:  # OpenCV 2.x and 4.x
79    contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
80  return contours
81
82
83def calc_chart_scaling(chart_distance, camera_fov):
84  """Returns charts scaling factor.
85
86  Args:
87   chart_distance: float; distance in cm from camera of displayed chart
88   camera_fov: float; camera field of view.
89
90  Returns:
91   chart_scaling: float; scaling factor for chart
92  """
93  chart_scaling = 1.0
94  camera_fov = float(camera_fov)
95  if (FOV_THRESH_TELE < camera_fov < FOV_THRESH_WFOV and
96      numpy.isclose(chart_distance, CHART_DISTANCE_WFOV, rtol=0.1)):
97    chart_scaling = SCALE_RFOV_IN_WFOV_BOX
98  elif (camera_fov <= FOV_THRESH_TELE and
99        numpy.isclose(chart_distance, CHART_DISTANCE_WFOV, rtol=0.1)):
100    chart_scaling = SCALE_TELE_IN_WFOV_BOX
101  elif (camera_fov <= FOV_THRESH_TELE25 and
102        numpy.isclose(chart_distance, CHART_DISTANCE_RFOV, rtol=0.1)):
103    chart_scaling = SCALE_TELE25_IN_RFOV_BOX
104  elif (camera_fov <= FOV_THRESH_TELE40 and
105        numpy.isclose(chart_distance, CHART_DISTANCE_RFOV, rtol=0.1)):
106    chart_scaling = SCALE_TELE40_IN_RFOV_BOX
107  elif (camera_fov <= FOV_THRESH_TELE and
108        numpy.isclose(chart_distance, CHART_DISTANCE_RFOV, rtol=0.1)):
109    chart_scaling = SCALE_TELE_IN_RFOV_BOX
110  return chart_scaling
111
112
113def scale_img(img, scale=1.0):
114  """Scale image based on a real number scale factor."""
115  dim = (int(img.shape[1] * scale), int(img.shape[0] * scale))
116  return cv2.resize(img.copy(), dim, interpolation=cv2.INTER_AREA)
117
118
119def gray_scale_img(img):
120  """Return gray scale version of image."""
121  if len(img.shape) == 2:
122    img_gray = img.copy()
123  elif len(img.shape) == 3:
124    if img.shape[2] == 1:
125      img_gray = img[:, :, 0].copy()
126    else:
127      img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
128  return img_gray
129
130
131class Chart(object):
132  """Definition for chart object.
133
134  Defines PNG reference file, chart, size, distance and scaling range.
135  """
136
137  def __init__(
138      self,
139      cam,
140      props,
141      log_path,
142      chart_loc=None,
143      chart_file=None,
144      height=None,
145      distance=None,
146      scale_start=None,
147      scale_stop=None,
148      scale_step=None):
149    """Initial constructor for class.
150
151    Args:
152     cam: open ITS session
153     props: camera properties object
154     log_path: log path to store the captured images.
155     chart_loc: chart locator arg.
156     chart_file: str; absolute path to png file of chart
157     height: float; height in cm of displayed chart
158     distance: float; distance in cm from camera of displayed chart
159     scale_start: float; start value for scaling for chart search
160     scale_stop: float; stop value for scaling for chart search
161     scale_step: float; step value for scaling for chart search
162    """
163    self._file = chart_file or CHART_FILE
164    self._height = height or CHART_HEIGHT
165    self._distance = distance or CHART_DISTANCE_RFOV
166    self._scale_start = scale_start or CHART_SCALE_START
167    self._scale_stop = scale_stop or CHART_SCALE_STOP
168    self._scale_step = scale_step or CHART_SCALE_STEP
169    self.xnorm, self.ynorm, self.wnorm, self.hnorm, self.scale = (
170        image_processing_utils.chart_located_per_argv(chart_loc))
171    if not self.xnorm:
172      self.locate(cam, props, log_path)
173
174  def _set_scale_factors_to_one(self):
175    """Set scale factors to 1.0 for skipped tests."""
176    self.wnorm = 1.0
177    self.hnorm = 1.0
178    self.xnorm = 0.0
179    self.ynorm = 0.0
180    self.scale = 1.0
181
182  def _calc_scale_factors(self, cam, props, fmt, log_path):
183    """Take an image with s, e, & fd to find the chart location.
184
185    Args:
186     cam: An open its session.
187     props: Properties of cam
188     fmt: Image format for the capture
189     log_path: log path to save the captured images.
190
191    Returns:
192      template: numpy array; chart template for locator
193      img_3a: numpy array; RGB image for chart location
194      scale_factor: float; scaling factor for chart search
195    """
196    req = capture_request_utils.auto_capture_request()
197    cap_chart = image_processing_utils.stationary_lens_cap(cam, req, fmt)
198    img_3a = image_processing_utils.convert_capture_to_rgb_image(
199        cap_chart, props)
200    img_3a = image_processing_utils.rotate_img_per_argv(img_3a)
201    af_scene_name = os.path.join(log_path, 'af_scene.jpg')
202    image_processing_utils.write_image(img_3a, af_scene_name)
203    template = cv2.imread(self._file, cv2.IMREAD_ANYDEPTH)
204    focal_l = cap_chart['metadata']['android.lens.focalLength']
205    pixel_pitch = (
206        props['android.sensor.info.physicalSize']['height'] / img_3a.shape[0])
207    logging.debug('Chart distance: %.2fcm', self._distance)
208    logging.debug('Chart height: %.2fcm', self._height)
209    logging.debug('Focal length: %.2fmm', focal_l)
210    logging.debug('Pixel pitch: %.2fum', pixel_pitch * 1E3)
211    logging.debug('Template height: %dpixels', template.shape[0])
212    chart_pixel_h = self._height * focal_l / (self._distance * pixel_pitch)
213    scale_factor = template.shape[0] / chart_pixel_h
214    logging.debug('Chart/image scale factor = %.2f', scale_factor)
215    return template, img_3a, scale_factor
216
217  def locate(self, cam, props, log_path):
218    """Find the chart in the image, and append location to chart object.
219
220    Args:
221      cam: Open its session.
222      props: Camera properties object.
223      log_path: log path to store the captured images.
224
225    The values appended are:
226    xnorm: float; [0, 1] left loc of chart in scene
227    ynorm: float; [0, 1] top loc of chart in scene
228    wnorm: float; [0, 1] width of chart in scene
229    hnorm: float; [0, 1] height of chart in scene
230    scale: float; scale factor to extract chart
231    """
232    fmt = {'format': 'yuv', 'width': VGA_WIDTH, 'height': VGA_HEIGHT}
233    cam.do_3a()
234    chart, scene, s_factor = self._calc_scale_factors(cam, props, fmt, log_path)
235    scale_start = self._scale_start * s_factor
236    scale_stop = self._scale_stop * s_factor
237    scale_step = self._scale_step * s_factor
238    self.scale = s_factor
239    max_match = []
240    # check for normalized image
241    if numpy.amax(scene) <= 1.0:
242      scene = (scene * 255.0).astype(numpy.uint8)
243    scene_gray = gray_scale_img(scene)
244    logging.debug('Finding chart in scene...')
245    for scale in numpy.arange(scale_start, scale_stop, scale_step):
246      scene_scaled = scale_img(scene_gray, scale)
247      if (scene_scaled.shape[0] < chart.shape[0] or
248          scene_scaled.shape[1] < chart.shape[1]):
249        continue
250      result = cv2.matchTemplate(scene_scaled, chart, cv2.TM_CCOEFF)
251      _, opt_val, _, top_left_scaled = cv2.minMaxLoc(result)
252      logging.debug(' scale factor: %.3f, opt val: %.f', scale, opt_val)
253      max_match.append((opt_val, top_left_scaled))
254
255    # determine if optimization results are valid
256    opt_values = [x[0] for x in max_match]
257    if 2.0 * min(opt_values) > max(opt_values):
258      estring = ('Warning: unable to find chart in scene!\n'
259                 'Check camera distance and self-reported '
260                 'pixel pitch, focal length and hyperfocal distance.')
261      logging.warning(estring)
262      self._set_scale_factors_to_one()
263    else:
264      if (max(opt_values) == opt_values[0] or
265          max(opt_values) == opt_values[len(opt_values) - 1]):
266        estring = ('Warning: Chart is at extreme range of locator.')
267        logging.warning(estring)
268      # find max and draw bbox
269      match_index = max_match.index(max(max_match, key=lambda x: x[0]))
270      self.scale = scale_start + scale_step * match_index
271      logging.debug('Optimum scale factor: %.3f', self.scale)
272      top_left_scaled = max_match[match_index][1]
273      h, w = chart.shape
274      bottom_right_scaled = (top_left_scaled[0] + w, top_left_scaled[1] + h)
275      top_left = ((top_left_scaled[0] // self.scale),
276                  (top_left_scaled[1] // self.scale))
277      bottom_right = ((bottom_right_scaled[0] // self.scale),
278                      (bottom_right_scaled[1] // self.scale))
279      self.wnorm = ((bottom_right[0]) - top_left[0]) / scene.shape[1]
280      self.hnorm = ((bottom_right[1]) - top_left[1]) / scene.shape[0]
281      self.xnorm = (top_left[0]) / scene.shape[1]
282      self.ynorm = (top_left[1]) / scene.shape[0]
283
284
285def component_shape(contour):
286  """Measure the shape of a connected component.
287
288  Args:
289    contour: return from cv2.findContours. A list of pixel coordinates of
290    the contour.
291
292  Returns:
293    The most left, right, top, bottom pixel location, height, width, and
294    the center pixel location of the contour.
295  """
296  shape = {'left': numpy.inf, 'right': 0, 'top': numpy.inf, 'bottom': 0,
297           'width': 0, 'height': 0, 'ctx': 0, 'cty': 0}
298  for pt in contour:
299    if pt[0][0] < shape['left']:
300      shape['left'] = pt[0][0]
301    if pt[0][0] > shape['right']:
302      shape['right'] = pt[0][0]
303    if pt[0][1] < shape['top']:
304      shape['top'] = pt[0][1]
305    if pt[0][1] > shape['bottom']:
306      shape['bottom'] = pt[0][1]
307  shape['width'] = shape['right'] - shape['left'] + 1
308  shape['height'] = shape['bottom'] - shape['top'] + 1
309  shape['ctx'] = (shape['left'] + shape['right']) // 2
310  shape['cty'] = (shape['top'] + shape['bottom']) // 2
311  return shape
312
313
314def find_circle(img, img_name, min_area, color):
315  """Find the circle in the test image.
316
317  Args:
318    img: numpy image array in RGB, with pixel values in [0,255].
319    img_name: string with image info of format and size.
320    min_area: float of minimum area of circle to find
321    color: int of [0 or 255] 0 is black, 255 is white
322
323  Returns:
324    circle = {'x', 'y', 'r', 'w', 'h', 'x_offset', 'y_offset'}
325  """
326  circle = {}
327  img_size = img.shape
328  if img_size[0]*img_size[1] >= LOW_RES_IMG_THRESH:
329    circlish_atol = CIRCLISH_ATOL
330  else:
331    circlish_atol = CIRCLISH_LOW_RES_ATOL
332
333  # convert to gray-scale image
334  img_gray = numpy.dot(img[..., :3], RGB_GRAY_WEIGHTS)
335
336  # otsu threshold to binarize the image
337  _, img_bw = cv2.threshold(numpy.uint8(img_gray), 0, 255,
338                            cv2.THRESH_BINARY + cv2.THRESH_OTSU)
339
340  # find contours
341  contours = find_all_contours(255-img_bw)
342
343  # Check each contour and find the circle bigger than min_area
344  num_circles = 0
345  logging.debug('Initial number of contours: %d', len(contours))
346  for contour in contours:
347    area = cv2.contourArea(contour)
348    num_pts = len(contour)
349    if (area > img_size[0]*img_size[1]*min_area and
350        num_pts >= CIRCLE_MIN_PTS):
351      shape = component_shape(contour)
352      radius = (shape['width'] + shape['height']) / 4
353      colour = img_bw[shape['cty']][shape['ctx']]
354      circlish = (math.pi * radius**2) / area
355      aspect_ratio = shape['width'] / shape['height']
356      logging.debug('Potential circle found. radius: %.2f, color: %d, '
357                    'circlish: %.3f, ar: %.3f, pts: %d', radius, colour,
358                    circlish, aspect_ratio, num_pts)
359      if (colour == color and
360          numpy.isclose(1.0, circlish, atol=circlish_atol) and
361          numpy.isclose(1.0, aspect_ratio, atol=CIRCLE_AR_ATOL) and
362          num_pts/radius >= CIRCLE_RADIUS_NUMPTS_THRESH):
363
364        # Populate circle dictionary
365        circle['x'] = shape['ctx']
366        circle['y'] = shape['cty']
367        circle['r'] = (shape['width'] + shape['height']) / 4
368        circle['w'] = float(shape['width'])
369        circle['h'] = float(shape['height'])
370        circle['x_offset'] = (shape['ctx'] - img_size[1]//2) / circle['w']
371        circle['y_offset'] = (shape['cty'] - img_size[0]//2) / circle['h']
372        logging.debug('Num pts: %d', num_pts)
373        logging.debug('Aspect ratio: %.3f', aspect_ratio)
374        logging.debug('Circlish value: %.3f', circlish)
375        logging.debug('Location: %.1f x %.1f', circle['x'], circle['y'])
376        logging.debug('Radius: %.3f', circle['r'])
377        logging.debug('Circle center position wrt to image center:%.3fx%.3f',
378                      circle['x_offset'], circle['y_offset'])
379        num_circles += 1
380        # if more than one circle found, break
381        if num_circles == 2:
382          break
383
384  if num_circles == 0:
385    image_processing_utils.write_image(img/255, img_name, True)
386    raise AssertionError('No black circle detected. '
387                         'Please take pictures according to instructions.')
388
389  if num_circles > 1:
390    image_processing_utils.write_image(img/255, img_name, True)
391    raise AssertionError('More than 1 black circle detected. '
392                         'Background of scene may be too complex.')
393
394  return circle
395
396
397def append_circle_center_to_img(circle, img, img_name):
398  """Append circle center and image center to image and save image.
399
400  Draws line from circle center to image center and then labels end-points.
401  Adjusts text positioning depending on circle center wrt image center.
402  Moves text position left/right half of up/down movement for visual aesthetics.
403
404  Args:
405    circle: dict with circle location vals.
406    img: numpy float image array in RGB, with pixel values in [0,255].
407    img_name: string with image info of format and size.
408  """
409  line_width_scaling_factor = 500
410  text_move_scaling_factor = 3
411  img_size = img.shape
412  img_center_x = img_size[1]//2
413  img_center_y = img_size[0]//2
414
415  # draw line from circle to image center
416  line_width = int(max(1, max(img_size)//line_width_scaling_factor))
417  font_size = line_width // 2
418  move_text_dist = line_width * text_move_scaling_factor
419  cv2.line(img, (circle['x'], circle['y']), (img_center_x, img_center_y),
420           CV2_RED, line_width)
421
422  # adjust text location
423  move_text_right_circle = -1
424  move_text_right_image = 2
425  if circle['x'] > img_center_x:
426    move_text_right_circle = 2
427    move_text_right_image = -1
428
429  move_text_down_circle = -1
430  move_text_down_image = 4
431  if circle['y'] > img_center_y:
432    move_text_down_circle = 4
433    move_text_down_image = -1
434
435  # add circles to end points and label
436  radius_pt = line_width * 2  # makes a dot 2x line width
437  filled_pt = -1  # cv2 value for a filled circle
438  # circle center
439  cv2.circle(img, (circle['x'], circle['y']), radius_pt, CV2_RED, filled_pt)
440  text_circle_x = move_text_dist * move_text_right_circle + circle['x']
441  text_circle_y = move_text_dist * move_text_down_circle + circle['y']
442  cv2.putText(img, 'circle center', (text_circle_x, text_circle_y),
443              cv2.FONT_HERSHEY_SIMPLEX, font_size, CV2_RED, line_width)
444  # image center
445  cv2.circle(img, (img_center_x, img_center_y), radius_pt, CV2_RED, filled_pt)
446  text_imgct_x = move_text_dist * move_text_right_image + img_center_x
447  text_imgct_y = move_text_dist * move_text_down_image + img_center_y
448  cv2.putText(img, 'image center', (text_imgct_x, text_imgct_y),
449              cv2.FONT_HERSHEY_SIMPLEX, font_size, CV2_RED, line_width)
450  image_processing_utils.write_image(img/255, img_name, True)  # [0, 1] values
451
452
453def get_angle(input_img):
454  """Computes anglular inclination of chessboard in input_img.
455
456  Args:
457    input_img (2D numpy.ndarray): Grayscale image stored as a 2D numpy array.
458  Returns:
459    Median angle of squares in degrees identified in the image.
460
461  Angle estimation algorithm description:
462    Input: 2D grayscale image of chessboard.
463    Output: Angle of rotation of chessboard perpendicular to
464            chessboard. Assumes chessboard and camera are parallel to
465            each other.
466
467    1) Use adaptive threshold to make image binary
468    2) Find countours
469    3) Filter out small contours
470    4) Filter out all non-square contours
471    5) Compute most common square shape.
472        The assumption here is that the most common square instances are the
473        chessboard squares. We've shown that with our current tuning, we can
474        robustly identify the squares on the sensor fusion chessboard.
475    6) Return median angle of most common square shape.
476
477  USAGE NOTE: This function has been tuned to work for the chessboard used in
478  the sensor_fusion tests. See images in test_images/rotated_chessboard/ for
479  sample captures. If this function is used with other chessboards, it may not
480  work as expected.
481  """
482  # Tuning parameters
483  square_area_min = (float)(input_img.shape[1] * SQUARE_AREA_MIN_REL)
484
485  # Creates copy of image to avoid modifying original.
486  img = numpy.array(input_img, copy=True)
487
488  # Scale pixel values from 0-1 to 0-255
489  img *= 255
490  img = img.astype(numpy.uint8)
491  img_thresh = cv2.adaptiveThreshold(
492      img, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 201, 2)
493
494  # Find all contours.
495  contours = find_all_contours(img_thresh)
496
497  # Filter contours to squares only.
498  square_contours = []
499  for contour in contours:
500    rect = cv2.minAreaRect(contour)
501    _, (width, height), angle = rect
502
503    # Skip non-squares
504    if not numpy.isclose(width, height, rtol=SQUARE_TOL):
505      continue
506
507    # Remove very small contours: usually just tiny dots due to noise.
508    area = cv2.contourArea(contour)
509    if area < square_area_min:
510      continue
511
512    square_contours.append(contour)
513
514  areas = []
515  for contour in square_contours:
516    area = cv2.contourArea(contour)
517    areas.append(area)
518
519  median_area = numpy.median(areas)
520
521  filtered_squares = []
522  filtered_angles = []
523  for square in square_contours:
524    area = cv2.contourArea(square)
525    if not numpy.isclose(area, median_area, rtol=SQUARE_TOL):
526      continue
527
528    filtered_squares.append(square)
529    _, (width, height), angle = cv2.minAreaRect(square)
530    filtered_angles.append(angle)
531
532  if len(filtered_angles) < ANGLE_NUM_MIN:
533    logging.debug(
534        'A frame had too few angles to be processed. '
535        'Num of angles: %d, MIN: %d', len(filtered_angles), ANGLE_NUM_MIN)
536    return None
537
538  return numpy.median(filtered_angles)
539
540
541class Cv2ImageProcessingUtilsTests(unittest.TestCase):
542  """Unit tests for this module."""
543
544  def test_get_angle_identify_rotated_chessboard_angle(self):
545    # Array of the image files and angles containing rotated chessboards.
546    test_cases = [
547        ('', 0),
548        ('_15_ccw', -15),
549        ('_30_ccw', -30),
550        ('_45_ccw', -45),
551        ('_60_ccw', -60),
552        ('_75_ccw', -75),
553    ]
554    test_fails = ''
555
556    # For each rotated image pair (normal, wide), check angle against expected.
557    for suffix, angle in test_cases:
558      # Define image paths.
559      normal_img_path = os.path.join(
560          TEST_IMG_DIR, f'rotated_chessboards/normal{suffix}.jpg')
561      wide_img_path = os.path.join(
562          TEST_IMG_DIR, f'rotated_chessboards/wide{suffix}.jpg')
563
564      # Load and color-convert images.
565      normal_img = cv2.cvtColor(cv2.imread(normal_img_path), cv2.COLOR_BGR2GRAY)
566      wide_img = cv2.cvtColor(cv2.imread(wide_img_path), cv2.COLOR_BGR2GRAY)
567
568      # Assert angle as expected.
569      normal = get_angle(normal_img)
570      wide = get_angle(wide_img)
571      valid_angles = (angle, angle+90)  # try both angle & +90 due to squares
572      e_msg = (f'\n Rotation angle test failed: {angle}, extracted normal: '
573               f'{normal:.2f}, wide: {wide:.2f}, valid_angles: {valid_angles}')
574      matched_angles = False
575      for a in valid_angles:
576        if (math.isclose(normal, a, abs_tol=ANGLE_CHECK_TOL) and
577            math.isclose(wide, a, abs_tol=ANGLE_CHECK_TOL)):
578          matched_angles = True
579
580      if not matched_angles:
581        test_fails += e_msg
582
583    self.assertEqual(len(test_fails), 0, test_fails)
584
585
586if __name__ == '__main__':
587  unittest.main()
588