def get_conda_opencv_info(sys_name):
try:
import cv2
except ImportError as e:
print(str(e))
print(
'Please install opencv for python in your current Anaconda environment via the following:\n'
'conda install -c conda-forge opencv=3.4.7')
exit(-1)
assert (cv2.getVersionMajor() == 3) #currently we do not support opencv4
if sys_name == 'Linux':
opencv_lib_dir = os.path.abspath(
os.path.join(os.path.dirname(cv2.__file__), *(['..'] * 2)))
available_opencv_libs = glob.glob(
os.path.join(opencv_lib_dir, 'libopencv*.so'))
elif sys_name == 'Darwin':
opencv_lib_dir = os.path.abspath(
os.path.join(os.path.dirname(cv2.__file__), *(['..'] * 2)))
available_opencv_libs = glob.glob(
os.path.join(opencv_lib_dir, 'libopencv*.dylib'))
else: #sys_name=='Windows'
opencv_lib_dir = os.path.abspath(
os.path.join(os.path.dirname(cv2.__file__), *['..'] * 2, 'Library',
'lib'))
available_opencv_libs = glob.glob(
os.path.join(opencv_lib_dir, 'opencv*.lib'))
opencv_inc_dir = os.path.abspath(
os.path.join(opencv_lib_dir, '..', 'include'))
assert (os.path.exists(opencv_lib_dir))
assert (os.path.exists(opencv_inc_dir))
assert (os.path.exists(os.path.join(opencv_inc_dir, 'opencv2')))
assert (any(available_opencv_libs))
available_opencv_libs = [
os.path.splitext(os.path.basename(it))[0].lstrip('lib')
for it in available_opencv_libs
]
if sys_name == 'Windows':
opencv_libs = [
lib for lib in available_opencv_libs
if any([lib.startswith(it) for it in required_opencv_modules])
]
else:
opencv_libs = [
lib for lib in required_opencv_modules
if lib in available_opencv_libs
]
return opencv_lib_dir, opencv_inc_dir, opencv_libs
def __init__(self, inside, outside, factor=1.0, width=1920, height=1080):
super().__init__()
self.inside = self._scale(inside, factor)
self.outside = self._scale(outside, factor)
self.width = int(width * factor)
self.height = int(height * factor)
opencv_major = cv2.getVersionMajor()
if opencv_major == 3:
self._cv_find_contours = _cv3_find_contours
elif opencv_major == 4:
self._cv_find_contours = _cv4_find_contours
else:
raise ValueError('OpenCV {} is not supported'.format(
cv2.getVersionMajor()))
self.inside_contours = self._find_contours(self.inside, factor)
self.outside_contours = self._find_contours(self.outside, factor)
self.inside_mask = self._create_mask(self.inside, factor)
self.outside_mask = self._create_mask(self.outside, factor)
self.buffer_mask = np.logical_not(
np.logical_or(self.inside_mask, self.outside_mask))
def check_find(img, threshhold, mark_thresh, check_type):
'''
Returns an image labelled with all relevant checks.
'''
if cv2.getVersionMajor() in [2, 4]:
contours, _ = cv2.findContours(threshold, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, contours, _ = cv2.findContours(threshold, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
font = cv2.FONT_HERSHEY_TRIPLEX
document_height, document_width = img.shape[0], img.shape[1]
mark_thresh = float(mark_thresh.strip('%')) / 100.0
for cnt in contours:
approx = cv2.approxPolyDP(cnt, 0.1 * cv2.arcLength(cnt, True), True)
coords = approx.ravel()
# If quadrilateral
if len(approx) == 4:
x, y, x2, y2 = coords[0], coords[1], coords[4], coords[5]
feature_height, feature_width = (y2 - y), (x2 - x)
# If the size of the quadrilateral found is significant (e.g. not hidden inside text)
if feature_width > float(
document_width) / 100 and feature_height > float(
document_width) / 100:
# If a square (± 5 pixels)
if abs(feature_height - feature_width) < 5:
crop_img = img[y:y + feature_height, x:x + feature_width]
# Thresholds the image to binary black and white
_, crop_thresh = cv2.threshold(
crop_img, 127, 255,
cv2.THRESH_BINARY | cv2.THRESH_OTSU)
total = crop_img.shape[0] * crop_img.shape[1]
count_black = total - cv2.countNonZero(crop_thresh)
if count_black > float(total) * mark_thresh and (
check_type == "filled" or check_type == "all"):
cv2.drawContours(img, [approx], 0, (0), 2)
cv2.putText(img, "Filled", (x, y), font, 1, (0))
elif check_type == "empty" or check_type == "all":
cv2.drawContours(img, [approx], 0, (0), 2)
cv2.putText(img, "Empty", (x, y), font, 0.5, (0))
if len(approx) > 15:
# TODO: Do something here if looking for radio buttons.
continue
return (img)
def _diff_pages(self, doc, matched):
ITERATIONS = 6
m1 = doc
m2 = matched
m1 = cv2.cvtColor(m1, cv2.COLOR_BGR2GRAY)
m2 = cv2.cvtColor(m2, cv2.COLOR_BGR2GRAY)
cv2.adaptiveThreshold(m1, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY_INV, 5, 15, m1)
cv2.adaptiveThreshold(m2, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY_INV, 5, 15, m2)
kernel = np.ones((3, 3), np.uint8)
m1 = cv2.dilate(m1, kernel, iterations=ITERATIONS)
m2 = cv2.dilate(m2, kernel, iterations=ITERATIONS)
combined = cv2.bitwise_xor(m1, m2)
combined = cv2.erode(combined, kernel, iterations=ITERATIONS)
if cv2.getVersionMajor() == 3:
# OpenCV 3.4
_, contours, _ = cv2.findContours(combined, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
else:
# OpenCV 4.1.0
contours, _ = cv2.findContours(combined, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
combined = cv2.cvtColor(combined, cv2.COLOR_GRAY2BGR)
good_contours = []
for i, k in enumerate(contours):
_, _, w, h = cv2.boundingRect(k)
if w > sizes.mmToPix(3, self._DPI) and h > sizes.mmToPix(
4, self._DPI):
combined = cv2.drawContours(combined, contours, i, (0, 0, 255),
cv2.FILLED)
good_contours.append(k)
return good_contours
def find_contours(self, img, p1, p2):
self.point1 = (max(0, min(p1[0], p2[0])), max(0, min(p1[1], p2[1])))
self.point2 = (min(max(p1[0], p2[0]),
img.shape[1]), min(max(p1[1], p2[1]), img.shape[0]))
if int(cv2.getVersionMajor()) >= 4:
cntrs, _ = cv2.findContours(img, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, cntrs, _ = cv2.findContours(img, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
boxes = [
cv2.boundingRect(cnt) for cnt in cntrs if self.filter_contour(cnt)
]
boxes.sort(key=lambda b: (b[2], b[3]),
reverse=True) # sort by width/height, larger first.
boxes.sort(key=lambda b: b[0]) # sort by X pos
return np.array(boxes)
def _test_color(self, img):
b = cv2.bitwise_not(cv2.extractChannel(img, 0))
g = cv2.bitwise_not(cv2.extractChannel(img, 1))
r = cv2.bitwise_not(cv2.extractChannel(img, 2))
cv2.threshold(b, 64, 255, cv2.THRESH_BINARY, dst=b)
cv2.threshold(g, 64, 255, cv2.THRESH_BINARY, dst=g)
cv2.threshold(r, 64, 255, cv2.THRESH_BINARY, dst=r)
sign = cv2.bitwise_or(
cv2.bitwise_or(cv2.bitwise_xor(b, r), cv2.bitwise_xor(b, g)),
cv2.bitwise_xor(r, g))
sign = cv2.GaussianBlur(sign, (11, 11), 0.0)
cv2.threshold(sign, 16, 255, cv2.THRESH_BINARY, dst=sign)
if cv2.getVersionMajor() == 3:
# OpenCV 3.4.0
_, contours, _ = cv2.findContours(sign, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
else:
# OpenCV 4.1.0
contours, _ = cv2.findContours(sign, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
combined = cv2.cvtColor(sign, cv2.COLOR_GRAY2BGR)
good_contours = []
for i, k in enumerate(contours):
x, y, w, h = cv2.boundingRect(k)
if w > sizes.mmToPix(3, self._DPI) and h > sizes.mmToPix(
5, self._DPI):
combined = cv2.drawContours(combined,
contours,
i, (0, 0, 255),
thickness=cv2.FILLED)
good_contours.append(k)
return good_contours
def find_Contours(self, lpr_img, green_type):
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(4, 4))
lpr_img = clahe.apply(lpr_img)
if green_type == 'sub':
lpr_img = cv2.GaussianBlur(lpr_img, (5, 5), 0)
lpr_img = cv2.erode(lpr_img,
np.ones((2, 2), np.uint8),
iterations=1)
lpr_img = cv2.dilate(lpr_img,
np.ones((3, 3), np.uint8),
iterations=2)
elif green_type in ['a', 'b']:
lpr_img = cv2.GaussianBlur(lpr_img, (3, 3), 0)
lpr_img = cv2.erode(lpr_img,
np.ones((2, 2), np.uint8),
iterations=1)
lpr_img = cv2.dilate(lpr_img,
np.ones((3, 2), np.uint8),
iterations=1)
lpr_img = imutils.auto_canny(lpr_img)
lpr_img = cv2.copyMakeBorder(lpr_img,
2,
2,
2,
2,
cv2.BORDER_CONSTANT,
value=[0, 0, 0])
lpr_img = cv2.dilate(lpr_img, np.ones((3, 1), np.uint8), iterations=1)
if int(cv2.getVersionMajor()) >= 4:
cntrs, _ = cv2.findContours(lpr_img, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, cntrs, _ = cv2.findContours(lpr_img, cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE)
return cntrs
def check_build_info():
success = True
print("OpenCV Version: {}".format(cv2.__version__))
if (cv2.getVersionMajor() != CURRENT_OPENCV_BUILD_VERSION[0]) and (
cv2.getVersionMinor() != CURRENT_OPENCV_BUILD_VERSION[1]) and (
cv2.getVersionRevision() != CURRENT_OPENCV_BUILD_VERSION[2]):
print("ERROR: OpenCV version is different than the expected.")
success = False
print("Available CPUs: ", cv2.getNumberOfCPUs())
print("Available threads: ", cv2.getNumThreads())
if cv2.getNumThreads() < cv2.getNumberOfCPUs():
print("ERROR: TBB is not enabled.")
success = False
cv2.CPU_NEON = 100 # Value taken from OpenCV doc. CPU labels don't work correctly in Python
print("Cpu NEON support: ", cv2.checkHardwareSupport(cv2.CPU_NEON))
if not cv2.checkHardwareSupport(cv2.CPU_NEON):
print("ERROR: NEON is not enabled.")
success = False
return success
def find_contour_center(img):
ret, thresh = cv2.threshold(img.copy(), int(255 * 0.5), 255, 0)
if cv2.getVersionMajor() in [2, 4]:
contours, hierarchy = cv2.findContours(
thresh,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, contours, hierarchy = cv2.findContours(
thresh,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
print(len(contours))
cx_max = 0
for i, cnt in enumerate(contours):
M = cv2.moments(cnt)
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
if cx_max < cx:
cx_max = cx
cx_result = cx
cy_result = cy
return cx_result, cy_result
def find_rois(confidence,
confidence_gt=None,
confidence_thresh=0.5,
area_thresh=50,
padding=50):
"""Find rois
gtとの比較はlossの方で行う.ここではconfidenceの推論からroiを提案すればよい.
Parametersa
----------
confidence : torch.Tensor or numpy.ndarray
NCHW or HW(numpy.ndarray)
padding : int, optional
Expand roi by padding, by default 50
Returns
-------
rois : torch.Tensor
rois [[[x1, y1, x2, y2], ..., [x1, y1, x2, y2]],
... [x1, y1, x2, y2], ..., [x1, y1, x2, y2]]
len(rois) = n (=batch size)
- example shape
rois_list (2,)
rois_list[0] torch.Size([46, 4])
rois_list[1] torch.Size([38, 4])
The first dimension of rois_list shows a batch,
which contains (the number of roi, (dx, dy, dw, dh)).
rois_center : numpy.ndarray
rois_center [[x, y]]
len(rois_center) = n (=batch size)
"""
if isinstance(confidence, torch.Tensor):
confidence = confidence.cpu().detach().numpy().copy()
elif isinstance(confidence, np.ndarray):
if len(confidence.shape) == 2:
confidence = confidence[None, None, ...]
rois = []
rois_center = []
for n in range(confidence.shape[0]):
rois_n = None
confidence_mask = confidence[n, ...].transpose(1, 2, 0)
confidence_mask[confidence_mask > 1] = 1
confidence_mask[confidence_mask < 0] = 0
confidence_mask *= 255
confidence_mask = confidence_mask.astype(np.uint8)
_, confidence_mask = cv2.threshold(confidence_mask,
int(255 * confidence_thresh), 255,
0)
# if sys.version_info[0] == 2:
if cv2.getVersionMajor() in [2, 4]:
contours, hierarchy = cv2.findContours(confidence_mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, contours, hierarchy = cv2.findContours(confidence_mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) == 0:
# set dummy rois. None にするとroi_alignでerrorが起きる.
rois_n = torch.tensor([[0, 0, 0, 0]],
dtype=torch.float32).to('cuda')
try:
rois.append(rois_n)
rois_center.append(np.array([0, 0]))
except Exception:
print('rois_n', rois_n)
raise
continue
box = None
for i, cnt in enumerate(contours):
try:
area = cv2.contourArea(cnt)
if area < area_thresh:
continue
box = cv2.boundingRect(cnt)
except Exception:
continue
box_center = [int(box[0] + box[2] / 2), int(box[1] + box[3] / 2)]
box = expand_box(box, confidence_mask.shape, padding=padding)
if rois_n is None:
rois_n = torch.tensor(
[[box[0], box[1], box[0] + box[2], box[1] + box[3]]],
dtype=torch.float32).to('cuda')
rois_n_c = np.array([box_center])
else:
rois_n = torch.cat(
(rois_n,
torch.tensor(
[[box[0], box[1], box[0] + box[2], box[1] + box[3]]],
dtype=torch.float32).to('cuda')))
rois_n_c = np.concatenate([rois_n_c, [box_center]])
if rois_n is None:
rois_n = torch.tensor([[0, 0, 0, 0]],
dtype=torch.float32).to('cuda')
rois_n_c = np.array([[0, 0]])
try:
rois.append(rois_n)
rois_center.append(rois_n_c)
except Exception:
print('rois_n', rois_n)
print('rois_n_c', rois_n_c)
raise
# return None, None
return (None, None) if rois == [] else (rois, rois_center)
def main():
cap = cv2.VideoCapture(2)
x = 0
while 1:
""" GET IMAGE MASK """
# load the image, convert it to grayscale, and blur it slightly
ret, image = cap.read()
width = image.shape[1]
height = image.shape[0]
height_adjust = 0.4
width_adjust = 0.4
y = (int)((height - height*height_adjust)*0.5 )-25
h = (int)(height*height_adjust )
x = (int)((width - width*width_adjust)*0.5 ) - 23
w = (int)(width * width_adjust )
image = image[y:y+h, x:x+w]
image = cv2.resize(image, (1920, 1080), interpolation=cv2.INTER_AREA)
gray = image
if ret == True:
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
# threshold the image, then perform a series of erosions +
# dilations to remove any small regions of noise
thresh = cv2.threshold(gray, 140, 170, cv2.THRESH_BINARY)[1]
thresh = cv2.erode(thresh, None, iterations=2)
thresh = cv2.dilate(thresh, None, iterations=2)
""" FIND CONTOURS """
output = image.copy()
contours, hierarchy = [0, 0]
if cv2.getVersionMajor() in [2, 4]:
contours, hierarchy = cv2.findContours(
thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
else:
# OpenCV 3 case
im2, contours, hierarchy = cv2.findContours(
thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
corners = []
dstTri = []
if len(contours) != 0:
# the contours are drawn here
cv2.drawContours(output, contours, -1, 255, 3)
# find the biggest area of the contour
c = max(contours, key=cv2.contourArea)
x, y, w, h = cv2.boundingRect(c)
# draw the 'human' contour (in green)
cv2.rectangle(output, (x, y), (x+w, y+h), (0, 255, 0), 2)
""" FIND CORNERS"""
rc = cv2.minAreaRect(c)
corners = cv2.boxPoints(rc)
#blue is bottom
#green is left
#red is top
# grey is right
cv2.circle(
output, (corners[0][0], corners[0][1]), 5, (200, 0, 0), 2)
# bottom right
cv2.circle(
output, (corners[1][0], corners[1][1]), 5, (0, 200, 0), 2)
# top left
cv2.circle(
output, (corners[2][0], corners[2][1]), 5, (0, 0, 200), 2)
# top right
cv2.circle(
output, (corners[3][0], corners[3][1]), 5, (100, 100, 100), 2)
print(corners)
print(corners[2])
print(corners[2, 0])
print(corners[2, 1])
if corners[1][1] > corners[3][1]:
print(corners[2][0])
dstTri = np.array(
[[corners[2][0],corners[2][1]], [corners[3][0],corners[3][1]], [corners[1][0],corners[1][1]]]).astype(np.float32)
else:
dstTri = np.array([
[corners[1][0],corners[1][1]], [corners[2][0],corners[2][1]], [corners[0][0],corners[0][1]]]).astype(np.float32)
cv2.imshow("Image", image) # original
cv2.imshow("gray", gray) # grayscale + gaussian blur
cv2.imshow("thresh", thresh) # mask
cv2.imshow("Result", output) # Bounding box + corners on biggest blob
""" PROJECTOR STUFF """
src = cv2.imread('pl-boraham.jpg')
if src is None:
print('image not avaible')
exit(0)
src = cv2.resize(src, (1920, 1080), interpolation=cv2.INTER_AREA)
# [Load the image]
srcTri = np.array([[0, 0], [src.shape[1] - 1, 0], [0, src.shape[0] - 1]]).astype(np.float32)
p = 0
name = 'warp'
warp_mat = cv2.getAffineTransform(srcTri, dstTri)
warp_dst = cv2.warpAffine(src, warp_mat, (src.shape[1], src.shape[0]))
cv2.imshow(name, warp_dst)
cv2.waitKey(20)
cv2.namedWindow(name)
#cv2.imshow(name, src)
cv2.moveWindow(name, 0, 0)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows() # Close all the windows
break
def check_find(img, thresholdimg, mark_thresh, check_type):
"""
Returns an image labelled with all relevant checks, and a json with the details
:param img:
:param thresholdimg:
:param mark_thresh:
:param check_type:
:return: image, results
"""
if cv2.getVersionMajor() in [2, 4]:
contours, _ = cv2.findContours(thresholdimg, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
else:
_, contours, _ = cv2.findContours(thresholdimg, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
font = cv2.FONT_HERSHEY_TRIPLEX
document_height, document_width = img.shape[0], img.shape[1]
mark_thresh = float(mark_thresh.strip('%')) / 100.0
results = []
count = 0
for cnt in contours:
approx = cv2.approxPolyDP(cnt, 0.1 * cv2.arcLength(cnt, True), True)
coords = approx.ravel()
# If quadrilateral
if len(approx) == 4:
border_thickness = 0.1
sqaureness = 5 # (pixels)
x, y, x2, y2 = coords[0], coords[1], coords[4], coords[5]
feature_height, feature_width = (y2 - y), (x2 - x)
# If the size of the quadrilateral found is significant (e.g. not hidden inside text)
if feature_width > float(
document_width) / 100 and feature_height > float(
document_width) / 100:
border_thickness_y = math.floor(border_thickness *
feature_height)
border_thickness_x = math.floor(border_thickness *
feature_width)
# If a square (± 5 pixels)
if abs(feature_height - feature_width) < sqaureness:
# Is it a duplicate (based on IOU)
duplicate = False
for result in results:
existingCheckBox = result['boundingbox']
if (bb_intersection_over_union(
[x, y, x2, y2], existingCheckBox) > 0.55):
duplicate = True
if not duplicate:
checkbox = {}
checkbox['boundingbox'] = [x, y, x2, y2]
crop_img = img[y + border_thickness_y:y +
feature_height - border_thickness_y,
x + border_thickness_x:x +
feature_width - border_thickness_x]
# Thresholds the image to binary black and white
_, crop_thresh = cv2.threshold(
crop_img, 127, 255,
cv2.THRESH_BINARY | cv2.THRESH_OTSU)
total = crop_img.shape[0] * crop_img.shape[1]
count_black = total - cv2.countNonZero(crop_thresh)
if count_black > float(total) * mark_thresh and (
check_type == "checked"
or check_type == "all"):
cv2.drawContours(img, [approx], 0, (0), 2)
cv2.putText(img, "Filled", (x, y), font, 1, (0))
checkbox['type'] = "checked"
elif check_type == "empty" or check_type == "all":
cv2.drawContours(img, [approx], 0, (0), 2)
cv2.putText(img, "Empty", (x, y), font, 0.5, (0))
checkbox['type'] = "empty"
results.append(checkbox)
if len(approx) > 15:
# TODO: Do something here if looking for radio buttons.
continue
results = sorted(results, key=lambda checkbox: checkbox['boundingbox'][1])
results = sorted(results, key=lambda checkbox: checkbox['boundingbox'][0])
for idx, result in enumerate(results):
result['id'] = idx
# add ID
return img, results
def cal_steer_angle(self, img, vis_img=None, depth_img=None):
"""
Calculate steering angle for car
:param img: bgr image
:return: steering angle (-60 to 60)
"""
# Init steering angle to 0
steer_angle = 0
# Get birdview image
img_bv = self.bird_view(img)
# Run semantic segmentation on RGB image
seg_masks = self.segmentation.get_masks(img)
if vis_img is not None:
vis_img = self.segmentation.get_visualization_img(
vis_img, seg_masks)
# Get road mask
road_mask = seg_masks[TrafficObject.ROAD.name]
# Filter to get only largest white area in road mask
if cv2.getVersionMajor() in [2, 4]:
# OpenCV 2, OpenCV 4 case
contours, hierarchy = cv2.findContours(road_mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
else:
# OpenCV 3 case
_, contours, hierarchy = cv2.findContours(road_mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# Choose largest contour
best = -1
maxsize = -1
count = 0
for cnt in contours:
if cv2.contourArea(cnt) > maxsize:
maxsize = cv2.contourArea(cnt)
best = count
count = count + 1
road_mask[:, :] = 0
if best != -1:
cv2.drawContours(road_mask, [contours[best]], 0, 255, -1)
# cv2.imshow("Debug", road_mask)
# cv2.waitKey(1)
car_mask = seg_masks[TrafficObject.CAR.name]
perdestrian_mask = seg_masks[TrafficObject.PERDESTRIAN.name]
road_mask = road_mask & cv2.bitwise_not(car_mask)
# Clear car mask if not use it
if not config.USE_CAR_MASK_SEMANTIC_SEG:
car_mask[:, :] = 0
# Use depth image
if depth_img is not None:
obstacle_mask = depth_img & road_mask
car_mask = obstacle_mask | car_mask
if self.debug_stream:
self.debug_stream.update_image('obstacle_mask', obstacle_mask)
# Convert to bird view
road_mask_bv = self.bird_view(road_mask)
# ====== Turning =======
if self.is_turning:
if self.turning_time_begin + config.TURNING_TIME < time.time():
self.is_turning = False
else:
return self.current_turning_direction * config.TURNING_ANGLE, vis_img
else:
interested_area = road_mask_bv[80:180, :]
lane_area = np.count_nonzero(interested_area)
if config.SHOW_AREA:
print("Lane area: {}".format(lane_area))
if lane_area > config.ROAD_AREA_TO_TURN:
print("Turning")
self.is_turning = True
self.turning_time_begin = time.time()
self.current_turning_direction = self.current_traffic_sign
print(self.current_turning_direction)
# Reset traffic sign
self.current_traffic_sign = config.SIGN_NO_SIGN
return self.current_turning_direction * config.TURNING_ANGLE, vis_img
# ====== If not turning, calculate steering angle using middle point =======
# TODO: The method to calculate the middle point and angle now is so simple.
# Research for others in the future
interested_row = road_mask_bv[int(road_mask_bv.shape[0] / 3 *
2), :].reshape((-1, ))
white_pixels = np.argwhere(interested_row > 0)
if white_pixels.size != 0:
middle_pos = np.mean(white_pixels)
else:
middle_pos = 160
if middle_pos != middle_pos: # is NaN
middle_pos = 0
# ====== Obstacle avoidance =======
danger_zone, danger_zone_y = self.obstacle_detector.find_danger_zone(
car_mask, perdestrian_mask)
# print(danger_zone, danger_zone_y)
# Avoid obstacles
if danger_zone != (0, 0):
# 2 objects
if danger_zone[0] == -1:
self.object_avoidance_direction = 0
# middle_pos = danger_zone[1]
# single object
else:
center_danger_zone = int((danger_zone[0] + danger_zone[1]) / 2)
count_road_pixels_left = np.count_nonzero(
road_mask[danger_zone_y, :center_danger_zone])
count_road_pixels_right = np.count_nonzero(
road_mask[danger_zone_y, center_danger_zone:])
# obstacle is on the right
if count_road_pixels_left > count_road_pixels_right:
self.object_avoidance_direction = -1
self.last_object_time = time.time()
# middle_pos = danger_zone[0]
print("OBSTACLE: RIGHT")
# left
elif count_road_pixels_left < count_road_pixels_right:
self.object_avoidance_direction = 1
self.last_object_time = time.time()
# middle_pos = danger_zone[1]
print("OBSTACLE: LEFT")
# Object avoidance
if self.last_object_time > time.time(
) - config.OBSTACLE_AVOIDANCE_TIME:
middle_pos += config.OBSTACLE_AVOIDANCE_OFFSET * self.object_avoidance_direction
print("Obstacle avoidance direction: " +
str(self.object_avoidance_direction))
elif self.last_object_time < time.time(
) - config.OBSTACLE_AVOIDANCE_TIME - 1:
self.object_avoidance_direction = 0
# print("Obstacle was over")
if self.debug_stream:
half_car_width = config.CAR_WIDTH // 2
cv2.line(img_bv, (int(middle_pos), self.h // 2),
(self.w // 2, self.h), (255, 0, 0), 2)
cv2.line(img_bv, (int(middle_pos) + half_car_width, self.h // 2),
(self.w // 2 + half_car_width, self.h), (255, 0, 255), 3)
cv2.line(img_bv, (int(middle_pos) - half_car_width, self.h // 2),
(self.w // 2 - half_car_width, self.h), (255, 0, 255), 3)
self.debug_stream.update_image('car_controlling', img_bv)
# Add offset to middle pos
middle_pos += config.MIDDLE_POS_OFFSET
# Distance between MiddlePos and CarPos
distance_x = middle_pos - self.w / 2
distance_y = self.h - self.h / 3 * 2
# Angle to middle position
steer_angle = math.atan(
float(distance_x) / distance_y) * 180 / math.pi * 1.2
# QIK MATH
# steer_angle = ((middle_pos - 160) / 160) * 60
return steer_angle, vis_img
