def find_epipolar_line_end_points(img, F, p):
img_width = img.shape[1]
el = np.dot(F, np.array([p[0], p[1], 1]).reshape(3, 1))
p1, p2 = (0, -el[2] / el[1]), (img.shape[1],
(-img_width * el[0] - el[2]) / el[1])
_, p1, p2 = cv2.clipLine((0, 0, img.shape[1], img.shape[0]), p1, p2)
return p1, p2
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(width, height) = image_shape(img)
(ret, pix1, pix2) = cv2.clipLine((0, 0, width, height), pix1, pix2)
if ret is False:
if len(self._pix_points) == 0:
self._pix_points.append(None)
self._pix_points.append(None)
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth * 2, colour)
if len(self._pix_points) == 0:
self._pix_points.append(pix1)
self._pix_points.append(pix2)
if self.arrow:
xdiff = pix2[0] - pix1[0]
ydiff = pix2[1] - pix1[1]
if (xdiff * xdiff +
ydiff * ydiff) > 400: # the segment is longer than 20 pix
SlipArrow(
self.key, self.layer,
(int(pix1[0] + xdiff / 2.0), int(pix1[1] + ydiff / 2.0)),
self.colour, self.linewidth,
math.atan2(ydiff, xdiff) + math.pi / 2.0).draw(img)
def draw_seg(img, pt1, pt2, color):
h, w = img.shape[:2]
(ret, pt1, pt2) = cv.clipLine((0, 0, w, h), cvPoint(pt1), cvPoint(pt2))
if ret == False:
return
rr, cc = skdraw.line(int(pt1[1]), int(pt1[0]), int(pt2[1]), int(pt2[0]))
img[rr, cc] = color
def draw_line(self, line: Line):
is_zero_normal_vector = False
v = line.normal_vector
if (0 - 1e-7) < v[0] < (0 + 1e-7): is_zero_normal_vector = True
if (0 - 1e-7) < v[1] < (0 + 1e-7): is_zero_normal_vector = True
if not is_zero_normal_vector:
zero_as_math = image_to_math(Point([0, 0]), self.width,
self.height)[0]
width_as_math = image_to_math(Point([self.width, self.width]),
self.width, self.width)[0]
first_point_math = Point(
[zero_as_math, int(line.get_y(zero_as_math))])
last_point_math = Point(
[width_as_math, int(line.get_y(width_as_math))])
first_point = math_to_image(first_point_math, self.width,
self.height)
last_point = math_to_image(last_point_math, self.width,
self.height)
image_dimension = (0, 0, self.width, self.height)
clipped_line = cv.clipLine(
image_dimension, (int(first_point[0]), int(first_point[1])),
(int(last_point[0]), int(last_point[1])))
if clipped_line[0]:
cv.line(self.image, clipped_line[1], clipped_line[2],
(120, 0, 190))
def calculate_pointing(fingertip, p3, dimensions, d_objects):
l1 = gesture.calc_line(tuple(p3), tuple(fingertip))
l2 = gesture.calc_line((0, dimensions[0]), (dimensions[1], dimensions[0]))
inters = gesture.intersection(l1, l2)
if inters:
# check pointing to target object
p3_int = (int(p3[0]), int(p3[1]))
inters_int = (int(inters[0]), int(inters[1]))
# check pointing quality to objects
pointing_qualities_color = []
for d_obj in d_objects:
color, bb_wh, bb = d_obj[0], d_obj[1], d_obj[2]
retval, intersection1, intersection2 = cv.clipLine(
bb_wh, tuple(p3_int), inters_int)
if retval:
pointing_quality = gesture.calc_poiting_probability(
bb, intersection1, intersection2)
pointing_qualities_color.append([pointing_quality, color])
if len(pointing_qualities_color) > 0:
best_pq = max(pointing_qualities_color, key=lambda x: x[1])
return True, best_pq
else:
return False, [None, None]
else:
return None, [None, None]
def clipped_line_points(image, slope, intercept):
y, x = image.shape
intercept = int(intercept * (y - 1))
pt1 = (0, intercept)
pt2 = (x, int(slope * x) + intercept)
visible, pt1, pt2 = cv2.clipLine((0, 0, x, y), pt1, pt2)
xval, yval = tuple(zip(pt1, pt2))
return xval, yval
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(ret, pix1, pix2) = cv2.clipLine((0, 0, img.shape[1], img.shape[0]), pix1, pix2)
if ret is False:
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth*2, colour)
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(width, height) = image_shape(img)
(ret, pix1, pix2) = cv2.clipLine((0, 0, width, height), pix1, pix2)
if ret is False:
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth * 2, colour)
def DrawLineOnImage(img, l):
d0 = 0
#left of image
d1 = img.shape[1] - 1
#right of image
y0 = int(-(l[0] * d0 + l[2]) / l[1])
y1 = int(-(l[0] * d1 + l[2]) / l[1])
_, pt1, pt2 = cv2.clipLine((0, 0, img.shape[1], img.shape[0]), (d0, y0),
(d1, y1))
cv2.line(img, pt1, pt2, (0, 255, 0), 4)
return
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pt1 = mp_util.constrain_latlon(pt1)
pt2 = mp_util.constrain_latlon(pt2)
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(width, height) = image_shape(img)
(ret, pix1, pix2) = cv2.clipLine((0, 0, width, height), pix1, pix2)
if ret is False:
return
cv2.line(img, pix1, pix2, colour, linewidth)
def line3d(out, pt1, pt2, color=(0x80, 0x80, 0x80), thickness=1):
"""draw a 3d line from pt1 to pt2"""
p0 = project(pt1.reshape(-1, 3))[0]
p1 = project(pt2.reshape(-1, 3))[0]
if np.isnan(p0).any() or np.isnan(p1).any():
return
p0 = tuple(p0.astype(int))
p1 = tuple(p1.astype(int))
rect = (0, 0, out.shape[1], out.shape[0])
inside, p0, p1 = cv2.clipLine(rect, p0, p1)
if inside:
cv2.line(out, p0, p1, color, thickness, cv2.LINE_AA)
def line3d(out, pt1, pt2, color=(0x80, 0x80, 0x80), thickness=1):
"""draw a 3d line from pt1 to pt2"""
p0 = project(pt1.reshape(-1, 3))[0]
p1 = project(pt2.reshape(-1, 3))[0]
if np.isnan(p0).any() or np.isnan(p1).any():
return
p0 = tuple(p0.astype(int))
p1 = tuple(p1.astype(int))
rect = (0, 0, out.shape[1], out.shape[0])
inside, p0, p1 = cv2.clipLine(rect, p0, p1)
if inside:
cv2.line(out, p0, p1, color, thickness, cv2.LINE_AA)
def isCrossingPedestrian(img,numberPlateCoordinates,resizingParameterNP,resizingParameter,zz): # print 1 trueFalse = [] # print len(numberPlateCoordinates) for i in range(len(numberPlateCoordinates)): # rectNP = cv2.rectangle(img, (int(numberPlateCoordinates[i][0]/resizingParameter), int(numberPlateCoordinates[i][1]/resizingParameter)-subY), (int((numberPlateCoordinates[i][0] + numberPlateCoordinates[i][3])/resizingParameter), int((numberPlateCoordinates[i][1] + numberPlateCoordinates[i][2] )/resizingParameter)+addH),( 0, 255, 0 ),2 ) # rectNP = cv2.rectangle(img, (0,0), (imgShape[1], imgShape[0]), (255,0,255), 2) # print cv2.clipLine(rectNP, (0,int(zz[1])), (img.shape[1], int(zz[0]*img.shape[1] + zz[1]))) # return cv2.clipLine(rectNP, (4,5), (8000,9000))[0] trueFalse.append(cv2.clipLine((int(numberPlateCoordinates[i][0]/resizingParameter) - subY,int(numberPlateCoordinates[i][1]/resizingParameter), int((numberPlateCoordinates[i][3])/resizingParameter), int((numberPlateCoordinates[i][2])/resizingParameter) + addH), (0,int(zz[1])), (img.shape[1], int(zz[0]*img.shape[1] + zz[1])))[0]) return trueFalse
def tagged_data_to_ellipse_envelope(job_spec):
tag_id, remote_data_filename, remote_cal_filename, start, degrees, radius_of_roi = job_spec
with open(lcu.remote_to_local_filename(remote_data_filename), 'rb') as data_file:
data_jpeg = data_file.read()
with open(lcu.remote_to_local_filename(remote_cal_filename), 'rb') as cal_file:
cal_jpeg = cal_file.read()
data = cv2.imdecode(np.fromstring(data_jpeg, np.uint8), cv2.CV_LOAD_IMAGE_GRAYSCALE)
cal = cv2.imdecode(np.fromstring(cal_jpeg, np.uint8), cv2.CV_LOAD_IMAGE_GRAYSCALE)
delta = lcu.better_delta(data, cal)
start = np.array(start)
adjust = np.array([int(radius_of_roi), int(radius_of_roi/2)], dtype=np.int32)
retval, roi_corner, roi_far_corner = cv2.clipLine(
(0, 0, 512, 384),
tuple(start - adjust),
tuple(start + adjust),
)
rotate_matrix = cv2.getRotationMatrix2D(tuple(start), degrees, 1)
roi = cv2.warpAffine(delta, rotate_matrix, (512, 384))[
roi_corner[1]:roi_far_corner[1],
roi_corner[0]:roi_far_corner[0]
]
roi = cv2.cvtColor(roi, cv2.COLOR_GRAY2RGB)
roi_corner = np.array(roi_corner)
start = start - roi_corner
color = int(np.average(
roi[start[1] - 2:start[1] + 2, start[0] - 2:start[0] + 2].astype(np.float32)
))
scores = list()
for x in range(20, 60):
y_min = int(x/2.3)
y_max = int(x/1.5)
for y in range(y_min, y_max):
template = np.zeros((y, x, 3), dtype=np.uint8)
cv2.ellipse(img=template, box=((x // 2, y // 2), (x, y), 0),
color=(color, color, color), thickness=-1)
match = cv2.minMaxLoc(cv2.matchTemplate(roi, template, cv2.TM_SQDIFF_NORMED))
scores.append((match[0], (x,y), match[2]))
good_scores = sorted(scores)[:10]
best_score, ellipse_size, ellipse_corner = sorted(good_scores, key=lambda x: -x[1][0]*x[1][1])[0]
return (tag_id, ellipse_size, color)
def bresenham_march(img, p1, p2):
x1 = p1[0]
y1 = p1[1]
x2 = p2[0]
y2 = p2[1]
#tests if any coordinate is outside the image
if (
x1 >= img.shape[0]
or x2 >= img.shape[0]
or y1 >= img.shape[1]
or y2 >= img.shape[1]
): #tests if line is in image, necessary because some part of the line must be inside, it respects the case that the two points are outside
if not cv2.clipLine((0, 0, *img.shape), p1, p2):
print("not in region")
return
steep = math.fabs(y2 - y1) > math.fabs(x2 - x1)
if steep:
x1, y1 = y1, x1
x2, y2 = y2, x2
# takes left to right
also_steep = x1 > x2
if also_steep:
x1, x2 = x2, x1
y1, y2 = y2, y1
dx = x2 - x1
dy = math.fabs(y2 - y1)
error = 0.0
delta_error = 0.0
# Default if dx is zero
if dx != 0:
delta_error = math.fabs(dy / dx)
y_step = 1 if y1 < y2 else -1
y = y1
ret = []
for x in range(x1, x2):
p = (y, x) if steep else (x, y)
if p[0] < img.shape[0] and p[1] < img.shape[1]:
ret.append((p, img[p]))
error += delta_error
if error >= 0.5:
y += y_step
error -= 1
if also_steep: # because we took the left to right instead
ret.reverse()
return ret
def clipLine(self, point1, point2, shape):
if point1 is None or point2 is None:
return False, point1, point2
if (-65535 >= point1).any() or (point1 >= 65535).any():
return False, point1, point2
if (-65535 >= point2).any() or (point2 >= 65535).any():
return False, point1, point2
point1 = point1.astype(int)
point2 = point2.astype(int)
ret, pt1, pt2 = cv2.clipLine((0, 0, shape[1], shape[0]), tuple(point1),
tuple(point2))
return ret, pt1, pt2
def in_line(x, y, lr):
# x = x
# y = y
if lr == 'left':
x1, x2 = x, WIDTH
y1, y2 = y, y + 1
pt1 = R_PNT, 0
pt2 = WIDTH, HEIGHT
imgRect = (x, y, x2 - x1, y2 - y1)
retval, rpt1, rpt2 = cv2.clipLine(imgRect, pt1, pt2)
elif lr == 'right':
x1, x2 = x, WIDTH
y1, y2 = y, y + 1
pt1 = L_PNT, 0
pt2 = 0, HEIGHT
imgRect = (x, y, x2 - x1, y2 - y1)
retval, rpt1, rpt2 = cv2.clipLine(imgRect, pt1, pt2)
return retval
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(ret, pix1, pix2) = cv2.clipLine((0, 0, img.shape[1], img.shape[0]), pix1, pix2)
if ret is False:
if len(self._pix_points) == 0:
self._pix_points.append(None)
self._pix_points.append(None)
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth*2, colour)
if len(self._pix_points) == 0:
self._pix_points.append(pix1)
self._pix_points.append(pix2)
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(width, height) = image_shape(img)
(ret, pix1, pix2) = cv2.clipLine((0, 0, width, height), pix1, pix2)
if ret is False:
if len(self._pix_points) == 0:
self._pix_points.append(None)
self._pix_points.append(None)
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth * 2, colour)
if len(self._pix_points) == 0:
self._pix_points.append(pix1)
self._pix_points.append(pix2)
def create_search_strip_set(vertical_interval, vertical_step, width_interval,
center_interval, clip_region):
'''
Returns a list of lane search strips (as LaneSearchStrip objects). There are five
arguments. The first, vertical_interval, is a 2-tuple containing the vertical pixel
range of the search strip set. Note the order as the other arguments are linearly
interpolated from this range. The second argument, vertical_step, is the vertical
pixel separation between each search strip. The width_interval and center_interval,
arguments three and four respectively, define what the width of the strip will be at
the start of the vertical interval and what the center of the strip will be (as a
2-tuple) at the start of the vertical interval. The fifth argument, clip_region,
is a 4-tuple defining the bounds of the image as (left, top, right, bottom) which is
used to clip the search strips into the image.
'''
# define some convenient local variables
vertical_range = vertical_interval[1] - vertical_interval[0]
width_range = width_interval[1] - width_interval[0]
center_range = center_interval[1] - center_interval[0]
# initialize the set of search strips
search_strips = []
# walk through each vertical step
for y in range(vertical_interval[0], vertical_interval[1] + vertical_step,
vertical_step):
# determine the width and center of the strip using linear interpolation
width = int(width_range * (y - vertical_interval[0]) /
vertical_range) + width_interval[0]
center = int(center_range * (y - vertical_interval[0]) /
vertical_range) + center_interval[0]
# define the strip as a segment
segment = ((center - width, y), (center + width, y))
# clip the segment to the image
segment_is_in_image, left_point, right_point = cv2.clipLine(
clip_region, segment[0], segment[1])
# if the segment isn't in the image, don't add it
if not segment_is_in_image:
continue
# add the clipped segment to the image
search_strips.append(
LaneSearchStrip(left_point, right_point[0] - left_point[0]))
return search_strips
def getRansacLines(thresholded_image, lines):
e1 = cv.getTickCount()
test_image = np.transpose(thresholded_image)
non_zero_points = np.nonzero(test_image)
list_x_y_points = []
for i in range(0, len(non_zero_points[0])):
point = (non_zero_points[0][i], non_zero_points[1][i])
list_x_y_points.append(point)
sorted_by_second = sorted(list_x_y_points, key=lambda tup: tup[1])
# print(sorted_by_second)
intializepointsinROI(sorted_by_second, lines)
for i in range(0, len(lines)):
# print(lines[i].list_x_y_points)
data = np.array(lines[i].list_x_y_points)
line = cv.fitLine(data, cv.DIST_FAIR, 0, 0.01, 0.01)
mult = max(gray_image.shape[0], gray_image.shape[1])
startpoint = (int(line[2] - mult * line[0]),
int(line[3] - mult * line[1]))
endpoint = (int(line[2] + mult * line[0]),
int(line[3] + mult * line[1]))
points = cv.clipLine((0, 0, gray_image.shape[1], gray_image.shape[0]),
startpoint, endpoint)
x_limit_max = max(lines[i].startpoint[0], lines[i].endpoint[0])
x_limit_min = min(lines[i].startpoint[0], lines[i].endpoint[0])
points = [list(i) for i in points[1:]]
# print(points[][0])
for i in range(0, len(points)):
if (points[i][0] < x_limit_min):
points[i][0] = x_limit_min
elif (points[i][0] > x_limit_max):
points[i][0] = x_limit_max
# print(points)
cv.line(gray_image, tuple(points[0]), tuple(points[1]), (0, 0, 255), 2)
# # print(line)
#cv.imshow("Result", gray_image)
#cv.waitKey(0)
#Write Image
e2 = cv.getTickCount()
time = (e2 - e1) / cv.getTickFrequency()
print("Time for Fitting line")
print(time)
def create_search_strip_set(vertical_interval, vertical_step, width_interval, center_interval, clip_region): ''' Returns a list of lane search strips (as LaneSearchStrip objects). There are five arguments. The first, vertical_interval, is a 2-tuple containing the vertical pixel range of the search strip set. Note the order as the other arguments are linearly interpolated from this range. The second argument, vertical_step, is the vertical pixel separation between each search strip. The width_interval and center_interval, arguments three and four respectively, define what the width of the strip will be at the start of the vertical interval and what the center of the strip will be (as a 2-tuple) at the start of the vertical interval. The fifth argument, clip_region, is a 4-tuple defining the bounds of the image as (left, top, right, bottom) which is used to clip the search strips into the image. ''' # define some convenient local variables vertical_range = vertical_interval[1]-vertical_interval[0] width_range = width_interval[1]-width_interval[0] center_range = center_interval[1]-center_interval[0] # initialize the set of search strips search_strips = [] # walk through each vertical step for y in range(vertical_interval[0], vertical_interval[1]+vertical_step, vertical_step): # determine the width and center of the strip using linear interpolation width = int(width_range*(y-vertical_interval[0])/vertical_range)+width_interval[0] center = int(center_range*(y-vertical_interval[0])/vertical_range)+center_interval[0] # define the strip as a segment segment = ((center-width, y), (center+width, y)) # clip the segment to the image segment_is_in_image, left_point, right_point = cv2.clipLine(clip_region, segment[0], segment[1]) # if the segment isn't in the image, don't add it if not segment_is_in_image: continue # add the clipped segment to the image search_strips.append(LaneSearchStrip(left_point, right_point[0]-left_point[0])) return search_strips
def draw_line(self, img, pixmapper, pt1, pt2, colour, linewidth):
'''draw a line on the image'''
pix1 = pixmapper(pt1)
pix2 = pixmapper(pt2)
(width, height) = image_shape(img)
(ret, pix1, pix2) = cv2.clipLine((0, 0, width, height), pix1, pix2)
if ret is False:
if len(self._pix_points) == 0:
self._pix_points.append(None)
self._pix_points.append(None)
return
cv2.line(img, pix1, pix2, colour, linewidth)
cv2.circle(img, pix2, linewidth*2, colour)
if len(self._pix_points) == 0:
self._pix_points.append(pix1)
self._pix_points.append(pix2)
if self.arrow:
xdiff = pix2[0]-pix1[0]
ydiff = pix2[1]-pix1[1]
if (xdiff*xdiff + ydiff*ydiff) > 400: # the segment is longer than 20 pix
SlipArrow(self.key, self.layer, (int(pix1[0]+xdiff/2.0), int(pix1[1]+ydiff/2.0)), self.colour,
self.linewidth, math.atan2(ydiff, xdiff)+math.pi/2.0).draw(img)
def image_matching(img_idx,
candidate_idx,
name_idx,
indexes,
features,
N=10000):
'''
In:
img_idx: index of current image
candidate_idx: numpy array of size m
name_idx: K; [(name, start index, end index)]
indexes: sum_N * 4 np matrix of index
features: sum_N * 4; [np matrix]
N: number of RANSAC iterations
Out:
fundamental_matrix: m * 3 * 3 np matrix, each row is a flat fundamental matrix
'''
img_name, img_start, img_end = name_idx[img_idx]
fundamental_matrix = []
inlier_number = []
inlier_set = []
epipolar_set = []
for c in candidate_idx:
# for each pair of matched images,
# calculate fundamental matrix
cand_name, cand_start, cand_end = name_idx[c]
XY = []
for i in range(img_start, img_end):
# for each key point in current image,
# find its inlier matched points in this candidate image
x1, y1, _, _ = features[i]
for j in indexes[i]:
if j < cand_end and j >= cand_start:
x2, y2, _, _ = features[j]
XY.append([x1, y1, x2, y2])
print('XY: {}'.format(len(XY)))
# Use RANSAC
# TODO estimate number of iteration needed
# inlier, _ = ransac_single_step(XY)
# print([inlier, len(XY)])
# num_iteration = int(np.log(0.05)/np.log(1-(inlier/len(XY))**len(XY)))
# print(num_iteration)
# use RANSAC to approximate F
F, e, inlier, inliers = ransac(XY, N)
fundamental_matrix.append(F)
epipolar_set.append(e)
inlier_number.append(inlier)
inlier_set.append(inliers)
print('inliers: {}'.format(inlier))
k = np.argmax(inlier_number)
# plot epipolar line
print(epipolar_set[k][0] / epipolar_set[k][0][-1])
print(epipolar_set[k][1] / epipolar_set[k][1][-1])
e1x, e1y, _ = epipolar_set[k][0] / epipolar_set[k][0][-1]
e2x, e2y, _ = epipolar_set[k][1] / epipolar_set[k][1][-1]
img1 = cv2.imread(os.path.join(DATA_DIR, img_name + '.jpg'))
img2 = cv2.imread(os.path.join(DATA_DIR, name_idx[k][0] + '.jpg'))
for pair in inlier_set[k]:
x1, y1, x2, y2 = pair
cv2.circle(img1, (int(x1), int(y1)), 1, (255, 255, 255), 5)
cv2.circle(img2, (int(x2), int(y2)), 1, (255, 255, 255), 5)
rect1 = (0, 0, img1.shape[1], img1.shape[0])
_, pt1, pt2 = cv2.clipLine(rect1, (int(x1), int(y1)),
(int(e1x), int(e1y)))
cv2.line(img1, pt1, pt2, (255, 255, 255), 2)
rect2 = (0, 0, img2.shape[1], img2.shape[0])
_, pt1, pt2 = cv2.clipLine(rect2, (int(x2), int(y2)),
(int(e2x), int(e2y)))
cv2.line(img2, pt1, pt2, (255, 255, 255), 2)
cv2.imwrite(img_name + '_' + img_name + '.jpg', img1)
cv2.imwrite(img_name + '_' + name_idx[k][0] + '.jpg', img2)
# sort fundamental matrix by number of inliers
# n = len(inlier_number)
# lst = list(range(n))
# lst.sort(key=inlier_number.__getitem__)
# fundamental_matrix = list(map(fundamental_matrix.__getitem__, lst))
# inlier_number = list(map(inlier_number.__getitem__, lst))
# inlier_set = list(map(inlier_set.__getitem__, lst))
# return fundamental_matrix, inlier_number, inlier_set
return candidate_idx[k], fundamental_matrix[k], inlier_number[
k], inlier_set[k]
def getProjection(image : np.ndarray, mask : np.ndarray) -> np.ndarray:
# Get contours
contours, _ = cv2.findContours(image = mask, mode = cv2.RETR_TREE, method = cv2.CHAIN_APPROX_SIMPLE) # check if RETR_LIST is better or other methods
# check if grayscale is better than mask
if not contours:
# Log for no contours found
loggingIt("No contours were found.")
return False
# Calculate biggest contour
maxContourArea : np.uint16 = 0
maxContourIndex : np.uint8 = 0
for idx, cont in enumerate(contours):
_, _, width, height = cv2.boundingRect(array = cont)
area = width * height
if area > maxContourArea:
maxContourArea = area
maxContourIndex = idx
# Check if contour is big enough to be the board
height, width = mask.shape
percentage = maxContourArea / (width * height) * 100
if percentage < 20:
# Log for board not found
loggingIt("Contour is too small to be the board.")
return False
# Get a blank image with only the contours
contourImage = np.zeros_like(a = mask, dtype = np.uint8)
cv2.drawContours(image = contourImage, contours = contours, contourIdx = maxContourIndex, color = (255))
# Get lines through HoughTransformation
lines = cv2.HoughLines(image = contourImage, rho = 1, theta = np.pi / 180, threshold = 60)
if lines.size == 0:
# Log for no HoughLines found
loggingIt("No HoughLines were found.")
return False
# Calculate the coordinates of the found lines and append to list
detectedLines : list = []
for line in lines:
(rho, theta) = line[0]
a = np.cos(theta)
b = np.sin(theta)
x0 = a*rho
y0 = b*rho
x1 = int(x0 + 1000*(-b))
y1 = int(y0 + 1000*(a))
x2 = int(x0 - 1000*(-b))
y2 = int(y0 - 1000*(a))
_, (x1, y1), (x2, y2) = cv2.clipLine(imgRect = (0, 0, width, height), pt1 = (x1, y1), pt2 = (x2, y2))
detectedLines.append(np.array([x1, y1, x2, y2]))
# Call to calculateBorderFromLines, just below
corners = getCorners(lines = detectedLines)
if not corners:
# Log not enough corners found
loggingIt("No corners were found.")
return False
cornersPicture = np.array([
(0, 0),
(width, 0),
(0, height),
(width, height)
])
cornersPicture = np.float32(cornersPicture[:,np.newaxis,:]) # New dimension needed for opencv2 functions
transform = cv2.getPerspectiveTransform(src = np.float32(corners), dst = cornersPicture)
dst = cv2.warpPerspective(src = image, M = transform, dsize = (width, height))
return dst
import matplotlib.pyplot as plt
import cv2
import numpy as np
import Func
image = Func.create_canvas_matplotlib(400,400)
image = cv2.line(image, (0,0), (400,400), (255,0,0), 2)
image = cv2.rectangle(image, (0,0), (200,200), (0,255,0), 2)
ret, p1, p2 = cv2.clipLine((0, 0, 200, 200), (0, 0), (400, 400))
if ret:
cv2.line(image, p1, p2, (0,255,255), 3)
Func.show_with_matplotlib(image, "test")
def clipToFrame(self):
if self.frame is not None:
rawStart, rawEnd = cv2.clipLine(self.frame, self.start, self.end)
self.start = Point(rawStart)
self.end = Point(rawEnd)
def compute_map(state,
height=960,
width=1280,
map_size=256,
map_scale=3,
fov=90.0,
beacon_scale=100,
pick_new_goal=False,
only_visible_beacons=True,
curr_goal=None):
depth_buffer = state.depth_buffer
player_x = state.game_variables[5]
player_y = state.game_variables[6]
player_z = state.game_variables[7]
player_angle = state.game_variables[8]
player_pitch = state.game_variables[9]
player_roll = state.game_variables[10]
canvas_size = 2 * map_size + 1
vis_map = np.zeros((canvas_size, canvas_size, 3), dtype=np.uint8)
simple_map = np.zeros((canvas_size, canvas_size), dtype=np.uint8)
r = canvas_size
offset = 225
y1 = int(r * math.cos(math.radians(offset + player_angle)))
x1 = int(r * math.sin(math.radians(offset + player_angle)))
y2 = int(r * math.cos(math.radians(offset + player_angle - fov)))
x2 = int(r * math.sin(math.radians(offset + player_angle - fov)))
_, p1, p2 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x1, map_size + y1))
_, p3, p4 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x2, map_size + y2))
game_unit = 110.0 / 14
ray_cast = (depth_buffer[height / 2] * game_unit) / float(map_scale)
ray_points = [(map_size, map_size)]
for i in range(canvas_size):
d = ray_cast[int(float(width) / canvas_size * i - 1)]
theta = (float(i) / canvas_size * fov)
ray_y = int(d * math.sin(math.radians(offset - theta))) + map_size
ray_x = int(d * math.cos(math.radians(offset - theta))) + map_size
_, _, p = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size), (ray_y, ray_x))
ray_points.append(p)
cv2.fillPoly(vis_map, np.array([ray_points], dtype=np.int32),
(255, 255, 255))
cv2.fillPoly(simple_map, np.array([ray_points], dtype=np.int32), (1, 1, 1))
for l in state.labels:
object_relative_x = -l.object_position_x + player_x
object_relative_y = -l.object_position_y + player_y
object_relative_z = -l.object_position_z + player_z
rotated_x = math.cos(
math.radians(-player_angle)) * object_relative_x - math.sin(
math.radians(-player_angle)) * object_relative_y
rotated_y = math.sin(
math.radians(-player_angle)) * object_relative_x + math.cos(
math.radians(-player_angle)) * object_relative_y
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
color = (0, 0, 255)
object_id = 2
simple_map[rotated_x, rotated_y] = object_id
cv2.circle(vis_map, (rotated_y, rotated_x), 2, color, thickness=-1)
quantized_x = int(player_x / beacon_scale) * beacon_scale
quantized_y = int(player_y / beacon_scale) * beacon_scale
beacon_radius = 10
beacons = []
for bnx in range(-beacon_radius, beacon_radius + 1):
for bny in range(-beacon_radius, beacon_radius + 1):
beacon_x = quantized_x + bnx * beacon_scale
beacon_y = quantized_y + bny * beacon_scale
beacons.append((beacon_x, beacon_y))
visble_beacons_world = []
for b in beacons:
object_relative_x = -b[0] + player_x
object_relative_y = -b[1] + player_y
rotated_x = math.cos(
math.radians(-player_angle)) * object_relative_x - math.sin(
math.radians(-player_angle)) * object_relative_y
rotated_y = math.sin(
math.radians(-player_angle)) * object_relative_x + math.cos(
math.radians(-player_angle)) * object_relative_y
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
color = (255, 0, 0)
object_id = 3
show = True
if simple_map[rotated_x, rotated_y] == 0:
show = (only_visible_beacons != True)
else:
visble_beacons_world.append((b[0], b[1]))
if show:
simple_map[rotated_x, rotated_y] = object_id
cv2.circle(vis_map, (rotated_y, rotated_x),
2,
color,
thickness=-1)
if pick_new_goal:
if len(visble_beacons_world) > 0:
beacon_idx = random.randint(0, len(visble_beacons_world) - 1)
"""
max_dist = 0
for b in range(len(visble_beacons_world)):
xdiff = visble_beacons_world[b][0] - player_x
ydiff = visble_beacons_world[b][1] - player_y
d = abs(xdiff) + abs(ydiff)
if d > max_dist:
beacon_idx = b
max_dist = d
"""
curr_goal = visble_beacons_world[beacon_idx]
if curr_goal is not None:
object_relative_x = -curr_goal[0] + player_x
object_relative_y = -curr_goal[1] + player_y
rotated_x = math.cos(
math.radians(-player_angle)) * object_relative_x - math.sin(
math.radians(-player_angle)) * object_relative_y
rotated_y = math.sin(
math.radians(-player_angle)) * object_relative_x + math.cos(
math.radians(-player_angle)) * object_relative_y
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
color = (255, 255, 0)
object_id = 4
if simple_map[rotated_x, rotated_y] > 0:
simple_map[rotated_x, rotated_y] = object_id
cv2.circle(vis_map, (rotated_y, rotated_x),
2,
color,
thickness=-1)
return vis_map, simple_map, curr_goal
cv2.rectangle(img, (x1, y1), (x2, y2), (0, 0, 255), -1)
px1 = 250, 40
py1 = 250, 450
cv2.line(img, px1, py1, (255, 0, 0), 2)
px2 = 40, 250
py2 = 450, 250
cv2.line(img, px2, py2, (255, 0, 0), 2)
imgRect1 = (x1, y1, x2 - x1, y2 - y1)
imgRect2 = (x1, y1, x2 - x1, y2 - y1)
retval1, rpt1, rpt2 = cv2.clipLine(imgRect1, px1, py1)
retval2, rpt3, rpt4 = cv2.clipLine(imgRect2, px2, py2)
if retval1:
cv2.circle(img, rpt1, radius=5, color=(0, 0, 0), thickness=-1)
cv2.circle(img, rpt2, radius=5, color=(0, 0, 0), thickness=-1)
if retval2:
cv2.circle(img, rpt3, radius=5, color=(0, 0, 0), thickness=-1)
cv2.circle(img, rpt4, radius=5, color=(0, 0, 0), thickness=-1)
cv2.imshow('img', img)
cv2.waitKey()
cv2.destroyAllWindows()
'dark_gray': (50, 50, 50), 'light_gray': (220, 220, 220)}
# We create the canvas to draw: 300 x 300 pixels, 3 channels, uint8 (8-bit unsigned integers)
# We set background to black using np.zeros():
image = np.zeros((300, 300, 3), dtype="uint8")
# If you want another background color you can do the following:
image[:] = colors['light_gray']
# 1. We are going to see how cv2.clipLine() works
# Draw a rectangle and a line:
cv2.line(image, (0, 0), (300, 300), colors['green'], 3)
cv2.rectangle(image, (0, 0), (100, 100), colors['blue'], 3)
# We call the function cv2.clipLine():
ret, p1, p2 = cv2.clipLine((0, 0, 100, 100), (0, 0), (300, 300))
# cv2.clipLine() returns False if the line is outside the rectangle
# And returns True otherwise
if ret:
cv2.line(image, p1, p2, colors['yellow'], 3)
# Show image:
show_with_matplotlib(image, 'cv2.clipLine()')
# Clean the canvas to draw again:
image[:] = colors['light_gray']
# 2. We are going to see how cv2.arrowedLine() works:
cv2.arrowedLine(image, (50, 50), (200, 50), colors['red'], 3, 8, 0, 0.1)
cv2.arrowedLine(image, (50, 120), (200, 120), colors['green'], 3, cv2.LINE_AA, 0, 0.3)
def update_grid_map(state, height=960, width=1280,
map_size=256, map_scale=3, fov=90.0,
grid_scale=50, nodes = {}, edges = {}):
depth_buffer = state.depth_buffer
player_x = state.game_variables[5]
player_y = state.game_variables[6]
player_z = state.game_variables[7]
player_angle = state.game_variables[8]
player_pitch = state.game_variables[9]
player_roll = state.game_variables[10]
canvas_size = 2*map_size + 1
simple_map = np.zeros((canvas_size, canvas_size), dtype=np.uint8)
r = canvas_size
offset = 225
y1 = int(r * math.cos(math.radians(offset + player_angle)))
x1 = int(r * math.sin(math.radians(offset + player_angle)))
y2 = int(r * math.cos(math.radians(offset + player_angle - fov)))
x2 = int(r * math.sin(math.radians(offset + player_angle - fov)))
_, p1, p2 = cv2.clipLine((0, 0, canvas_size, canvas_size), (map_size, map_size),
(map_size + x1, map_size + y1))
_, p3, p4 = cv2.clipLine((0, 0, canvas_size, canvas_size), (map_size, map_size),
(map_size + x2, map_size + y2))
game_unit = 100.0/14
ray_cast = (depth_buffer[height/2] * game_unit)/float(map_scale)
ray_points = [ (map_size, map_size) ]
for i in range(10, canvas_size-10):
d = ray_cast[int(float(width)/canvas_size * i - 1)]
theta = (float(i)/canvas_size * fov)
ray_y = int(d * math.sin(math.radians(offset - theta))) + map_size
ray_x = int(d * math.cos(math.radians(offset - theta))) + map_size
_, _, p = cv2.clipLine((0, 0, canvas_size, canvas_size), (map_size, map_size),
(ray_y, ray_x))
ray_points.append(p)
cv2.fillPoly(simple_map, np.array([ray_points], dtype=np.int32), (1, 1, 1))
quantized_x = int(player_x/grid_scale) * grid_scale
quantized_y = int(player_y/grid_scale) * grid_scale
beacon_radius = 10
beacons = []
for bnx in range(-beacon_radius, beacon_radius+1):
for bny in range(-beacon_radius, beacon_radius+1):
beacon_x = quantized_x + bnx * grid_scale
beacon_y = quantized_y + bny * grid_scale
beacons.append((beacon_x, beacon_y))
visble_beacons_world = {}
for b in beacons:
object_relative_x = -b[0] + player_x
object_relative_y = -b[1] + player_y
rotated_x = math.cos(math.radians(-player_angle)) * object_relative_x - math.sin(math.radians(-player_angle)) * object_relative_y
rotated_y = math.sin(math.radians(-player_angle)) * object_relative_x + math.cos(math.radians(-player_angle)) * object_relative_y
rotated_x = int(rotated_x/map_scale + map_size)
rotated_y = int(rotated_y/map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and
rotated_y >= 0 and rotated_y < canvas_size):
if simple_map[rotated_x, rotated_y] != 0:
visble_beacons_world[(b[0], b[1])] = True
for b in visble_beacons_world:
if b not in nodes:
nodes[b] = True
neighbors = [ (b[0], b[1] - grid_scale),
(b[0], b[1] + grid_scale),
(b[0] - grid_scale, b[1]),
(b[0] + grid_scale, b[1]),
(b[0] - grid_scale, b[1] - grid_scale),
(b[0] + grid_scale, b[1] + grid_scale),
(b[0] - grid_scale, b[1] + grid_scale),
(b[0] + grid_scale, b[1] - grid_scale),
]
for n in neighbors:
if n in visble_beacons_world:
if (b, n) not in edges:
edges[(b, n)] = True
edges[(n, b)] = True
"""
def compute_map(state,
height=960,
width=1280,
map_size=256,
map_scale=3,
fov=90.0,
beacon_scale=50,
pick_new_goal=False,
only_visible_beacons=True,
curr_goal=None,
explored_goals={}):
# Extract agent state from game
depth_buffer = state.depth_buffer
player_x = state.game_variables[5]
player_y = state.game_variables[6]
player_z = state.game_variables[7]
player_angle = state.game_variables[8]
player_pitch = state.game_variables[9]
player_roll = state.game_variables[10]
# Initialize maps
canvas_size = 2 * map_size + 1
vis_map = np.zeros((canvas_size, canvas_size, 3), dtype=np.uint8)
simple_map = np.zeros((canvas_size, canvas_size), dtype=np.uint8)
# Compute upper left and upper right extreme coordinates
r = canvas_size
offset = 225
y1 = int(r * math.cos(math.radians(offset + player_angle)))
x1 = int(r * math.sin(math.radians(offset + player_angle)))
y2 = int(r * math.cos(math.radians(offset + player_angle - fov)))
x2 = int(r * math.sin(math.radians(offset + player_angle - fov)))
# Draw FOV boundaries
_, p1, p2 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x1, map_size + y1))
_, p3, p4 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x2, map_size + y2))
# Ray cast from eye line to project depth map into 2D ray points
game_unit = 100.0 / 14
ray_cast = (depth_buffer[height / 2] * game_unit) / float(map_scale)
ray_points = [(map_size, map_size)]
for i in range(10, canvas_size - 10):
d = ray_cast[int(float(width) / canvas_size * i - 1)]
theta = (float(i) / canvas_size * fov)
ray_y = int(d * math.sin(math.radians(offset - theta))) + map_size
ray_x = int(d * math.cos(math.radians(offset - theta))) + map_size
_, _, p = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size), (ray_y, ray_x))
ray_points.append(p)
# Fill free space on 2D map with colour
cv2.fillPoly(vis_map, np.array([ray_points], dtype=np.int32),
(255, 255, 255)) # NOQA
cv2.fillPoly(simple_map, np.array([ray_points], dtype=np.int32), (1, 1, 1))
quantized_x = int(player_x / beacon_scale) * beacon_scale
quantized_y = int(player_y / beacon_scale) * beacon_scale
beacon_radius = 10
# Get beacons within range of current agent position
beacons = []
for bnx in range(-beacon_radius, beacon_radius + 1):
for bny in range(-beacon_radius, beacon_radius + 1):
beacon_x = quantized_x + bnx * beacon_scale
beacon_y = quantized_y + bny * beacon_scale
beacons.append((beacon_x, beacon_y))
# Compute set of visible beacons and draw onto the map
visble_beacons_world = []
for b in beacons:
object_relative_x = -b[0] + player_x
object_relative_y = -b[1] + player_y
rotated_x = math.cos(
math.radians(-player_angle)) * object_relative_x - math.sin(
math.radians(-player_angle)) * object_relative_y # NOQA
rotated_y = math.sin(
math.radians(-player_angle)) * object_relative_x + math.cos(
math.radians(-player_angle)) * object_relative_y # NOQA
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
color = (255, 0, 0)
object_id = 3
show = True
if simple_map[rotated_x, rotated_y] == 0:
show = (only_visible_beacons is not True)
else:
visble_beacons_world.append((b[0], b[1]))
if show:
simple_map[rotated_x, rotated_y] = object_id
cv2.circle(vis_map, (rotated_y, rotated_x),
2,
color,
thickness=-1)
# Pick new goal from unexplored visible beacons if required
if pick_new_goal:
unexplored_beacons = []
for b in visble_beacons_world:
if b not in explored_goals:
unexplored_beacons.append(b)
if len(unexplored_beacons) > 0:
beacon_idx = random.randint(0, len(unexplored_beacons) - 1)
"""
max_dist = 0
for b in range(len(unexplored_beacons)):
xdiff = unexplored_beacons[b][0] - player_x
ydiff = unexplored_beacons[b][1] - player_y
d = abs(xdiff) + abs(ydiff)
if d > max_dist:
beacon_idx = b
max_dist = d
"""
curr_goal = unexplored_beacons[beacon_idx]
explored_goals[curr_goal] = True
else:
curr_goal = None
"""
elif len(visble_beacons_world) > 0:
beacon_idx = random.randint(0, len(visble_beacons_world)-1)
curr_goal = visble_beacons_world[beacon_idx]
explored_goals[curr_goal] = True
"""
# Draw current goal location on map
if curr_goal is not None:
object_relative_x = -curr_goal[0] + player_x
object_relative_y = -curr_goal[1] + player_y
rotated_x = math.cos(
math.radians(-player_angle)) * object_relative_x - math.sin(
math.radians(-player_angle)) * object_relative_y # NOQA
rotated_y = math.sin(
math.radians(-player_angle)) * object_relative_x + math.cos(
math.radians(-player_angle)) * object_relative_y # NOQA
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
color = (255, 255, 0)
object_id = 4
if simple_map[rotated_x, rotated_y] > 0:
simple_map[rotated_x, rotated_y] = object_id
cv2.circle(vis_map, (rotated_y, rotated_x),
2,
color,
thickness=-1)
return vis_map, simple_map, curr_goal
import numpy as np
import cv2
img = np.zeros((512, 512, 3), np.uint8)
img = img[:] + 255
x1, y1 = 100, 100
x2, y2 = 400, 400
pt1 = (120, 50)
pt2 = (300, 500)
green = (0, 255, 0)
blue = (255, 0, 0)
red = (0, 0, 255)
cv2.rectangle(img, (x1, y1), (x2, y2), red, 2)
cv2.line(img, pt1, pt2, blue, 2)
imgRect = (x1, y1, x2 - x1, y2 - y1)
ret, p1, p2 = cv2.clipLine(imgRect, pt1, pt2)
if ret is True:
cv2.circle(img, p1, 10, green, 2)
cv2.circle(img, p2, 10, green, 2)
cv2.imshow('img', img)
cv2.waitKey()
cv2.destroyAllWindows()
def grow(self, dpOrig, ptDst, dir):
# auxiliar
ptEnd1 = [0,0]; ptEnd2 = [0,0]
dpEnd = [[0,0],0,0]
dpRef = [[0,0], 0, 0] # dpEnd = DIRPOINT([[0,0],0,0]); dpRef = DIRPOINT( [[0,0], 0, 0])
# Init output
ptDst = dpOrig[0]
# Starting gradient vector and director vector
gX = 0.0; gY = 0.0
if dir == 1:
gX = dpOrig[1]
gY = dpOrig[2]
elif dir == 2:
gX = -dpOrig[1]
gY = -dpOrig[2]
else:
return [Enum['RETERROR'],dpOrig, ptDst, dir]
# Compute currentAngle in 1°4°
error1 = 0.0
auxAngle = 0.0
minAngle = 0.0
maxAngle = 0.0
diffAngle = 0.0
growAngle = math.atan2(gY,gX)
# Starting point and angle - Bresenham
pt1 = dpOrig[0]
pt2 = [pt1[0] + int(1000*(-gY)),pt1[1] + int(1000*(gX))]
cv2.clipLine(self.imPadSize, pt1, pt2) # !be careful
# Loop - Bresenham
k1 = 0
x1 = pt1[0], x2 = pt2[0]
y1 = pt1[1], y2 = pt2[1]
dx = ABS(x2-x1)
dy = ABS(y2-y1)
sx = 0; sy = 0; err = 0; e2 = 0
if self.verbose:
print ("From (%d,%d) to (%d,%d)..." % (x1, y1, x2, y2))
# fflush(stdout)
if x1 < x2:
sx = 1
else:
sx = -1
if y1 < y2:
sy = 1
else:
sy = -1
err = dx - dy
maxNumZeroPixels = 2 * self.R; countZeroPixels = 0
while True:
'''
Current value is (x1,y1)
if(verbose) { printf("\n\tBresenham(%d,%d)", x1, y1); fflush(stdout); }
-------------------------------
Do...
Check if angle has been computed
'''
if self.A[y1,x1] != NOTAVALIDANGLE:
auxAngle = self.A[y1,x1]
else:
auxAngle = math.atan2(1.0*self.Gy[y1,x1], 1.0*self.Gx[y1,x1])
self.A[y1,x1] = auxAngle
# Check early-termination of Bresenham
if self.G[y1,x1] == 0:
countZeroPixels = countZeroPixels + 1
if countZeroPixels >= maxNumZeroPixels:
break
# Check angular limits
if growAngle - self.margin < -PI2:
# e.g. currentAngle = -80°, margin = 20°
minAngle = growAngle - self.margin + 1.0*CVPI # e.g. -80 -20 +180 = 80
maxAngle = growAngle + self.margin # e.g. -80 +20
if auxAngle < 0:
if auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - auxAngle)
else:
if auxAngle < minAngle:
break
diffAngle = ABS(growAngle - (auxAngle - 1.0*CVPI))
elif growAngle + self.margin > PI2:
# e.g. currentAngle = 80º, margin = 20º
minAngle = growAngle - self.margin # e.g. 80 - 20 = 60
maxAngle = growAngle + self.margin - 1.0*CVPI # e.g. 80 +20 -180 = -80
if auxAngle > 0:
if auxAngle < minAngle:
break
diffAngle = ABS(growAngle - auxAngle)
else:
if auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - (auxAngle + 1.0*CVPI))
else:
minAngle = growAngle - self.margin
maxAngle = growAngle + self.margin
if auxAngle < minAngle or auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - auxAngle)
error1 = error1 + diffAngle
ptEnd1 = [x1, y1]
k1 = k1 + 1
# Check end
if x1 == x2 and y1 == y2:
break
# Update position for next iteration
e2 = 2*err
if e2 > -dy:
err = err - dy
x1 = x1 + sx
if e2 < dx:
err = err + dx
y1 = y1 + sy
# "k1": how many pints have been visited
# "ptEnd": last valid point
if k1 == 0:
# this means that even the closest point has not been accepted
ptEnd1 = dpOrig[0]
error1 = 1.0*CVPI
else:
error1 /= k1
if self.verbose:
print (", Arrived to (%d,%d), error=%.2f" % (ptEnd1[0],ptEnd1[1],error1))
# fflush(stdout)
# Set ptDst
ptDst = copy.copy(ptEnd1)
# Apply Mean-Shift to refine the end point
if self.verbose:
print (", Dist = (%d,%d)\n" % (ABS(ptEnd1[0] - dpOrig[0][0]), ABS(ptEnd1[1] - dpOrig[0][1])))
if ABS(ptEnd1[0] - dpOrig[0][0]) > self.R or ABS(ptEnd1[1] - dpOrig[0][1]) > self.R:
# ONLY IF THERE HAS BEEN (SIGNIFICANT) MOTION FROM PREVIOUS MEAN-SHIFT MAXIMA
counter = 0
while True:
if self.verbose:
print ("\tMean-Shift(Ext): from (%d,%d,%.2f,%.2f) to..." % (ptEnd1[0], ptEnd1[1], gX, gY))
# fflush(stdout)
counter = counter + 1
# Mean-Shift on the initial extremum
dpEnd[0] = copy.copy(ptEnd1)
dpEnd[1] = copy.copy(gX)
dpEnd[2] = copy.copy(gY)
dpRef[0] = copy.copy(ptEnd1)
dpRef[1] = copy.copy(gX)
dpRef[2] = copy.copy(gY)
retMSExt, dpEnd, dpRef,self.M = self.weightedMeanShift(dpEnd, dpRef,self.M) # !< function of weightedMeanShift
if self.verbose:
print ("(%d,%d,%.2f,%.2f)\n" % (dpRef[0][0],dpRef[0][1], dpRef[1], dpRef[2]))
if retMSExt == Enum['RETERROR']:
ptDst = copy.copy(ptEnd1)
return [Enum['RETOK'],dpOrig, ptDst, dir]
# Check motion caused by Mean-Shift
if (dpRef[0][0] == dpEnd[0][0]) and (dpRef[0][1] == dpEnd[0][1]):
ptDst = copy.copy(dpRef[0])
return [Enum['RETOK'],dpOrig, ptDst, dir]
# Check displacement from dpOrig
gX = float(dpRef[0][1] - dpOrig[0][1])
gY = float(dpOrig[0][0] - dpRef[0][0])
if gX == 0 and gY == 0:
ptDst = copy.copy(dpRef[0])
return [Enum['RETOK'],dpOrig, ptDst, dir]
norm = 1.0*math.sqrt(gX*gX + gY*gY)
gX /= norm
gY /= norm
# New Bresenham procedure
if gX < 0:
# Move to 1°4°
gX = -gX
gY = -gY
growAngle = math.atan2(gY,gX)
k2 = 0; error2 = 0.0
pt2[0] = pt1[0] + int(1000*(-gY))
pt2[1] = pt1[1] + int(1000*gX)
x1 = copy.copy(pt1[0]); x2 = copy.copy(pt2[0])
y1 = copy.copy(pt1[1]); y2 = copy.copy(pt2[1])
dx = ABS(x2-x1)
dy = ABS(y2-y1)
if x1 < x2:
sx = 1
else:
sx = -1
if y1 < y2:
sy = 1
else:
sy = -1
err = dx - dy
if self.verbose:
print ("\tRefined GROW: From (%d,%d) to (%d,%d)..." % x1, y1, x2, y2)
# fflush(stdout)
while True:
if self.A[y1,x1] != NOTAVALIDANGLE:
auxAngle = self.A[y1,x1]
else:
auxAngle = math.atan2(1.0*self.Gy[y1,x1],1.0*self.Gx[y1,x1])
self.A[y1,x1] = auxAngle
# Check early-termination of Bresenham
if self.G[y1,x1] == 0:
countZeroPixels = countZeroPixels + 1
if countZeroPixels >= maxNumZeroPixels:
break # No gradient point
# Check angular limits
if growAngle - self.margin < -PI2:
minAngle = growAngle - self.margin + 1.0*CVPI
maxAngle = growAngle + self.margin
if auxAngle < 0:
if auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - auxAngle)
else:
if auxAngle < minAngle:
break
diffAngle = ABS(growAngle - (auxAngle - 1.0*CVPI))
elif growAngle + self.margin > PI2:
minAngle = growAngle - self.margin
maxAngle = growAngle + self.margin - 1.0*CVPI
if auxAngle > 0:
if auxAngle < minAngle:
break
diffAngle = ABS(growAngle - auxAngle)
else:
if auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - (auxAngle + 1.0*CVPI))
else:
minAngle = growAngle - self.margin
maxAngle = growAngle + self.margin
if auxAngle < minAngle or auxAngle > maxAngle:
break
diffAngle = ABS(growAngle - auxAngle)
error2 = error2 + diffAngle
ptEnd2 = [x1,y1]
k2 = k2 + 1
# Check end
if x1 == x2 and y1 == y2:
break
# Update position for next iteration
e2 = 2*err
if e2 > -dy:
err = err - dy
x1 = x1 + sx
if e2 < dx:
err = err + dx
y1 = y1 + sy
# Bresenham while
# "k2": how many points have been visited
# "ptEnd2": last valid point
if k2 == 0:
# this means that even the closest point has not been accepted
ptEnd2 = copy.copy(dpOrig[0])
error2 = 1.0*CVPI
else:
error2 = error2 / k2
# fflush(stdout) # Don't really know why, but this is necessary to avoid dead loops...
if self.verbose:
print (", Arrived to (%d,%d), error=%.2f" % (ptEnd2[0], ptEnd2[1], error2))
# fflush(stdout)
if self.verbose:
print (", Dist = (%d,%d)\n" % (ABS(ptEnd2[0] - dpOrig[0][0]), ABS(ptEnd1[1] - dpOrig[0][1])))
# Compare obtained samples
if error1 <= error2:
ptDst = copy.copy(ptEnd1)
return error1
else:
ptEnd1 = copy.copy(ptEnd2)
k1 = copy.copy(k2)
error1 = copy.copy(error2)
# Mean-Shift while
return [error1,dpOrig, ptDst, dir]
def update_event(self, input_called=-1):
result = cv2.clipLine(self.input(0), self.input(1), self.input(2))
self.set_output_val(0, result[0])
def visualize_free_space(state,
height=960,
width=1280,
map_size=256,
map_scale=5,
fov=90.0):
depth_buffer = state.depth_buffer
labels_buffer = state.labels_buffer
screen_buffer = state.screen_buffer
player_x = state.game_variables[5]
player_y = state.game_variables[6]
player_z = state.game_variables[7]
player_angle = state.game_variables[8]
player_pitch = state.game_variables[9]
player_roll = state.game_variables[10]
print('player:', player_x, player_y, player_z)
print('player:', player_angle, player_pitch, player_roll)
canvas_size = 2 * map_size + 1
simple_map = np.zeros((canvas_size, canvas_size, 3), dtype=np.uint8)
r = canvas_size
offset = 225
y1 = int(r * math.cos(math.radians(offset + player_angle)))
x1 = int(r * math.sin(math.radians(offset + player_angle)))
y2 = int(r * math.cos(math.radians(offset + player_angle - fov)))
x2 = int(r * math.sin(math.radians(offset + player_angle - fov)))
_, p1, p2 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x1, map_size + y1))
_, p3, p4 = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size),
(map_size + x2, map_size + y2))
#cv2.line(simple_map, p1, p2, (255, 255, 255), thickness=1)
#cv2.line(simple_map, p3, p4, (255, 255, 255), thickness=1)
game_unit = 110.0 / 14
ray_cast = (depth_buffer[height / 2] * game_unit) / float(map_scale)
ray_points = [(map_size, map_size)]
for i in range(canvas_size):
d = ray_cast[int(float(width) / canvas_size * i - 1)]
theta = (float(i) / canvas_size * fov)
#ray_y = int(d * math.sin(math.radians(offset + player_angle - theta))) + map_size
#ray_x = int(d * math.cos(math.radians(offset + player_angle - theta))) + map_size
ray_y = int(d * math.sin(math.radians(offset - theta))) + map_size
ray_x = int(d * math.cos(math.radians(offset - theta))) + map_size
_, _, p = cv2.clipLine((0, 0, canvas_size, canvas_size),
(map_size, map_size), (ray_y, ray_x))
ray_points.append(p)
#if (ray_x >= 0 and ray_x < canvas_size and
# ray_y >= 0 and ray_y < canvas_size):
# ray_points.append((ray_y, ray_x))
# simple_map[(ray_x, ray_y)] = 255
cv2.fillPoly(simple_map, np.array([ray_points], dtype=np.int32),
(255, 255, 255))
for l in state.labels:
object_relative_x = -l.object_position_x + player_x
object_relative_y = -l.object_position_y + player_y
object_relative_z = -l.object_position_z + player_z
print(l.object_name, (object_relative_x), (object_relative_y),
(object_relative_z))
scaled_x = object_relative_x
scaled_y = object_relative_y
rotated_x = math.cos(
math.radians(-player_angle)) * scaled_x - math.sin(
math.radians(-player_angle)) * scaled_y
rotated_y = math.sin(
math.radians(-player_angle)) * scaled_x + math.cos(
math.radians(-player_angle)) * scaled_y
rotated_x = int(rotated_x / map_scale + map_size)
rotated_y = int(rotated_y / map_scale + map_size)
if (rotated_x >= 0 and rotated_x < canvas_size and rotated_y >= 0
and rotated_y < canvas_size):
simple_map[(rotated_x, rotated_y)] = (0, 0, 255)
cv2.imshow('ViZDoom Simplemap Buffer', simple_map)
# 0302.py
import cv2
import numpy as np
img = np.zeros(shape=(512, 512, 3), dtype=np.uint8) + 255
x1, x2 = 100, 400
y1, y2 = 100, 400
cv2.rectangle(img, (x1, y1), (x2, y2), (0, 0, 255))
pt1 = 120, 50
pt2 = 300, 500
cv2.line(img, pt1, pt2, (255, 0, 0), 2)
imgRect = (x1, y1, x2 - x1, y2 - y1)
retval, rpt1, rpt2 = cv2.clipLine(imgRect, pt1, pt2)
if retval:
cv2.circle(img, rpt1, radius=5, color=(0, 255, 0), thickness=-1)
cv2.circle(img, rpt2, radius=5, color=(0, 255, 0), thickness=-1)
cv2.imshow('img', img)
cv2.waitKey()
cv2.destroyAllWindows()
def draw_boxes_on_image_py(self,
rgb,
corners_pix,
centers_pix,
scores,
tids,
boxes=None,
thickness=1):
# all inputs are numpy tensors
# rgb is H x W x 3
# corners_pix is N x 8 x 2, in xy order
# centers_pix is N x 1 x 2, in xy order
# scores is N
# tids is N
# boxes is N x 9 < this is only here to print some rotation info
rgb = np.transpose(rgb, [1, 2, 0]) # put channels last
rgb = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR)
H, W, C = rgb.shape
assert (C == 3)
N, D, E = corners_pix.shape
assert (D == 8)
assert (E == 2)
if boxes is not None:
rx = boxes[:, 6]
ry = boxes[:, 7]
rz = boxes[:, 8]
else:
rx = 0
ry = 0
rz = 0
color_map = matplotlib.cm.get_cmap('tab20')
color_map = color_map.colors
# draw
for ind, corners in enumerate(corners_pix):
# corners is 8 x 2
if True:
# print 'score = %.2f' % scores[ind]
color_id = 1 % 20
color = color_map[color_id]
color = np.array(color) * 255.0
# color = (0,191,255)
# color = (255,191,0)
# print 'tid = %d; score = %.3f' % (tids[ind], scores[ind])
cv2.circle(rgb,
(centers_pix[ind, 0, 0], centers_pix[ind, 0, 1]), 2,
color, -1)
cv2.putText(
rgb,
'%d (%.2f)' % (tids[ind], scores[ind]),
(np.min(corners[:, 0]), np.min(corners[:, 1])),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, # font size
color,
1) # font weight
for c in corners:
# rgb[pt1[0], pt1[1], :] = 255
# rgb[pt2[0], pt2[1], :] = 255
# rgb[np.clip(int(c[0]), 0, W), int(c[1]), :] = 255
c0 = np.clip(int(c[0]), 0, W - 1)
c1 = np.clip(int(c[1]), 0, H - 1)
rgb[c1, c0, :] = 255
# we want to distinguish between in-plane edges and out-of-plane ones
# so let's recall how the corners are ordered:
# (new clockwise ordering)
xs = np.array([
1 / 2., 1 / 2., -1 / 2., -1 / 2., 1 / 2., 1 / 2., -1 / 2.,
-1 / 2.
])
ys = np.array([
1 / 2., 1 / 2., 1 / 2., 1 / 2., -1 / 2., -1 / 2., -1 / 2.,
-1 / 2.
])
zs = np.array([
1 / 2., -1 / 2., -1 / 2., 1 / 2., 1 / 2., -1 / 2., -1 / 2.,
1 / 2.
])
for ii in list(range(0, 2)):
cv2.circle(
rgb,
(corners_pix[ind, ii, 0], corners_pix[ind, ii, 1]), 3,
color, -1)
for ii in list(range(2, 4)):
cv2.circle(
rgb,
(corners_pix[ind, ii, 0], corners_pix[ind, ii, 1]), 2,
color, -1)
xs = np.reshape(xs, [8, 1])
ys = np.reshape(ys, [8, 1])
zs = np.reshape(zs, [8, 1])
offsets = np.concatenate([xs, ys, zs], axis=1)
corner_inds = list(range(8))
combos = list(combinations(corner_inds, 2))
for combo in combos:
pt1 = offsets[combo[0]]
pt2 = offsets[combo[1]]
# draw this if it is an in-plane edge
eqs = pt1 == pt2
if np.sum(eqs) == 2:
i, j = combo
pt1 = (corners[i, 0], corners[i, 1])
pt2 = (corners[j, 0], corners[j, 1])
retval, pt1, pt2 = cv2.clipLine((0, 0, W, H), pt1, pt2)
if retval:
cv2.line(rgb, pt1, pt2, color, thickness,
cv2.LINE_AA)
# rgb[pt1[0], pt1[1], :] = 255
# rgb[pt2[0], pt2[1], :] = 255
rgb = cv2.cvtColor(rgb.astype(np.uint8), cv2.COLOR_BGR2RGB)
# utils_basic.print_stats_py('rgb_uint8', rgb)
# imageio.imwrite('boxes_rgb.png', rgb)
return rgb
import matplotlib.pyplot as plt
image = np.zeros((500, 400, 3), dtype="uint8")
image[:] = constant.LIGHT_GRAY
cv.imshow("image", image)
#show_in_matplot
image_matplotlib = image[:, :, ::-1]
# cv.line(image, (50, 60), (300,60),constant.GREEN , 8)
# cv.rectangle(image, (10, 50), (60, 300), constant.GREEN,4)
# cv.rectangle(image, (80, 50), (130, 300), constant.GREEN, -1)
# cv.circle(image, (50, 50), 20, constant.BLUE, 3)
# cv.circle(image, (100, 100), 30, constant.RED, -1)
cv.line(image, (0, 0), (300, 300), constant.BLUE, 3)
cv.rectangle(image, (0, 0), (100, 100), constant.RED, 3)
ret, p1, p2 = cv.clipLine((0, 0, 100, 100), (0, 0), (300, 400))
if ret:
cv.line(image, p1, p2, constant.YELLOW, 3)
plt.subplot(111)
cv.arrowedLine(image, (200, 50), (30, 50), constant.RED, 3, 8, 0, 0.1)
cv.arrowedLine(image, (50, 120), (200, 120), constant.YELLOW, 3, cv.LINE_AA, 0,
0.3)
# These points define a triangle
pts = np.array([[250, 5], [220, 80], [280, 80]], np.int32)
# Reshape to shape (number_vertex, 1, 2)
pts = pts.reshape((-1, 1, 2))
# Print the shapes: this line is not necessary, only for visualization
print("shape of pts {}".format(pts.shape))
# Draw this poligon with True option
cv.polylines(image, [pts], True, constant.CYAN, 3)