def draw_judgement(im, judgements, delta=1.0):
points = judgements['intrinsic_points']
comparisons = judgements['intrinsic_comparisons']
id_to_points = {p['id']: p for p in points}
rows, cols = im.shape[0:2]
for c in comparisons:
darker = c['darker']
if darker not in ('1', '2', 'E'):
continue
weight = c['darker_score']
if weight <= 0 or weight is None:
continue
point1 = id_to_points[c['point1']]
point2 = id_to_points[c['point2']]
if not point1['opaque'] or not point2['opaque']:
continue
x1 = int(point1['x'] * cols)
y1 = int(point1['y'] * rows)
x2 = int(point2['x'] * cols)
y2 = int(point2['y'] * rows)
if darker == '1':
cv2.arrowedLine(im, (x2, y2), (x1, y1), (0, 0, 255), 2)
elif darker == '2':
cv2.arrowedLine(im, (x1, y1), (x2, y2), (0, 0, 255), 2)
else:
cv2.line(im, (x1, y1), (x2, y2), (0, 255, 0), 1)
return im
def drawcoorperspective(vis,points,col_out=black,col_in=red,radius=2):
"""
Function to draw interaction with points to obtain perspective.
:param vis: image array.
:param points: list of points.
:param col_out: outer color of point.
:param col_in: inner color of point.
:param radius: radius of drawn points.
:return:
"""
points = np.array(points,INT)
thickness = radius-1
if len(points)>1 and len(points)<5:
for i in range(len(points)-1):
if i%2:
for j in range(i+1,min(len(points),i+3)):
if j%2:
#print "i=",i," j=",j
pt1 = (points[i][0], points[i][1])
pt2 = (points[j][0], points[j][1])
cv2.arrowedLine(vis, pt1, pt2, col_in, thickness)
else:
for j in range(i+1,min(len(points),i+3)):
#print "i=",i," j=",j
pt1 = (points[i][0], points[i][1])
pt2 = (points[j][0], points[j][1])
cv2.arrowedLine(vis, pt1, pt2, col_in, thickness)
vis = drawcoorpoints(vis,points,col_out,col_in,radius)
else:
vis = drawcoorpoints(vis,points,col_out,col_in,radius)
return vis
def draw_arrow(overlay, z_rotation, position,
line_width,
arrow_length,
arrow_color):
x_to = np.round(position[0] + arrow_length * np.cos(z_rotation)).astype(np.int32)
y_to = np.round(position[1] + arrow_length * np.sin(z_rotation)).astype(np.int32)
cv2.arrowedLine(overlay, tuple(position), (x_to, y_to),
arrow_color, line_width, cv2.LINE_AA)
def make_quiver(U, V, scale=5):
stride = 15 # plot every so many rows, columns
color = (0, 255, 0) # green
img_out = np.zeros((V.shape[0], U.shape[1], 3), dtype=np.uint8)
# print U
# print V
for y in xrange(0, V.shape[0], stride):
for x in xrange(0, U.shape[1], stride):
cv2.arrowedLine(img_out, (x, y), (x + int(U[y, x] * scale), y + int(V[y, x] * scale)), color, 1, tipLength=.3)
return img_out
def draw_arrowed_line(image, segment, color=None, thickness=1):
cv2.arrowedLine(
image.cv_image,
(round(segment.point1.x), round(segment.point1.y)),
(round(segment.point2.x), round(segment.point2.y)),
Color.random().to_bgr() if color == None else color.to_bgr(),
thickness,
cv2.LINE_AA,
0,
0.2,
)
def arrow(self, from_, to):
if self._image is None:
return
cv2.arrowedLine(
self._image,
tuple(from_.astype(int)),
tuple(to.astype(int)),
(255, 255, 255),
2,
cv2.LINE_AA,
)
def drawFeaturesMovementPers(kp_list, image, M):
# vg.im_transf = drawFeaturesMovementPers(
# vg.pv.kp_list,
# vg.im_transf,
# vg.map_M)
for kps in kp_list:
pt1 = transformPerspective(kps[0], M)
pt2 = transformPerspective(kps[1], M)
cv2.arrowedLine(image, intT(pt2), intT(pt1), (255,0,0), 2)
return image
def dessinerRobot(self):
if self.stationBase.getCarte().getRobot() is not None:
position = (self.stationBase.getCarte().getRobot().getX(), self.stationBase.getCarte().getRobot().getY())
self.anciennePosRobot.append(position)
if len(self.anciennePosRobot) >= 2:
for i in reversed(range(len(self.anciennePosRobot) - 1)):
cv2.arrowedLine(
self.imageVirtuelle, self.anciennePosRobot[i], self.anciennePosRobot[i + 1], (0, 0, 0), 2
)
else:
self.anciennePosRobot = []
def plot2(self, size = 960, area = 0.1): ha = celestial_rot() + self.status['gps'][1] qha = Quaternion([90-ha, 0, 0]) img = np.zeros((size, size, 3), dtype=np.uint8) c = size / 2 scale = size / area if self.ra is not None and self.dec is not None: t = Quaternion.from_ra_dec_pair([self.ra, self.dec], [self.prec_ra, self.prec_dec]) else: t = Quaternion.from_ra_dec_pair([0.0, 90.0], [self.prec_ra, self.prec_dec]) polaris = [37.9529, 89.2642] polaris_target = self.prec_q.transform_ra_dec(polaris) prec = t.transform_ra_dec([0, 90]) polaris_real = t.transform_ra_dec(polaris_target) polaris_target = qha.transform_ra_dec(polaris_target) prec = qha.transform_ra_dec(prec) polaris_real = qha.transform_ra_dec(polaris_real) polaris_target_xyz = ra_dec_to_xyz(polaris_target) polaris_r = (polaris_target_xyz[0] ** 2 + polaris_target_xyz[1] ** 2)**0.5 prec_xyz = ra_dec_to_xyz(prec) polaris_real_xyz = ra_dec_to_xyz(polaris_real) cv2.circle(img, (c,c), int(polaris_r * scale), (0, 255, 0), 1) for i in range (0, 24): a = np.deg2rad([i * 360.0 / 24.0]) sa = math.sin(a) ca = math.cos(a) cv2.line(img, (int(c + sa * polaris_r * scale), int(c + ca * polaris_r * scale)), (int(c + sa * (polaris_r * scale + 8)), int(c + ca * (polaris_r * scale + 8))), (0, 255, 0), 1) cv2.circle(img, (int(c + polaris_target_xyz[0] * scale), int(c + polaris_target_xyz[1] * scale)), 4, (0, 255, 0), 2) cv2.line(img, (0, c), (size, c), (0, 255, 0), 1) cv2.line(img, (c, 0), (c, size), (0, 255, 0), 1) if self.ra is not None and self.dec is not None: cv2.circle(img, (int(c + prec_xyz[0] * scale), int(c + prec_xyz[1] * scale)), 4, (255, 255, 255), 2) cv2.circle(img, (int(c + polaris_real_xyz[0] * scale), int(c + polaris_real_xyz[1] * scale)), 4, (255, 255, 255), 2) pole_dist = (prec_xyz[0] ** 2 + prec_xyz[1] ** 2) ** 0.5 if pole_dist >= area / 2: cv2.putText(img, "%0.1fdeg" % (90 - prec[1]), (int(c + prec_xyz[0] / pole_dist * area / 5 * scale - 50), int(c + prec_xyz[1] / pole_dist * area / 5 * scale)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255), 2) cv2.arrowedLine(img, (int(c + prec_xyz[0] / pole_dist * area / 3 * scale), int(c + prec_xyz[1] / pole_dist * area / 3 * scale)), (int(c + prec_xyz[0] / pole_dist * area / 2 * scale), int(c + prec_xyz[1] / pole_dist * area / 2 * scale)), (255, 255, 255), 2) return img
def draw_arrow(frame, position, direction, colour, length=750):
global scale, img_h
end = position + length * direction
x, y, z = position
pt1 = (int(x*scale), img_h-int(y*scale))
x, y, z = end
pt2 = (int(x*scale), img_h-int(y*scale))
# draw
cv2.circle(frame, pt1, 2, colour, 2)
cv2.arrowedLine(frame, pt1, pt2, colour, 2)
def dessinerTrajetPrevu(self):
if len(self.stationBase.getTrajectoirePrevue()) > 1:
pointInitial = None
for pointFinal in self.stationBase.getTrajectoirePrevue():
if pointInitial is None:
pointInitial = pointFinal
else:
cv2.arrowedLine(self.imageVirtuelle, pointFinal, pointInitial, (0, 255, 0), 2)
pointInitial = pointFinal
else:
cv2.putText(
self.imageVirtuelle, "Phase d" "alignement", (1000, 800), self.police, 1, (0, 0, 0), 1, cv2.LINE_AA
)
def visualize_node_arrows(self, img, node_id, size=3, color=(255,0,255)):
l, r, t, b = self.adjacency[node_id]
srcX, srcY = self.node_coordinates[node_id]
for direction, r in enumerate((l,r,t,b)):
if r == -1: continue
targetX, targetY = self.node_coordinates[r]
# Use constant arrow head size. As arrowedLine() takes a fraction of the length, we need to reverse that
length = np.hypot(targetX - srcX, targetY - srcY)
arrowHeadSizeWant = 15 #px
arrowHeadSize = arrowHeadSizeWant / length
print("Drawing <{3}> arrow from #{0} to #{1} of length {2}".format(
r, node_id, length, {0: "left", 1: "right", 2:"top", 3:"bottom"}[direction]))
cv2.arrowedLine(img, (int(targetX), int(targetY)), (int(srcX), int(srcY)),
color=color, thickness=3, tipLength=arrowHeadSize, line_type=cv2.LINE_AA)
def drawArrows(frame, fromCoords, toCoords, color, thickness, **kwargs):
"""
Draws arrows on an image
:param frame: image to draw arrows on
:param fromCoords: list of tuples containing from x/y coorinates
:param toCoords: list of tuples containing to x/y coorinates
:param color: color of arrows
:param thickness: arrow thicknes in pixels
:param **kwargs: extra aruments passed to cv2.arrowedLine
:return: None, edits image in place
"""
for i in xrange(len(toCoords)):
fromX,fromY = fromCoords[i]
toX, toY = toCoords[i]
cv2.arrowedLine(frame, (fromX,fromY), (toX,toY), color, thickness, **kwargs)
def draw_bot(position, front):
img = settings.maze['image']
if hasattr(img, 'shape'):
image = np.zeros(settings.maze['image'].shape, np.float32)
else:
image = np.zeros((img.width(), img.height()), np.float32)
try:
arrow_tip = (int(front[0]), int(front[1]))
position = (int(position[0]), int(position[1]))
# image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
cv2.arrowedLine(image, position, arrow_tip, (180, 255, 255), 2)
return image
except TypeError:
print "Bot missing!"
return image
def drawFeaturesMovement(kp_list, image, M, vect):
for kps in kp_list:
pt1 = transformPerspective(kps[0], M)
pt2 = transformPerspective(kps[1], M)
if isGoodMatch(pt1, pt2, vect):
cv2.arrowedLine(image, kps[1], kps[0], (255,0,0), 2)
else:
cv2.arrowedLine(image, kps[1], kps[0], (0,0,255), 2)
# cv2.putText(image,
# str(dst.euclidean(pt1,pt2)),
# (kps[1][0]+5,kps[1][1]+5),
# cv2.FONT_HERSHEY_DUPLEX,0.5,(255,0,255))
return image
def animate_contours(self, path='default'):
"""
Animates the found contours in order
"""
self.im2 = np.zeros(self.img.shape)
if path == 'default':
path = self.contours
elif path == 'sorted':
path = self.path
for contour in path:
for idx, point in enumerate(contour[0:-1]):
# Define line as line from first point to next point
p1 = (int(contour[idx][0]), int(contour[idx][1]))
p2 = (int(contour[idx+1][0]), int(contour[idx+1][1]))
cv2.arrowedLine(self.im2, p1, p2, (255,255,255))
cv2.imshow('contours', self.im2)
cv2.waitKey(1)
cv2.waitKey(0)
def draw_angled_arrow(image, center, angle):
"""
Draws a double sided arrow on image centered at center
at an angle of angle degrees.
"""
sin, cos = np.sin(radians(angle)), np.cos(radians(angle))
rotated_dir = np.array(((cos, -sin), (sin, cos))).dot(
np.array((0, -1)))
line_length = min(image.shape[0], image.shape[1]) * 0.17
line_start = np.array((center)) + rotated_dir * line_length
line_end = np.array((center)) - rotated_dir * line_length
def get_tup(vec):
return int(vec[0]), int(vec[1])
cv2.arrowedLine(image, get_tup(line_start),
get_tup(line_end), (255, 255, 0), 2)
cv2.arrowedLine(image, get_tup(line_end),
get_tup(line_start), (255, 255, 0), 2)
def filter_overlay(frame, config):
world = config.vision.world_latest
global D_POINT
if D_POINT is not None:
cv2.circle(frame, (int(D_POINT.x), int(D_POINT.y)), 5, BGR_COMMON['blue'], 3)
if world.ball is not None:
ball = world.ball
cv2.circle(frame, (int(ball.x), int(ball.y)), 6, BGR_COMMON['red'], 3)
if(config.vision.world_previous.ball is not None):
x = world.ball.x - config.vision.world_previous.ball.x
y = world.ball.y - config.vision.world_previous.ball.y
arrowhead = (int(x * 10 + world.ball.x), int(y * 10 + world.ball.y))
cv2.arrowedLine(frame, ball.centre, arrowhead, BGR_COMMON['red'], 2, cv2.LINE_AA)
for team in ['blue', 'yellow']:
for colour in ['pink', 'green']:
robot = getattr(world, "robot_{}_{}".format(team, colour))
if robot is None:
continue
cv2.circle(frame, (int(robot.x), int(robot.y)), 10, BGR_COMMON[colour], 3)
cv2.circle(frame, (int(robot.x), int(robot.y)), 6, BGR_COMMON[team], 3)
length = 15
complex = cmath.rect(length, robot.angle)
y = -complex.imag
x = complex.real
if robot.velocity != 0:
arrowhead = (int(x + robot.x), int(y + robot.y))
cv2.arrowedLine(frame, robot.centre, arrowhead, BGR_COMMON['black'], 2, cv2.LINE_AA)
return frame
def draw_vectors(image_bw, starting_points, vectors):
"""Draws vectors on a given black and white image.
Parameters
----------
image_bw : numpy matrix of integers
Stores the binary image that are being studied, with contents marked by
255s and background marked by 0s.
starting_points : List[(float, float)]
Stores the starting point of each vector.
vectors : List[List[(float, float)]]
Stores the vectors at each starting point.
Returns
-------
None
"""
for i in range(len(starting_points)):
pos = (int(starting_points[i][0]), int(starting_points[i][1]))
cv2.putText(image_bw, str(i), pos, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 255,\
1, cv2.LINE_AA, False)
for j in range(len(vectors[i])):
cv2.arrowedLine(image_bw, pos,\
(int(np.ceil(pos[0] + 10 * vectors[i][j][0])),\
int(np.ceil(pos[1] + 10 * vectors[i][j][1]))), 255)
def highlight_object(self, f, trace=False, stats=False):
if self.consec_time_unseen == 0:
other_edge = (self.box[0] + self.box[2], self.box[1] + self.box[3])
cv2.rectangle(f, self.box[0:2], other_edge, (0, 0, 255), 1)
cv2.circle(f, self.center, 3, (0, 0, 255), -1)
if trace:
f_copy = f.copy()
for i in range(max(0, len(self.path) - 20), len(self.path) - 1):
if i not in self.disappearance_indices:
cv2.line(f, self.path[i], self.path[i + 1], (0, 0, 0), 2)
cv2.addWeighted(f, .8, f_copy, .2, 0, f)
if stats:
t = self.tracking_threshold
if len(self.path) > t:
subset = self.path[-t:]
line_end_x = (subset[t - 1][0] - subset[t - 3][0]) * 2 + subset[t - 1][0]
line_end_y = (subset[t - 1][1] - subset[t - 3][1]) * 2 + subset[t - 1][1]
cv2.arrowedLine(f, subset[t - 1], (line_end_x, line_end_y), (255, 0, 0), 2)
prediction = self.predicted_path(5)
for p in prediction:
cv2.circle(f, p, 3, (50, 255, 50), -1)
def GetRectAngles(gray):
contours = 0
thresh = 0
rectangles = list()
count = 0
_, thresh = cv2.threshold(gray, 65, 255, 0)
_, contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
for cnt in contours:
rect = cv2.minAreaRect(cnt)
box = cv2.boxPoints(rect)
box = np.int0(box)
xy, wh, angle = rect
w, h = np.int0(wh)
if ((w * w + h * h) ** 0.5 > MinDiagonal) & ((w * w + h * h) ** 0.5 < MaxDiagonal):
count = count + 1
rectangles.append(rect)
cv2.drawContours(gray, [box], 0, (10, 20, 255), 2)
x, y = np.int0(xy)
if w > h:
x1 = np.int0(x + w)
y1 = np.int0(y + w * math.tan(math.radians(angle)))
cv2.arrowedLine(gray, (x, y), (x1, y1), (255, 255, 255))
else:
x1 = np.int0(x + w)
y1 = np.int0(y + w * math.tan(math.radians(angle + 90)))
cv2.arrowedLine(gray, (x, y), (x1, y1), (255, 255, 255))
cv2.imshow("ShowFoundRects", gray)
print count, " rectangles found. Press ESC to exit"
return rectangles
def plotforces(self, overlay, imageoutput):
sc = 2
#Get original pt locations
ox = self.orig_x[0:(2*self.N)].reshape((-1,2))
#Get prediction location
px = self.pred_x[0:(2*self.N)].reshape((-1,2))
#Get template, flow and mask 'force'
tv = self.tv[0:(2*self.N)].reshape((-1,2))
fv = self.fv[0:(2*self.N)].reshape((-1,2))
mv = self.mv[0:(2*self.N)].reshape((-1,2))
blank = np.zeros(overlay.shape, dtype=np.uint8)
blank[:,:,3] = 255
overlay = cv2.addWeighted(overlay, 0.5, blank, 0.5, 0)
#Resize image
overlay = cv2.resize(overlay, (0,0), fx = sc, fy = sc)
for idx in range(self.N):
cv2.arrowedLine(overlay, (sc*int(ox[idx,0]),sc*int(ox[idx,1])),\
(sc*int(px[idx,0]),sc*int(px[idx,1])), (255,255,255, 255), thickness = 2)
cv2.arrowedLine(overlay, (sc*int(px[idx,0]),sc*int(px[idx,1])),\
(sc*int(px[idx,0]+10*tv[idx,0]),sc*int(px[idx,1]+10*tv[idx,1])), (255,0,0, 255), thickness = 2)
cv2.arrowedLine(overlay, (sc*int(px[idx,0]),sc*int(px[idx,1])),\
(sc*int(px[idx,0]+10*fv[idx,0]),sc*int(px[idx,1]+10*fv[idx,1])), (0,255,0, 255), thickness = 2)
cv2.arrowedLine(overlay, (sc*int(px[idx,0]),sc*int(px[idx,1])),\
(sc*int(px[idx,0]+10*mv[idx,0]),sc*int(px[idx,1]+10*mv[idx,1])), (0,0,255, 255), thickness = 2)
font = cv2.FONT_HERSHEY_SIMPLEX
#cv2.putText(img,'Hello World!',(10,500), font, 1,(255,255,255),2)
legendtext = 'red = mask force\ngreen = flow force\nblue = template force\nwhite = prediction'
x0, y0 = (20,20)
dy = 20
for i, line in enumerate(legendtext.split('\n')):
y = y0 + i*dy
cv2.putText(overlay, line, (x0, y), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255,255,255,255))
#cv2.putText(overlay, legendtext, (20, 20), font, 1, (255, 255, 255), 2)
fn = './' + imageoutput + '_forces_' + strftime("%Y-%m-%d_%H:%M:%S", gmtime()) + '.png'
cv2.imwrite(fn, overlay)
def show_lines(self, movement):
"""
Plots the average vectors of each of the four quadrants of the image
"""
# quadrant 1 vector
cv2.arrowedLine(
self.frame,
(int(self.frame.shape[1] * 1 / 4), int(self.frame.shape[0] * 1 / 4)),
(int(self.frame.shape[1] * 1 / 4 + movement[0][0]), int(self.frame.shape[0] * 1 / 4 + movement[0][1])),
(0, 0, 255),
thickness=3,
)
# quadrant 2 vector
cv2.arrowedLine(
self.frame,
(int(self.frame.shape[1] * 3 / 4), int(self.frame.shape[0] * 1 / 4)),
(int(self.frame.shape[1] * 3 / 4 + movement[1][0]), int(self.frame.shape[0] * 1 / 4 + movement[1][1])),
(0, 0, 255),
thickness=3,
)
# quadrant 3 vector
cv2.arrowedLine(
self.frame,
(int(self.frame.shape[1] * 1 / 4), int(self.frame.shape[0] * 3 / 4)),
(int(self.frame.shape[1] * 1 / 4 + movement[2][0]), int(self.frame.shape[0] * 3 / 4 + movement[2][1])),
(255, 0, 0),
thickness=3,
)
# quadrant 4 vector
cv2.arrowedLine(
self.frame,
(int(self.frame.shape[1] * 3 / 4), int(self.frame.shape[0] * 3 / 4)),
(int(self.frame.shape[1] * 3 / 4 + movement[3][0]), int(self.frame.shape[0] * 3 / 4 + movement[3][1])),
(255, 0, 0),
thickness=3,
)
def dispOpticalFlow(Image, Flow, Divisor=1):
"Display image with a visualisation of a flow over the top. A divisor controls the density of the quiver plot."
PictureShape = np.shape(Image)
# determine number of quiver points there will be
Imax = int(PictureShape[0] / Divisor)
Jmax = int(PictureShape[1] / Divisor)
# create a blank mask, on which lines will be drawn.
mask = np.zeros_like(Image)
for i in range(1, Imax):
for j in range(1, Jmax):
X1 = (i) * Divisor
Y1 = (j) * Divisor
X2 = int(X1 + Flow[X1, Y1, 1])
Y2 = int(Y1 + Flow[X1, Y1, 0])
X2 = np.clip(X2, 0, PictureShape[0])
Y2 = np.clip(Y2, 0, PictureShape[1])
# add all the lines to the mask
# mask = cv2.line(mask, (Y1,X1),(Y2,X2), [0, 0, 100], 2)
mask = cv2.arrowedLine(mask, (Y1, X1), (Y2, X2), [100, 0, 0], 1)
# superpose lines onto image
img = cv2.add(Image / np.max(Image) * 2, mask)
# print image
return img
def run(self):
self.threads = [
threading.Thread(target=self.face_thread),
threading.Thread(target=self.land_pose_thread),
threading.Thread(target=self.gaze_thread)
]
for thread in self.threads:
thread.start()
while self.should_run():
try:
read_correctly, new_frame = self.get_frame()
except RuntimeError:
continue
if not read_correctly:
break
self.fps.update()
self.frame = new_frame
self.debug_frame = self.frame.copy()
if not camera:
nn_data = depthai.NNData()
nn_data.setLayer("data", to_planar(self.frame, (300, 300)))
self.face_in.send(nn_data)
if debug: # face
if self.gaze is not None and self.left_bbox is not None and self.right_bbox is not None:
re_x = (self.right_bbox[0] + self.right_bbox[2]) // 2
re_y = (self.right_bbox[1] + self.right_bbox[3]) // 2
le_x = (self.left_bbox[0] + self.left_bbox[2]) // 2
le_y = (self.left_bbox[1] + self.left_bbox[3]) // 2
x, y = (self.gaze * 100).astype(int)[:2]
if args.lazer:
beam_img = np.zeros(self.debug_frame.shape, np.uint8)
for t in range(10)[::-2]:
cv2.line(beam_img, (re_x, re_y),
((re_x + x * 100), (re_y - y * 100)),
(0, 0, 255 - t * 10), t * 2)
cv2.line(beam_img, (le_x, le_y),
((le_x + x * 100), (le_y - y * 100)),
(0, 0, 255 - t * 10), t * 2)
self.debug_frame |= beam_img
else:
cv2.arrowedLine(self.debug_frame, (le_x, le_y),
(le_x + x, le_y - y), (255, 0, 255), 3)
cv2.arrowedLine(self.debug_frame, (re_x, re_y),
(re_x + x, re_y - y), (255, 0, 255), 3)
if not args.lazer:
for raw_bbox in self.bboxes:
bbox = frame_norm(self.frame, raw_bbox)
cv2.rectangle(self.debug_frame, (bbox[0], bbox[1]),
(bbox[2], bbox[3]), (10, 245, 10), 2)
if self.nose is not None:
cv2.circle(self.debug_frame,
(self.nose[0], self.nose[1]),
2, (0, 255, 0),
thickness=5,
lineType=8,
shift=0)
if self.left_bbox is not None:
cv2.rectangle(self.debug_frame,
(self.left_bbox[0], self.left_bbox[1]),
(self.left_bbox[2], self.left_bbox[3]),
(245, 10, 10), 2)
if self.right_bbox is not None:
cv2.rectangle(self.debug_frame,
(self.right_bbox[0], self.right_bbox[1]),
(self.right_bbox[2], self.right_bbox[3]),
(245, 10, 10), 2)
if self.pose is not None and self.nose is not None:
draw_3d_axis(self.debug_frame, self.pose, self.nose)
if camera:
cv2.imshow("Camera view", self.debug_frame)
else:
aspect_ratio = self.frame.shape[1] / self.frame.shape[0]
cv2.imshow(
"Video view",
cv2.resize(self.debug_frame,
(int(900), int(900 / aspect_ratio))))
if cv2.waitKey(1) == ord('q'):
cv2.destroyAllWindows()
break
self.fps.stop()
print("FPS: {:.2f}".format(self.fps.fps()))
if not camera:
self.cap.release()
cv2.destroyAllWindows()
for i in range(1, 5): # https://stackoverflow.com/a/25794701/5494277
cv2.waitKey(1)
self.running = False
def connectdots(keypoints_dark, keypoints_light, im_with_keypoints,
latticethresh, line_length_lower, line_length_upper,
IslandProperties):
for dark in keypoints_dark:
xd = int(
dark.pt[0]) #assigns x and y values and puts them as a coordinate
yd = int(dark.pt[1])
cd = (xd, yd)
for light in keypoints_light:
xl = int(light.pt[0])
yl = int(light.pt[1])
cl = (xl, yl)
xa, ya = (xd + xl) / 2, (yd + yl) / 2 #assigns middle coordinates
pixelcolour = latticethresh[int(xa), int(
ya)] #gets pixel colour on the lattice image
if pixelcolour != 255: #if the pixel colour is not black, continue to line drawing.
if (
xd - xl
) == 0: #fixes problem when divinding by zero in tan function.
if yd < yl:
xd += 0.1
else:
xl += 0.1
if yd == yl:
if xd > xl:
deg = 0
if xl > xd:
deg = 180
elif xd > xl and yd > yl: #4th quad
deg = math.degrees(math.atan((yd - yl) / (xd - xl)))
deg = 360 - deg
#print(deg)
elif yd > yl and xd < xl:
deg = math.degrees(math.atan(
(yd - yl) / (xl - xd))) #third
deg = 180 + deg
#print(deg)
elif yd < yl and xd < xl:
deg = math.degrees(math.atan(
(yl - yd) / (xl - xd))) #second
deg = 180 - deg
#print(deg)
elif yd < yl and xd > xl:
deg = math.degrees(math.atan(
(yl - yd) / (xd - xl))) #first
if line_length_lower < (
(xd - xl)**2 + (yd - yl)**2)**0.5 < line_length_upper and (
(deg < degree_change) or ((60 - degree_change) < deg <
(60 + degree_change)) or
((120 - degree_change) < deg <
(120 + degree_change)) or
((180 - degree_change) < deg <
(180 + degree_change)) or
((240 - degree_change) < deg <
(240 + degree_change)) or
((300 - degree_change) < deg <
(3000 + degree_change)) or (deg >
(360 - degree_change))):
mx, my = 0, 0
if (deg < degree_change) or (deg > (360 - degree_change)):
mx = 1
my = 0
cv2.arrowedLine(im_with_keypoints, cl, cd, (255, 0, 0),
2)
print(deg)
if (60 - degree_change) < deg < (60 + degree_change):
mx = 0.5
my = 0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd,
(255, 182, 0), 2)
if (120 - degree_change) < deg < (120 + degree_change):
mx = -0.5
my = 0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd,
(178, 255, 0), 2)
if (180 - degree_change) < deg < (180 + degree_change):
mx = -1
my = 0
cv2.arrowedLine(im_with_keypoints, cl, cd,
(0, 242, 255), 2)
if (240 - degree_change) < deg < (240 + degree_change):
mx = -0.5
my = -0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd, (0, 0, 255),
2)
if (300 - degree_change) < deg < (300 + degree_change):
mx = 0.5
my = -0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd,
(255, 0, 255), 2)
IslandProperties.append([xa, ya, mx, my])
return (IslandProperties)
def draw_ant( a, img, length = 5 ):
# OpenCV works in 4th quadrant
p1 = a.x, a.y
p2 = a.x + int(length * math.cos( a.angle)), a.y - int(length * math.sin( a.angle))
cv2.arrowedLine( img, (p2[1], p2[0]), (p1[1], p1[0]), 255, 2 )
ret, frame = camera.read()
frame = cv2.resize(frame, (640, 480))
robot = RealDifferentialDrive(get_robot_xyyaw(frame,1.0), \
0.12, \
0.18, \
0.03, \
1.5, 0.2)
now = time.time()
while (1):
ret, frame = camera.read()
frame = cv2.resize(frame, (640, 480))
h, w, c = frame.shape
pos = get_robot_xyyaw(frame, 1.0)
robot.update_info(time.time() - now, pos)
x = int(robot.x * h)
y = int(h - robot.y * h)
cv2.circle(frame, (x, y), 4, [0, 0, 255], 2)
finish = (x + int(50 * math.cos(robot.yaw)),
y + int(50 * math.sin(robot.yaw)))
cv2.arrowedLine(frame, (x, y), finish, [0, 0, 255], 1)
cv2.imshow('robot', frame)
if cv2.waitKey(30) & 0xff == 27:
break
now = time.time()
cv2.destroyAllWindows()
def draw(self, image):
# print (self.color)
cv2.arrowedLine(image, (self.circle.x, self.circle.y), (self.x, self.y), self.color, self.size)
new = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
try:
flow = 3 * cv2.calcOpticalFlowFarneback(
prvs, new, flow, 0.5, 3, 15, 3, 7, 1.5,
cv2.OPTFLOW_USE_INITIAL_FLOW)
cumulative = flow * 0.4 + 0.6 * cumulative # exponential smoothing
r = estimateHeading(cumulative)
print("heading: {}".format(r))
sth = estimateSomething(r, cumulative)
blank = np.zeros((H, W, 3), np.uint8)
frame2 = draw_flow(new, cumulative)
drawn = drawSth(sth, blank)
drawn = cv2.arrowedLine(drawn, (W // 2, H // 2),
(W // 2 + int(r * 100), H // 2),
(255, 0, 0),
thickness=2)
cv2.imshow("original image", frame2)
cv2.imshow("recognition", drawn)
except NameError:
pass
# time.sleep(0.1)
k = cv2.waitKey(30) & 0xff
if k == 27:
break
elif k == ord('s'):
cv2.imwrite('optical_flow.png', drawn)
prvs = new
cap.release()
def show(self):
"""First shows a bar plot of the action distribution. Then all observations are shown.
This function is for demonstration purposes only. Use left and right arrow keys to navigate
"""
data = []
actions = []
conditions = []
for i in self.data_dir.rglob('*.npy'):
obs = np.load(i, allow_pickle=True).item()
new_file = {
"obs": obs['observation'],
"condition": obs['condition'],
"action": self.class_names[obs['action'] - 1],
"name": i.name
}
data.append(new_file)
actions.append(self.class_names[obs['action'] - 1])
conditions.append(obs['condition'])
unique_actions, count_actions = np.unique(actions, return_counts=True)
fig, ax = plt.subplots()
plt.bar(unique_actions, count_actions)
plt.show()
#show the individual observations:
num_files = len(data)
i = 0
while i < num_files:
print("{}/{}".format(i + 1, num_files))
obs = data[i]
image = cv2.cvtColor(obs['obs'], cv2.COLOR_BGR2RGB)
cv2.putText(image, obs["action"], (10, 25),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255))
cv2.putText(image, obs["name"], (10, 220),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
if obs["condition"] == 1:
arrow_start = (300, 30)
arrow_end = (300, 10)
elif obs["condition"] == 2:
arrow_start = (310, 20)
arrow_end = (290, 20)
elif obs["condition"] == 3:
arrow_start = (300, 10)
arrow_end = (300, 30)
elif obs["condition"] == 4:
arrow_start = (290, 20)
arrow_end = (310, 20)
cv2.arrowedLine(image,
arrow_start,
arrow_end, (0, 0, 255),
2,
tipLength=0.5)
cv2.imshow('image', image)
cvkey = cv2.waitKeyEx(0)
if cvkey == 27: #escape
break
elif cvkey == 2555904: #right
i += 1
elif cvkey == 2424832 and i > 0: #left
i -= 1
def paint_overlay_line(self, image, steer, accel):
print(steer, accel)
x_0, y_0 = image.shape[1] // 2, image.shape[0] // 2
x_1, y_1 = round(x_0 + steer * 50), y_0 - round((accel * 1.14) * 200)
cv2.arrowedLine(image, (x_0, y_0), (x_1, y_1), (255, 0, 0), 3)
return image
import numpy as np
import cv2
img = cv2.imread('D:\\puppies\\puppy1.jpg')
img = cv2.line(img, (0, 0), (255, 255), (128, 255, 0), 5)
img = cv2.arrowedLine(img, (0, 0), (255, 255), (128, 255, 0), 5)
cv2.imshow('image', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
def calculate_pitch_yaw_vertical(self, bbox_list):
target_centroid = list()
area = list()
for i in bbox_list:
pt1 = (int(i[0]), int(i[1]))
pt2 = (int(i[2]), int(i[3]))
image_roi = self.result[pt1[1]:pt2[1], pt1[0]:pt2[0]]
try:
image_roi = cv2.inRange(image_roi, self.LOWER_RED_RANGE,
self.UPPER_RED_RANGE)
image_roi = cv2.medianBlur(image_roi, 11)
cv2.imshow('ROI', image_roi)
_, contours, hierarchy = cv2.findContours(
image_roi.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if contours:
dx = pt2[0] - pt1[0]
dy = pt2[1] - pt1[1]
target_centroid.append(
(int(pt1[0] + dx / 2), int(pt1[1] + dy / 2)))
cv2.rectangle(self.result,
pt1=pt1,
pt2=pt2,
color=[0, 0, 255],
thickness=6)
area.append(dx * dy)
except:
print("Red Person Not Found!")
self.pitch_rate = 0
self.yaw_rate = 0
self.vertical_rate = 0
pass
# 드론 중점 그림
cv2.circle(self.result,
self.drone_centroid,
radius=4,
color=[255, 0, 0],
thickness=2)
dst = list()
# 드론 중점과 타겟간 중점 그림
for i in target_centroid:
cv2.circle(self.result,
i,
radius=4,
color=[0, 0, 255],
thickness=2)
cv2.arrowedLine(self.result,
self.drone_centroid,
i,
color=[255, 0, 0],
thickness=4)
dst.append(distance.euclidean(self.drone_centroid, i))
try:
if dst[0] > 10:
self.yaw_rate = int(dst[0] / 2)
self.vertical_rate = int(dst[0] / 20)
# 우하단
if self.drone_centroid[0] <= target_centroid[0][
0] and self.drone_centroid[1] <= target_centroid[0][1]:
self.vertical_rate = -self.vertical_rate
# 좌하단
elif self.drone_centroid[0] > target_centroid[0][
0] and self.drone_centroid[1] <= target_centroid[0][1]:
self.yaw_rate = -self.yaw_rate
self.vertical_rate = -self.vertical_rate
# 좌상단
elif self.drone_centroid[0] > target_centroid[0][
0] and self.drone_centroid[1] > target_centroid[0][1]:
self.yaw_rate = -self.yaw_rate
else:
self.yaw_rate = 0
self.vertical_rate = 0
if area[0] > 25000:
self.pitch_rate = -int(area[0] / 30000) * 2
else:
self.pitch_rate = int(30000 / area[0]) * 2
print("Red Person & Centroid Found!\narea[0]:{}, dst[0]:{}".format(
area[0], dst[0]))
except IndexError:
print("Centroid Not Found!")
self.pitch_rate = 0
self.yaw_rate = 0
self.vertical_rate = 0
pass #list index out of range 일 경우 아무것도 하지 않도록 예외처리
side_img, (int(side_img.shape[1] / 2), int(side_img.shape[0] / 2)))
ball_loc, side_img = ball_detection(side_img)
# print("ball loc",ball_loc)
# print("prev ball loc", ball_loc_prev)
print(ball_loc)
#calculate velocity vector and plot
if ball_loc != None and ball_loc_prev != None:
velocity_vec = tuple(map(lambda i, j: (i - j), ball_loc,
ball_loc_prev))
# print(velocity_vec)
print("velocity:", np.linalg.norm(np.array(velocity_vec) / 1 / 60))
side_img = cv.arrowedLine(side_img,
pt1=ball_loc,
pt2=tuple(
map(lambda i, j: i + j * scale, ball_loc,
velocity_vec)),
color=(255, 0, 0),
thickness=3)
# frame when ball touches ground
if velocity_vec[0] == 0:
velocity_slope = np.inf
else:
velocity_slope = velocity_vec[1] / velocity_vec[0]
# print(velocity_slope)
if contact_loc == None and velocity_slope >= 0:
contact_loc = ball_loc_prev
if contact_loc != None:
cv.circle(side_img, contact_loc, 10, (0, 0, 255), -1)
# print(contact_loc)
def redo(im_with_keypoints, IslandProperties):
cv2.imshow("Keypoints", im_with_keypoints)
cv2.setMouseCallback("Keypoints", click_event_bars)
cv2.waitKey(-1)
for i in range(int(len(newbar) * 0.5)):
xl = int(newbar[2 * i][0])
yl = int(newbar[2 * i][1])
cl = (xl, yl)
xd = int(newbar[(2 * i) + 1][0])
yd = int(newbar[(2 * i) + 1][1])
cd = (xd, yd)
xa, ya = (xd + xl) / 2, (yd + yl) / 2 #assigns middle coordinates
ca = (xa, ya)
if (xd - xl
) == 0: #fixes problem when divinding by zero in tan function.
if yd < yl:
xd += 0.1
else:
xl += 0.1
if yd == yl:
if xd > xl:
deg = 0
if xl > xd:
deg = 180
elif xd > xl and yd > yl: #4th quad
deg = math.degrees(math.atan((yd - yl) / (xd - xl)))
deg = 360 - deg
#print(deg)
elif yd > yl and xd < xl:
deg = math.degrees(math.atan((yd - yl) / (xl - xd))) #third
deg = 180 + deg
#print(deg)
elif yd < yl and xd < xl:
deg = math.degrees(math.atan((yl - yd) / (xl - xd))) #second
deg = 180 - deg
#print(deg)
elif yd < yl and xd > xl:
deg = math.degrees(math.atan((yl - yd) / (xd - xl))) #first
if (deg > 45 and deg < 225):
cv2.arrowedLine(im_with_keypoints, cl, cd, (0, 0, 255), 2)
elif (deg < 45 or deg > 225):
cv2.arrowedLine(im_with_keypoints, cl, cd, (255, 0, 0), 2)
if (deg < degree_change) or (deg > (360 - degree_change)):
mx = 1
my = 0
cv2.arrowedLine(im_with_keypoints, cl, cd, (255, 0, 0), 2)
print(deg)
if (60 - degree_change) < deg < (60 + degree_change):
mx = 0.5
my = 0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd, (255, 182, 0), 2)
if (120 - degree_change) < deg < (120 + degree_change):
mx = -0.5
my = 0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd, (178, 255, 0), 2)
if (180 - degree_change) < deg < (180 + degree_change):
mx = -1
my = 0
cv2.arrowedLine(im_with_keypoints, cl, cd, (0, 242, 255), 2)
if (240 - degree_change) < deg < (240 + degree_change):
mx = -0.5
my = -0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd, (0, 0, 255), 2)
if (300 - degree_change) < deg < (300 + degree_change):
mx = 0.5
my = -0.866025
cv2.arrowedLine(im_with_keypoints, cl, cd, (255, 0, 255), 2)
IslandProperties.append([xa, ya, mx, my])
return (IslandProperties)
def draw_arrow(self, frame):
"""This Function draws an arrow according to the current direction of the ball along the ball.
Args:
self: This class.
frame: Image or Frame to draw the arrow in.
Returns:
None.
"""
box = self.box
if len(box) == 0:
return
center_line_a = ((box[1][0] + int(
(box[0][0] - box[1][0]) / 2)) if box[0][0] > box[1][0] else
(box[0][0] + int(
(box[1][0] - box[0][0]) / 2)), (box[1][1] + int(
(box[0][1] - box[1][1]) / 2))
if box[0][1] > box[1][1] else (box[0][1] + int(
(box[1][1] - box[0][1]) / 2)))
center_line_b = ((box[2][0] + int(
(box[1][0] - box[2][0]) / 2)) if box[1][0] > box[2][0] else
(box[1][0] + int(
(box[2][0] - box[1][0]) / 2)), (box[2][1] + int(
(box[1][1] - box[2][1]) / 2))
if box[1][1] > box[2][1] else (box[1][1] + int(
(box[2][1] - box[1][1]) / 2)))
center_line_c = ((box[3][0] + int(
(box[2][0] - box[3][0]) / 2)) if box[2][0] > box[3][0] else
(box[2][0] + int(
(box[3][0] - box[2][0]) / 2)), (box[3][1] + int(
(box[2][1] - box[3][1]) / 2))
if box[2][1] > box[3][1] else (box[2][1] + int(
(box[3][1] - box[2][1]) / 2)))
center_line_d = ((box[0][0] + int(
(box[3][0] - box[0][0]) / 2)) if box[3][0] > box[0][0] else
(box[3][0] + int(
(box[0][0] - box[3][0]) / 2)), (box[0][1] + int(
(box[3][1] - box[0][1]) / 2))
if box[3][1] > box[0][1] else (box[3][1] + int(
(box[0][1] - box[3][1]) / 2)))
left = center_line_a if center_line_a[0] < center_line_b[
0] else center_line_b
left = left if left[0] < center_line_c[0] else center_line_c
left = left if left[0] < center_line_d[0] else center_line_d
right = center_line_a if center_line_a[0] > center_line_b[
0] else center_line_b
right = right if right[0] > center_line_c[0] else center_line_c
right = right if right[0] > center_line_d[0] else center_line_d
if self.left and left and right:
self.lastArrowHead = self.arrowhead
self.lastArrowTail = self.arrowtail
self.arrowhead = left
self.arrowtail = right
cv2.arrowedLine(frame, right, left, (0, 255, 0), 3)
return
elif left and right:
self.lastArrowHead = self.arrowhead
self.lastArrowTail = self.arrowtail
self.arrowhead = right
self.arrowtail = left
cv2.arrowedLine(frame, left, right, (0, 255, 0), 3)
return
import cv2
import numpy as np
if __name__ == "__main__":
print(f"cv2 version is {cv2.__version__}")
size = np.array([480, 640, 3])
# 白のキャンバス
img = np.full(size, (255, 255, 255), dtype=np.uint8)
color = np.array([0., 0., 255])
cv2.arrowedLine(
img=img,
pt1=(30, 50),
pt2=(320, 50),
color=color,
thickness=5,
)
cv2.arrowedLine(
img=img,
pt1=(610, 100),
pt2=(320, 100),
color=color,
thickness=5,
)
cv2.arrowedLine(
img=img,
pt1=(0, 480),
pt2=(320, 240),
color=color,
thickness=5,
def visualize_video(video):
frame_count = video[3]
if frame_count == -1:
return
moi_count = len(video[1])
track_list = []
t = time.time()
video_name = os.path.basename(os.path.normpath(video[4]))
video[4] = cv2.VideoCapture(video[4])
vid = video[4]
vid_fps = int(vid.get(cv2.CAP_PROP_FPS))
width, height = int(vid.get(cv2.CAP_PROP_FRAME_WIDTH)), int(
vid.get(cv2.CAP_PROP_FRAME_HEIGHT))
codec = cv2.VideoWriter_fourcc(*'mp4v')
video[2] = cv2.VideoWriter(video[2], codec, vid_fps, (width, height))
curr_count = np.zeros(shape=(moi_count, 5), dtype=int)
img = Image.new('L', (width, height), 0)
ImageDraw.Draw(img).polygon([tuple(x) for x in video[0]],
outline=1,
fill=1)
roi_mask = np.array(img)
segment_start = list(range(0, frame_count, FRAME_PER_SEGMENT))
segment_end = segment_start[1:].append(frame_count)
num_segment = len(segment_start)
flash_list = []
max_traffic = max([sum([len(y) for y in x]) for x in video[5]])
print('max traffic', video_name, max_traffic)
for i in range(num_segment):
print('Video: {}. Segment {:03d}/{:03d} ({:05.2f}%)'.format(
video_name, (i + 1), num_segment, 100 * (i + 1) / num_segment))
img = np.ndarray(shape=(FRAME_PER_SEGMENT, height, width, 3),
dtype=np.uint8)
for j in range(FRAME_PER_SEGMENT):
_, frame = vid.read()
img[j] = frame
for j in range(FRAME_PER_SEGMENT):
# cv2.polylines(img[j], [video[0]], isClosed=True, color=ROI_COLOR_BGR, thickness=4)
ALPHA = 96
ROI_COLOR_BGR_MODIFIED = np.array(ROI_COLOR_BGR, dtype=np.float32)
ROI_COLOR_BGR_MODIFIED = np.clip(
ROI_COLOR_BGR_MODIFIED *
(1 + 0.5 * len(flash_list) / max_traffic), 0, 255)
img[j][np.where(roi_mask)] = (
ROI_COLOR_BGR_MODIFIED * ALPHA / 255 +
img[j][np.where(roi_mask)].astype(np.float32) *
(1 - ALPHA / 255)).astype(np.uint8)
for moi_id, moi in enumerate(video[1]):
if moi_id == 0:
continue
cv2.polylines(img[j], [moi[:-1]],
isClosed=False,
color=getColorMOI_BGR(moi_id),
thickness=2)
cv2.arrowedLine(img[j],
tuple(moi[-2]),
tuple(moi[-1]),
color=getColorMOI_BGR(moi_id),
thickness=2,
tipLength=0.03)
cv2.rectangle(img[j], (1060 - 175 * ((moi_count - 2) // 6), 0),
(1280, 200),
color=(224, 224, 224),
thickness=-1)
for moi_id in range(1, moi_count):
obj_list = video[5][i * FRAME_PER_SEGMENT + j][moi_id]
for obj in obj_list:
curr_count[moi_id][obj[0]] += 1
if obj[1][0] > -1:
flash_list.append(
(i * FRAME_PER_SEGMENT + j, obj, moi_id))
count_str = ' '.join([str(x) for x in curr_count[moi_id][1:]])
moi = video[1][moi_id]
cv2.putText(img[j],
count_str, (1080 - 175 * ((moi_id - 1) // 6), 35 +
((moi_id - 1) % 6) * 25),
cv2.FONT_HERSHEY_COMPLEX,
fontScale=0.6,
color=getColorMOI_BGR(moi_id),
thickness=2)
# Remove frames older than 0.25s
flash_list = [
flash for flash in flash_list
if (i * FRAME_PER_SEGMENT + j - flash[0] < (vid_fps * 0.25))
]
for flash in flash_list:
radius = (30 * flash[1][1][1] // height)
if radius <= 12: radius = 12
cv2.circle(img[j],
flash[1][1],
radius=radius,
color=getColorMOI_BGR(flash[2]),
thickness=-1)
cv2.putText(img[j],
str(flash[1][0]),
(flash[1][1][0] - radius, flash[1][1][1] - radius),
cv2.FONT_HERSHEY_COMPLEX,
fontScale=max(0.4, flash[1][1][1] / height),
color=(0, 255, 255),
thickness=2)
frame_str = "frame_id: " + str(i * FRAME_PER_SEGMENT + j + 1)
cv2.putText(img[j],
frame_str, (30, 30),
cv2.FONT_HERSHEY_COMPLEX,
fontScale=1,
color=(0, 0, 255),
thickness=2)
[video[2].write(frame) for frame in img]
print(time.time() - t)
video[2].release()
img = img.astype(np.uint8)
out_path = os.path.join(out_dir, os.path.basename(img_path[img_idx]))
img_idx += 1
h, w = flow_x.shape
n_hregion = int(h / ksize)
n_wregion = int(w / ksize)
for h_idx in range(n_hregion):
for w_idx in range(n_wregion):
w_start = ksize * w_idx
w_end = w_start + ksize
h_start = ksize * h_idx
h_end = h_start + ksize
mean_x = np.mean(flow_x[h_start:h_end, w_start:w_end])
mean_y = np.mean(flow_y[h_start:h_end, w_start:w_end])
#print("(h, w) = (%d, %d), (y, x) = (%d, %d)" % (h_start, w_start, mean_y, mean_x))
pt1, pt2 = calc_point(w_start, w_end, h_start, h_end, mean_x,
mean_y)
cv2.arrowedLine(img,
pt1,
pt2, (255, 0, 0),
arrow_weight,
tipLength=tipLength)
misc.imsave(out_path, img)
import cv2
import numpy as np
img = cv2.imread('lena.jpg', 1)
img = cv2.line(img, (0,0), (255,255), (0, 255, 255), 3)
img = cv2.arrowedLine(img, (0,255), (255,255), (255, 255, 0), 3)
img = cv2.rectangle(img, (384,0), (510,128), (255,100,100), -1)
img = cv2.circle(img, (447,63), 63, (100, 0, 255), -1)
font = cv2.FONT_HERSHEY_SIMPLEX
img = cv2.putText(img, 'OpenCV', (30,480), font, 4, (255,255,255),5,cv2.LINE_AA)
cv2.imshow('image', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows,cols,channels) = cv_image.shape
blueColor = (255,0,0)
greenColor = (0,255,0)
redColor = (0,0,255)
###########################################################################################
# COLOR DETECTION (B,G,R)
###########################################################################################
# Define color detection area
detectionAreaYRange = (int(rows/3), int(2*rows/3))
detectionAreaXRange = (int(cols/3), int(2*cols/3))
colorROI = np.array([
[detectionAreaXRange[0], 0],
[detectionAreaXRange[1], 0],
[detectionAreaXRange[1], rows],
[detectionAreaXRange[0], rows]
])
# Count blue and/or green pixels within detection area
numBluePixels = 0
numGreenPixels = 0
# for i in range(detectionAreaYRange[0], detectionAreaYRange[1], 3):
for i in range(0, rows, 10):
# for j in range(0, cols, 10):
for j in range(detectionAreaXRange[0], detectionAreaXRange[1], 10):
if cv_image[i,j,0] >= 200:
numBluePixels += 1
# cv_image[i,j] = [0,255,0]
elif cv_image[i,j,1] >= 240:
numGreenPixels += 1
# Decide if there are enough blue pixels to consider them a blue tool
if numBluePixels >= 20:
toolDetectionMsg = "Blue tool detected"
blueToolDetected = True
else:
toolDetectionMsg = "No tool detected"
blueToolDetected = False
# Decide if there are enough blue pixels to consider them a mounting dock containing a Trocar
if numGreenPixels >= 20:
trocarDetectionMsg = "Trocar detected"
trocarDetected = True
else:
trocarDetectionMsg = "No Trocar detected"
trocarDetected = False
###########################################################################################
# SHAPE DETECTION
###########################################################################################
# Restrict detection in the center columns
# img_detection_region = cv_image[0:rows, int(cols/3):int(2*cols/3)]
img_detection_region = cv_image
# convert to grayscale
gray = cv2.cvtColor(img_detection_region, cv2.COLOR_BGR2GRAY)
# make boundary of image white so that contours will be always closed loops within the region
gray[0:img_detection_region.shape[0], 0].fill(255)
gray[0:img_detection_region.shape[0], img_detection_region.shape[1]-1].fill(255)
gray[0, 0:img_detection_region.shape[1]].fill(255)
gray[img_detection_region.shape[0]-1, 0:img_detection_region.shape[1]].fill(255)
# blur the image
blur = cv2.blur(gray, (3, 3))
# thresholded image for blue color
ret, thresh = cv2.threshold(blur, 50, 255, cv2.THRESH_BINARY)
# thresholded image for green color
ret, threshGreen = cv2.threshold(blur, 110, 255, cv2.THRESH_BINARY)
# Finding contours for the thresholded image
im2, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
im2, contoursGreen, hierarchy = cv2.findContours(threshGreen, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# create hull array for convex hull points
hull = []
# calculate points for each contour
for i in range(len(contours)):
# creating convex hull object for each contour
hull.append(cv2.convexHull(contours[i], False))
# Calculate and store the center of mass of each contour
contourCenterOfMass = []
for i in range(len(contours)):
contourCenterOfMass.append(geometry.centerOfMass(contours[i]))
# For each convex hull, calculate mean distance of hull points from center of mass
hullsMeanDistanceFromCenter = []
for i in range(len(hull)):
sumD = 0
for point in hull[i]:
cm = contourCenterOfMass[i]
distance = math.sqrt((point[0][0] - cm[0])**2 + (point[0][1] - cm[1])**2)
sumD += distance
avgD = sumD/len(hull[i])
hullsMeanDistanceFromCenter.append(avgD)
# Calculate max convex hull by finding the maximum mean distance
if len(hull) > 1:
startFrom = 1 # Ignore first convex hull (if there are many), because it is the whole picture
else:
startFrom = 0
maxHullIndex = startFrom
for i in range(startFrom, len(hull)):
if hasPolygonCenterOfColor(cv_image, contours[i], blueColor):
if hullsMeanDistanceFromCenter[i] > hullsMeanDistanceFromCenter[maxHullIndex]:
maxHullIndex = i
###########################################################################################
# POSE DETECTION
###########################################################################################
# Calculate ROI of selected tool using it's convex hull
min_x = cols
min_y = rows
max_x = max_y = 0
for point in hull[maxHullIndex]:
if point[0][0] < min_x:
min_x = point[0][0]
if point[0][1] < min_y:
min_y = point[0][1]
if point[0][0] > max_x:
max_x = point[0][0]
if point[0][1] > max_y:
max_y = point[0][1]
toolROI = np.array([
[min_x, min_y],
[max_x, min_y],
[max_x, max_y],
[min_x, max_y]
])
# Get pixels inside the selected contour
# CAUTION: This is very heavy computanionally and drops FPS by half or more!
# Could be avoided with a more clever handling
toolPixels = []
# for i in range(detectionAreaYRange[0], detectionAreaYRange[1], 3):
for i in range(min_y, max_y, 10):
for j in range(min_x, max_x, 10):
# for j in range(detectionAreaXRange[0], detectionAreaXRange[1], 10):
point = (j,i)
if cv2.pointPolygonTest(contours[maxHullIndex], point, False) != -1:
numBluePixels += 1
cv_image[i,j] = [0,255,0]
toolPixels.append(np.array([[i,j]]))
elif cv_image[i,j,1] >= 240:
numGreenPixels += 1
toolPixels = np.array(toolPixels)
# Center of mass of tool (weighted average of cm computed from contour and cm computed from toolPixels)
toolPixelsCM = geometry.centerOfMass(toolPixels) # toolPixelsCM coordinates are (Y,X) (??)
maxHullCmX = (contourCenterOfMass[maxHullIndex][0] + 5*toolPixelsCM[1]) / 6
maxHullCmY = (contourCenterOfMass[maxHullIndex][1] + 5*toolPixelsCM[0]) / 6
# Find orientation vectors of tool
a,b = geometry.orientationVectors(toolPixels)
# attach vectors to center of mass point
a = a + [maxHullCmX, maxHullCmY]
b = b + [maxHullCmX, maxHullCmY]
# Update toolCenterOfMass point variable (and orientation) only if there is a significant change in (x,y) values,
# in order to make the point and orientation stable at all times
cmChangeThreshold = 2
if (abs(self.toolCenterOfMass[0] - maxHullCmX) > cmChangeThreshold) and (abs(self.toolCenterOfMass[1] - maxHullCmY) > cmChangeThreshold):
self.toolCenterOfMass[0] = maxHullCmX
self.toolCenterOfMass[1] = maxHullCmY
self.toolOrientX = a
self.toolOrientY = b
###########################################################################################
# VISUAL SERVOING COMMAND
###########################################################################################
# The origin is the desired position for our visual servo controller
originY = int(rows/2)
originX = int(cols/2)
# Actual position of center of mass
cmX = self.toolCenterOfMass[0]
cmY = self.toolCenterOfMass[1]
if not disable_servo and blueToolDetected:
ex = round(0.1*(cmX - originX)/float(cols), 4)
ey = -round(0.1*(cmY - originY)/float(rows), 4)
eth = round(math.atan(a[0]/a[1]), 4)/math.pi
error_norm = math.sqrt(ex**2 + ey**2)
# Filter out sudden spikes in error which occur from sudden temporary detection of another tool
if error_norm > 2*self.prev_error_norm and self.prev_error_norm > 0.0:
ex = self.prev_ex
ey = self.prev_ey
error_norm = self.prev_error_norm
twist = Twist()
# Only send the command if one of the errors is bigger than the allowed tolerance, so that
# we avoid small oscillations in the target position
if error_norm > self.error_tolerance or eth > self.error_tolerance:
twist.linear.x = self.Kp*ex + self.Kd*(ex - self.prev_ex)
twist.linear.y = self.Kp*ey + self.Kd*(ey - self.prev_ey)
# twist.angular.z = eth
self.servo_cmd.publish(twist)
self.error_pub.publish(error_norm)
self.error_theta_pub.publish(eth)
self.prev_ex = ex
self.ex_sum += ex
self.prev_ey = ey
self.ey_sum += ey
self.prev_error_norm = math.sqrt(self.prev_ex**2 + self.prev_ey**2)
###########################################################################################
# FIND GRASP POINTS - FORCE CLOSURE
###########################################################################################
# create an image filled with zeros, single-channel, same size as img_detection_region.
blank = np.zeros(img_detection_region.shape[0:2])
cv2.circle(img_detection_region, (cmX,cmY), 100, 1, 1)
circle_img = cv2.circle(blank.copy(), (cmX,cmY), 100, 1, 1)
contour_img = cv2.drawContours(blank.copy(), contours, maxHullIndex, 1, 1, 8)
intersectedImg = circle_img + contour_img
intersectionPoints = np.argwhere(intersectedImg == np.max(intersectedImg))
reducedIntersectionPoints = [intersectionPoints[0]]
for p in range(1, len(intersectionPoints)):
pt1 = intersectionPoints[p]
canAddPoint = True
for pt2 in reducedIntersectionPoints:
# Manhattan Distance
if abs(pt1[0] - pt2[0]) + abs(pt1[1] - pt2[1]) < 10:
canAddPoint = False
break
if canAddPoint:
reducedIntersectionPoints.append(pt1)
if len(reducedIntersectionPoints) > 3:
reducedIntersectionPoints = reducedIntersectionPoints[0:3]
# print("reducedIntersectionPoints", reducedIntersectionPoints)
###########################################################################################
# DRAW OPENCV IMAGE
###########################################################################################
if not disable_servo:
# Draw target at the center (origin) of the image
cv2.circle(img_detection_region,(originX,originY),10,(255,0,255),-1)
# draw contour and hull points of the biggest convex hull,
# Draw if there was a blue tool
contour_color = (0, 0, 255)
convex_hull_color = (0, 255, 0)
# for i in range(len(contours)):
# cv2.drawContours(img_detection_region, contours, i, contour_color, 2, 8)
if len(hull) > 1 and blueToolDetected: # if there is only one hull, it means this is the while picture (default convex hull)
# draw max convex hull object
cv2.drawContours(img_detection_region, hull, maxHullIndex, convex_hull_color, 2, 8)
# draw center of mass of convex hull
cv2.circle(img_detection_region,(cmX,cmY),5,(0,0,255),-1)
# draw orientation vectors
a = self.toolOrientX
b = self.toolOrientY
cv2.arrowedLine(img_detection_region, (cmX,cmY), (a[0], a[1]), redColor, 2, 8, 0, 0.1)
cv2.arrowedLine(img_detection_region, (cmX,cmY), (b[0], b[1]), greenColor, 2, 8, 0, 0.1)
if not disable_servo:
# Draw arrow from detected tool center of mass to center of the image
# This arrow will also be used as the visual servoing command
cv2.arrowedLine(img_detection_region, (cmX,cmY), (originX, originY), (0,255,255), 2, 8, 0, 0.1)
if trocarDetected:
for i in range(1, len(contoursGreen)):
# draw ith contour
cv2.drawContours(img_detection_region, contoursGreen, i, contour_color, 2, 8, hierarchy)
# Draw Color Region Of Interest
# cv2.polylines(cv_image, [colorROI], True, (0,255,0), thickness=3)
# Draw Tool Region Of Interest
cv2.polylines(cv_image, [toolROI], True, (255,255,255), thickness=2)
# Draw grasp points and their triangle
for point in reducedIntersectionPoints:
cv2.circle(img_detection_region,(point[1],point[0]),7,(10,255,255),-1)
grasp_triangle = np.flip(np.array(reducedIntersectionPoints)).reshape((-1,1,2))
cv2.polylines(cv_image, [grasp_triangle], True, (0,255,255))
# Draw tool detection message
font = cv2.FONT_HERSHEY_SIMPLEX
topLeftCorner = (50,50)
fontScale = 0.5
lineType = 1
cv2.putText(
cv_image,
toolDetectionMsg,
topLeftCorner,
font,
fontScale,
blueColor,
lineType
)
# Draw trocar detection message
position = (50,70)
cv2.putText(
cv_image,
trocarDetectionMsg,
position,
font,
fontScale,
greenColor,
lineType
)
# Calculate and display FPS
endOfFrameTime = time.time()
seconds = endOfFrameTime - self.prevFrameTime
self.prevFrameTime = endOfFrameTime
fps = round(1/seconds, 2)
fpsInfo = "FPS = {}".format(fps)
position = (cols-200,50)
cv2.putText(
cv_image,
fpsInfo,
position,
font,
fontScale,
blueColor,
lineType
)
cv2.imshow("OpenCV Image", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def draw_edge_result(img, result, edge_thresh=0.5, keynode_thresh=0.5):
"""Draw text and their relationship on empty images.
Args:
img (np.ndarray): The original image.
result (dict): The result of model forward_test, including:
- img_metas (list[dict]): List of meta information dictionary.
- nodes (Tensor): Node prediction with size:
number_node * node_classes.
- edges (Tensor): Edge prediction with size: number_edge * 2.
edge_thresh (float): Score threshold for edge classification.
keynode_thresh (float): Score threshold for node
(``key``) classification.
Returns:
np.ndarray: The image with key, value and relation drawn on it.
"""
h, w = img.shape[:2]
vis_area_width = w // 3 * 2
vis_area_height = h
dist_key_to_value = vis_area_width // 2
dist_pair_to_pair = 30
bbox_x1 = dist_pair_to_pair
bbox_y1 = 0
new_w = vis_area_width
new_h = vis_area_height
pred_edge_img = np.ones((new_h, new_w, 3), dtype=np.uint8) * 255
nodes = result['nodes'].detach().cpu()
texts = result['img_metas'][0]['ori_texts']
num_nodes = result['nodes'].size(0)
edges = result['edges'].detach().cpu()[:, -1].view(num_nodes, num_nodes)
# (i, j) will be a valid pair
# either edge_score(node_i->node_j) > edge_thresh
# or edge_score(node_j->node_i) > edge_thresh
pairs = (torch.max(edges, edges.T) > edge_thresh).nonzero(as_tuple=True)
pairs = (pairs[0].numpy().tolist(), pairs[1].numpy().tolist())
# 1. "for n1, n2 in zip(*pairs) if n1 < n2":
# Only (n1, n2) will be included if n1 < n2 but not (n2, n1), to
# avoid duplication.
# 2. "(n1, n2) if nodes[n1, 1] > nodes[n1, 2]":
# nodes[n1, 1] is the score that this node is predicted as key,
# nodes[n1, 2] is the score that this node is predicted as value.
# If nodes[n1, 1] > nodes[n1, 2], n1 will be the index of key,
# so that n2 will be the index of value.
result_pairs = [(n1, n2) if nodes[n1, 1] > nodes[n1, 2] else (n2, n1)
for n1, n2 in zip(*pairs) if n1 < n2]
result_pairs.sort()
result_pairs_score = [
torch.max(edges[n1, n2], edges[n2, n1]) for n1, n2 in result_pairs
]
key_current_idx = -1
pos_current = (-1, -1)
newline_flag = False
key_font_size = 15
value_font_size = 15
key_font_color = (0, 0, 0)
value_font_color = (0, 0, 255)
arrow_color = (0, 0, 255)
score_color = (0, 255, 0)
for pair, pair_score in zip(result_pairs, result_pairs_score):
key_idx = pair[0]
if nodes[key_idx, 1] < keynode_thresh:
continue
if key_idx != key_current_idx:
# move y-coords down for a new key
bbox_y1 += 10
# enlarge blank area to show key-value info
if newline_flag:
bbox_x1 += vis_area_width
tmp_img = np.ones(
(new_h, new_w + vis_area_width, 3), dtype=np.uint8) * 255
tmp_img[:new_h, :new_w] = pred_edge_img
pred_edge_img = tmp_img
new_w += vis_area_width
newline_flag = False
bbox_y1 = 10
key_text = texts[key_idx]
key_pos = (bbox_x1, bbox_y1)
value_idx = pair[1]
value_text = texts[value_idx]
value_pos = (bbox_x1 + dist_key_to_value, bbox_y1)
if key_idx != key_current_idx:
# draw text for a new key
key_current_idx = key_idx
pred_edge_img, text_sizes = draw_texts_by_pil(
pred_edge_img, [key_text],
draw_box=False,
on_ori_img=True,
font_size=key_font_size,
fill_color=key_font_color,
draw_pos=[key_pos],
return_text_size=True)
pos_right_bottom = (key_pos[0] + text_sizes[0][0],
key_pos[1] + text_sizes[0][1])
pos_current = (pos_right_bottom[0] + 5, bbox_y1 + 10)
pred_edge_img = cv2.arrowedLine(
pred_edge_img, (pos_right_bottom[0] + 5, bbox_y1 + 10),
(bbox_x1 + dist_key_to_value - 5, bbox_y1 + 10), arrow_color,
1)
score_pos_x = int(
(pos_right_bottom[0] + bbox_x1 + dist_key_to_value) / 2.)
score_pos_y = bbox_y1 + 10 - int(key_font_size * 0.3)
else:
# draw arrow from key to value
if newline_flag:
tmp_img = np.ones((new_h + dist_pair_to_pair, new_w, 3),
dtype=np.uint8) * 255
tmp_img[:new_h, :new_w] = pred_edge_img
pred_edge_img = tmp_img
new_h += dist_pair_to_pair
pred_edge_img = cv2.arrowedLine(
pred_edge_img, pos_current,
(bbox_x1 + dist_key_to_value - 5, bbox_y1 + 10), arrow_color,
1)
score_pos_x = int(
(pos_current[0] + bbox_x1 + dist_key_to_value - 5) / 2.)
score_pos_y = int((pos_current[1] + bbox_y1 + 10) / 2.)
# draw edge score
cv2.putText(pred_edge_img, '{:.2f}'.format(pair_score),
(score_pos_x, score_pos_y), cv2.FONT_HERSHEY_COMPLEX, 0.4,
score_color)
# draw text for value
pred_edge_img = draw_texts_by_pil(pred_edge_img, [value_text],
draw_box=False,
on_ori_img=True,
font_size=value_font_size,
fill_color=value_font_color,
draw_pos=[value_pos],
return_text_size=False)
bbox_y1 += dist_pair_to_pair
if bbox_y1 + dist_pair_to_pair >= new_h:
newline_flag = True
return pred_edge_img
def interactive_mode(self):
cv2.namedWindow("Build", cv2.WINDOW_NORMAL)
cv2.resizeWindow("Build", self.image_size, self.image_size)
pi2 = np.pi * 2
img = self.prep_image()
pos_start, pos_cen, pos_arrow = self.necessary_data()
for frame, (x_start,y_start),arr_cen, arr_pos in zip(self.frame_data['o'][self.start_particle:self.number_test],\
pos_start[self.start_particle:self.number_test],\
pos_cen[self.start_particle:self.number_test],\
pos_arrow[self.start_particle:self.number_test]):
temp_img = img[x_start:x_start + self.training_shape,
y_start:y_start + self.training_shape]
resize_img = resize(temp_img, (self.training_shape*self.upscale_size, self.training_shape*self.upscale_size),\
mode = 'reflect', anti_aliasing = True)
if np.isnan(arr_pos[0]):
cv2.imshow("Build", resize_img)
else:
arrow_plot = cv2.arrowedLine(resize_img,
(arr_cen[1], arr_cen[0]),
(arr_pos[1], arr_pos[0]),
(255, 0, 0), 2)
cv2.imshow('Build', resize_img)
print("detect or is: {}".format(frame))
press = cv2.waitKey(0)
# everything is good, store current information into training sets
if press == 49:
self.training_dict["training_x"].append(temp_img)
self.training_dict["training_regress"].append(frame)
self.training_dict["training_class"].append(1)
# enter plot mode
elif press == 50:
points_store = []
button_down = False
cv2.imshow("Build", resize_img)
def orientation_click(event, x, y, flags, param):
global button_down
if event == cv2.EVENT_LBUTTONUP and button_down:
button_down = False
points_store.append((x, y))
cv2.arrowedLine(resize_img, points_store[0],
points_store[1], (255, 0, 0), 1)
cv2.imshow("Build", resize_img)
elif event == cv2.EVENT_MOUSEMOVE and button_down:
button_down = True
image = resize_img.copy()
cv2.arrowedLine(image, points_store[0], (x, y),
(255, 0, 0), 1)
cv2.imshow("Build", image)
elif event == cv2.EVENT_LBUTTONDOWN and len(
points_store) < 2:
button_down = True
points_store.insert(0, (x, y))
cv2.setMouseCallback('Build', orientation_click, points_store)
enter = cv2.waitKey(0)
orientation = np.arctan2(
points_store[1][0] - points_store[0][0],
points_store[1][1] - points_store[0][1])
orientation = (orientation + pi2) % pi2
print("Click orientation is: {}".format(orientation))
if enter == 49:
self.training_dict["training_x"].append(temp_img)
self.training_dict["training_regress"].append(orientation)
self.training_dict["training_class"].append(1)
# wrong detected particles goes to mode 3, no orientation detected give it class 1
elif press == 51:
self.training_dict["training_x"].append(temp_img)
self.training_dict["training_regress"].append(np.nan)
self.training_dict["training_class"].append(0)
elif press == 27:
break
cv2.destroyAllWindows()
for i in range(1, 5):
cv2.waitKey(1)
return
if last_center_point is None:
last_center_point = center_point
# find vector direction
v1 = np.subtract(center_point, last_center_point)
v1 = tuple(v1 * VECTOR_SIZE_NORMALIZE)
currentMouse.update_history(v1)
super = currentMouse.get_super_avg()
super_end_point = tuple(np.add(center_point, super))
vector_end_point = tuple(np.add(center_point, tuple(v1)))
if np.linalg.norm(v1) > SHORT_MOVEMENT_THRESHOLD:
cv2.arrowedLine(im_with_keypoints, center_point, vector_end_point, COLOR_GREEN)
if np.linalg.norm(super) > GENERAL_MOVEMENT_THRESHOLD:
cv2.arrowedLine(im_with_keypoints, center_point, super_end_point, COLOR_ORANGE, 2)
last_center_point = center_point
res = angle_between(v1, super)
if not np.isnan(res) and (np.linalg.norm(v1) > SHORT_MOVEMENT_THRESHOLD and np.linalg.norm(super) > GENERAL_MOVEMENT_THRESHOLD):
currentMouse.movement_sum += np.linalg.norm(v1)
assert angle_between(v1, super) >= 0
#currentMouse.angle_sum += angle_between(v1,super)
#currentMouse.avg_angle = currentMouse.angle_sum/currentMouse.movement_sum
currentMouse.data['x'].append(currentTime/(1000*60))
def main():
model.eval()
ttime_all = []
for inx in range(len(test_left_img)):
idxname = test_left_img[inx].split('/')[-1].split('.')[0]
print(test_left_img[inx])
imgL_o = cv2.imread(test_left_img[inx])[:, :, ::-1]
imgR_o = cv2.imread(test_right_img[inx])[:, :, ::-1]
# for gray input images
if len(imgL_o.shape) == 2:
imgL_o = np.tile(imgL_o[:, :, np.newaxis], (1, 1, 3))
imgR_o = np.tile(imgR_o[:, :, np.newaxis], (1, 1, 3))
# resize
maxh = imgL_o.shape[0] * args.testres
maxw = imgL_o.shape[1] * args.testres
max_h = int(maxh // 64 * 64)
max_w = int(maxw // 64 * 64)
if max_h < maxh: max_h += 64
if max_w < maxw: max_w += 64
input_size = imgL_o.shape
imgL = cv2.resize(imgL_o, (max_w, max_h))
imgR = cv2.resize(imgR_o, (max_w, max_h))
imgL_noaug = torch.Tensor(imgL / 255.)[np.newaxis].float().cuda()
# flip channel, subtract mean
imgL = imgL[:, :, ::-1].copy() / 255. - np.asarray(mean_L).mean(0)[
np.newaxis, np.newaxis, :]
imgR = imgR[:, :, ::-1].copy() / 255. - np.asarray(mean_R).mean(0)[
np.newaxis, np.newaxis, :]
imgL = np.transpose(imgL, [2, 0, 1])[np.newaxis]
imgR = np.transpose(imgR, [2, 0, 1])[np.newaxis]
# modify module according to inputs
from models.VCNplus import WarpModule, flow_reg
for i in range(len(model.module.reg_modules)):
model.module.reg_modules[i] = flow_reg([1,max_w//(2**(6-i)), max_h//(2**(6-i))],
ent=getattr(model.module, 'flow_reg%d'%2**(6-i)).ent,\
maxdisp=getattr(model.module, 'flow_reg%d'%2**(6-i)).md,\
fac=getattr(model.module, 'flow_reg%d'%2**(6-i)).fac).cuda()
for i in range(len(model.module.warp_modules)):
model.module.warp_modules[i] = WarpModule(
[1, max_w // (2**(6 - i)), max_h // (2**(6 - i))]).cuda()
# get intrinsics
if '2015' in args.dataset:
from utils.util_flow import load_calib_cam_to_cam
ints = load_calib_cam_to_cam(test_left_img[inx].replace(
'image_2', 'calib_cam_to_cam')[:-7] + '.txt')
K0 = ints['K_cam2']
K1 = K0
fl = K0[0, 0]
cx = K0[0, 2]
cy = K0[1, 2]
bl = ints['b20'] - ints['b30']
fl_next = fl
intr_list = [
torch.Tensor(inxx).cuda()
for inxx in [[fl], [cx], [cy], [bl], [1], [0], [0], [1], [0],
[0]]
]
elif 'sintel' in args.dataset and not 'test' in test_left_img[inx]:
from utils.sintel_io import cam_read
passname = test_left_img[inx].split('/')[-1].split('_')[-4]
seqname1 = test_left_img[inx].split('/')[-1].split('_')[-3]
seqname2 = test_left_img[inx].split('/')[-1].split('_')[-2]
framename = int(
test_left_img[inx].split('/')[-1].split('_')[-1].split('.')[0])
#TODO add second camera
K0, _ = cam_read(
'/data/gengshay/tf_depth/sintel-data/training/camdata_left/%s_%s/frame_%04d.cam'
% (seqname1, seqname2, framename + 1))
K1, _ = cam_read(
'/data/gengshay/tf_depth/sintel-data/training/camdata_left/%s_%s/frame_%04d.cam'
% (seqname1, seqname2, framename + 2))
fl = K0[0, 0]
cx = K0[0, 2]
cy = K0[1, 2]
fl_next = K1[0, 0]
bl = 0.1
intr_list = [
torch.Tensor(inxx).cuda()
for inxx in [[fl], [cx], [cy], [bl], [1], [0], [0], [1], [0],
[0]]
]
elif 'seq' in args.dataset:
fl, cx, cy = seqcalib[inx]
bl = 1
fl_next = fl
K0 = np.eye(3)
K0[0, 0] = fl
K0[1, 1] = fl
K0[0, 2] = cx
K0[1, 2] = cy
K1 = K0
intr_list = [
torch.Tensor(inxx).cuda()
for inxx in [[fl], [cx], [cy], [bl], [1], [0], [0], [1], [0],
[0]]
]
else:
print('NOT using given intrinsics')
fl = min(input_size[0], input_size[1]) * 2
fl_next = fl
cx = input_size[1] / 2.
cy = input_size[0] / 2.
bl = 1
K0 = np.eye(3)
K0[0, 0] = fl
K0[1, 1] = fl
K0[0, 2] = cx
K0[1, 2] = cy
K1 = K0
intr_list = [
torch.Tensor(inxx).cuda()
for inxx in [[fl], [cx], [cy], [bl], [1], [0], [0], [1], [0],
[0]]
]
intr_list.append(torch.Tensor([input_size[1] / max_w
]).cuda()) # delta fx
intr_list.append(torch.Tensor([input_size[0] / max_h
]).cuda()) # delta fy
intr_list.append(torch.Tensor([fl_next]).cuda())
disc_aux = [None, None, None, intr_list, imgL_noaug, None]
if args.disp_path == '': disp_input = None
else:
try:
disp_input = disparity_loader('%s/%s_disp.pfm' %
(args.disp_path, idxname))
except:
disp_input = disparity_loader('%s/%s.png' %
(args.disp_path, idxname))
disp_input = torch.Tensor(disp_input.copy())[np.newaxis,
np.newaxis].cuda()
# forward
imgL = Variable(torch.FloatTensor(imgL).cuda())
imgR = Variable(torch.FloatTensor(imgR).cuda())
with torch.no_grad():
imgLR = torch.cat([imgL, imgR], 0)
model.eval()
torch.cuda.synchronize()
start_time = time.time()
rts = model(imgLR, disc_aux, disp_input)
torch.cuda.synchronize()
ttime = (time.time() - start_time)
print('time = %.2f' % (ttime * 1000))
ttime_all.append(ttime)
flow, occ, logmid, logexp, fgmask, heatmap, polarmask, disp = rts
bbox = polarmask['bbox']
polarmask = polarmask['mask']
polarcontour = polarmask[:polarmask.shape[0] // 2]
polarmask = polarmask[polarmask.shape[0] // 2:]
# upsampling
occ = cv2.resize(occ.data.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
logexp = cv2.resize(logexp.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
logmid = cv2.resize(logmid.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
fgmask = cv2.resize(fgmask.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
heatmap = cv2.resize(heatmap.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
polarcontour = cv2.resize(polarcontour, (input_size[1], input_size[0]),
interpolation=cv2.INTER_NEAREST)
polarmask = cv2.resize(polarmask, (input_size[1], input_size[0]),
interpolation=cv2.INTER_NEAREST).astype(int)
polarmask[np.logical_and(fgmask > 0, polarmask == 0)] = -1
if args.disp_path == '':
disp = cv2.resize(disp.cpu().numpy(),
(input_size[1], input_size[0]),
interpolation=cv2.INTER_LINEAR)
else:
disp = np.asarray(disp_input.cpu())[0, 0]
flow = torch.squeeze(flow).data.cpu().numpy()
flow = np.concatenate([
cv2.resize(flow[0],
(input_size[1], input_size[0]))[:, :, np.newaxis],
cv2.resize(flow[1],
(input_size[1], input_size[0]))[:, :, np.newaxis]
], -1)
flow[:, :, 0] *= imgL_o.shape[1] / max_w
flow[:, :, 1] *= imgL_o.shape[0] / max_h
flow = np.concatenate(
(flow, np.ones([flow.shape[0], flow.shape[1], 1])), -1)
bbox[:, 0] *= imgL_o.shape[1] / max_w
bbox[:, 2] *= imgL_o.shape[1] / max_w
bbox[:, 1] *= imgL_o.shape[0] / max_h
bbox[:, 3] *= imgL_o.shape[0] / max_h
# draw instance center and motion in 2D
ins_center_vis = np.zeros(flow.shape[:2])
for k in range(bbox.shape[0]):
from utils.detlib import draw_umich_gaussian
draw_umich_gaussian(ins_center_vis,
bbox[k, :4].reshape(2, 2).mean(0), 15)
ins_center_vis = 256 * np.stack([
ins_center_vis,
np.zeros(ins_center_vis.shape),
np.zeros(ins_center_vis.shape)
], -1)
if args.refine:
## depth and scene flow estimation
# save initial disp and flow
init_disp = disp.copy()
init_flow = flow.copy()
init_logmid = logmid.copy()
if args.mask_path == '':
mask_input = polarmask
else:
mask_input = cv2.imread(
'%s/%s.png' % (args.mask_path, idxname), 0)
if mask_input is None:
mask_input = cv2.imread(
'%s/%s.png' % (args.mask_path, idxname.split('_')[0]),
0)
bgmask = (mask_input == 0)
scene_type, T01_c, R01, RTs = ddlib.rb_fitting(
bgmask,
mask_input,
disp,
flow,
occ,
K0,
K1,
bl,
parallax_th=4,
mono=(args.sensor == 'mono'),
sintel='Sintel' in idxname)
print('camera trans: ')
print(T01_c)
disp, flow, disp1 = ddlib.mod_flow(bgmask,
mask_input,
disp,
disp / np.exp(logmid),
flow,
occ,
bl,
K0,
K1,
scene_type,
T01_c,
R01,
RTs,
fgmask,
mono=(args.sensor == 'mono'),
sintel='Sintel' in idxname)
logmid = np.clip(np.log(disp / disp1), -1, 1)
# draw ego vehicle
ct = [4 * input_size[0] // 5, input_size[1] // 2][::-1]
cv2.circle(ins_center_vis,
tuple(ct),
radius=10,
color=(0, 255, 255),
thickness=10)
obj_3d = K0[0, 0] * bl / np.median(
disp[bgmask]) * np.linalg.inv(K0).dot(
np.hstack([ct, np.ones(1)]))
obj_3d2 = obj_3d + (-R01.T.dot(T01_c))
ed = K0.dot(obj_3d2)
ed = (ed[:2] / ed[-1]).astype(int)
if args.sensor == 'mono':
direct = (ed - ct)
direct = 50 * direct / (1e-9 + np.linalg.norm(direct))
else:
direct = (ed - ct)
ed = (ct + direct).astype(int)
if np.linalg.norm(direct) > 1:
ins_center_vis = cv2.arrowedLine(
ins_center_vis,
tuple(ct),
tuple(ed), (0, 255, 255),
6,
tipLength=float(30. / np.linalg.norm(direct)))
# draw each object
for k in range(mask_input.max()):
try:
obj_mask = mask_input == k + 1
if obj_mask.sum() == 0: continue
ct = np.asarray(
np.nonzero(obj_mask)).mean(1).astype(int)[::-1] # Nx2
cv2.circle(ins_center_vis,
tuple(ct),
radius=5,
color=(255, 0, 0),
thickness=5)
if RTs[k] is not None:
#ins_center_vis[mask_input==k+1] = imgL_o[mask_input==k+1]
obj_3d = K0[0, 0] * bl / np.median(
disp[mask_input == k + 1]) * np.linalg.inv(K0).dot(
np.hstack([ct, np.ones(1)]))
obj_3d2 = obj_3d + (-RTs[k][0].T.dot(RTs[k][1]))
ed = K0.dot(obj_3d2)
ed = (ed[:2] / ed[-1]).astype(int)
if args.sensor == 'mono':
direct = (ed - ct)
direct = 50 * direct / (np.linalg.norm(direct) +
1e-9)
else:
direct = (ed - ct)
ed = (ct + direct).astype(int)
if np.linalg.norm(direct) > 1:
ins_center_vis = cv2.arrowedLine(
ins_center_vis,
tuple(ct),
tuple(ed), (255, 0, 0),
3,
tipLength=float(30. / np.linalg.norm(direct)))
except:
pdb.set_trace()
cv2.imwrite('%s/%s/mvis-%s.jpg' % (args.outdir, args.dataset, idxname),
ins_center_vis[:, :, ::-1])
# save predictions
with open('%s/%s/flo-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, flow[::-1].astype(np.float32))
flowvis = point_vec(imgL_o, flow)
cv2.imwrite(
'%s/%s/visflo-%s.jpg' % (args.outdir, args.dataset, idxname),
flowvis)
imwarped = ddlib.warp_flow(imgR_o, flow[:, :, :2])
cv2.imwrite('%s/%s/warp-%s.jpg' % (args.outdir, args.dataset, idxname),
imwarped[:, :, ::-1])
cv2.imwrite(
'%s/%s/warpt-%s.jpg' % (args.outdir, args.dataset, idxname),
imgL_o[:, :, ::-1])
cv2.imwrite(
'%s/%s/warps-%s.jpg' % (args.outdir, args.dataset, idxname),
imgR_o[:, :, ::-1])
with open('%s/%s/occ-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, occ[::-1].astype(np.float32))
with open('%s/%s/exp-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, logexp[::-1].astype(np.float32))
with open('%s/%s/mid-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, logmid[::-1].astype(np.float32))
with open('%s/%s/fg-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, fgmask[::-1].astype(np.float32))
with open('%s/%s/hm-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, heatmap[::-1].astype(np.float32))
with open('%s/%s/pm-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, polarmask[::-1].astype(np.float32))
ddlib.write_calib(
K0, bl, polarmask.shape, K0[0, 0] * bl / (np.median(disp) / 5),
'%s/%s/calib-%s.txt' % (args.outdir, args.dataset, idxname))
# submit to KITTI benchmark
if 'test' in args.dataset:
outdir = 'benchmark_output'
# kitti scene flow
import skimage.io
skimage.io.imsave('%s/disp_0/%s.png' % (outdir, idxname),
(disp * 256).astype('uint16'))
skimage.io.imsave('%s/disp_1/%s.png' % (outdir, idxname),
(disp1 * 256).astype('uint16'))
flow[:, :, 2] = 1.
write_flow('%s/flow/%s.png' % (outdir, idxname.split('.')[0]),
flow)
# save visualizations
with open('%s/%s/disp-%s.pfm' % (args.outdir, args.dataset, idxname),
'w') as f:
save_pfm(f, disp[::-1].astype(np.float32))
try:
# point clouds
from utils.fusion import pcwrite
hp2d0 = np.concatenate(
[
np.tile(
np.arange(0, input_size[1]).reshape(1, -1),
(input_size[0], 1)).astype(float)[None], # 1,2,H,W
np.tile(
np.arange(0, input_size[0]).reshape(-1, 1),
(1, input_size[1])).astype(float)[None],
np.ones(input_size[:2])[None]
],
0).reshape(3, -1)
hp2d1 = hp2d0.copy()
hp2d1[:2] += np.transpose(flow, [2, 0, 1])[:2].reshape(2, -1)
p3d0 = (K0[0, 0] * bl /
disp.flatten()) * np.linalg.inv(K0).dot(hp2d0)
p3d1 = (K0[0, 0] * bl /
disp1.flatten()) * np.linalg.inv(K1).dot(hp2d1)
def write_pcs(points3d, imgL_o, mask_input, path):
# remove some points
points3d = points3d.T.reshape(input_size[:2] + (3, ))
points3d[points3d[:, :,
-1] > np.median(points3d[:, :, -1]) * 5] = 0
#points3d[:2*input_size[0]//5] = 0. # KITTI
points3d = np.concatenate([points3d, imgL_o], -1)
validid = np.linalg.norm(points3d[:, :, :3], 2, -1) > 0
bgidx = np.logical_and(validid, mask_input == 0)
fgidx = np.logical_and(validid, mask_input > 0)
pcwrite(path.replace('/pc', '/fgpc'), points3d[fgidx])
pcwrite(path.replace('/pc', '/bgpc'), points3d[bgidx])
pcwrite(path, points3d[validid])
if inx == 0:
write_pcs(p3d0,
imgL_o,
mask_input,
path='%s/%s/pc0-%s.ply' %
(args.outdir, args.dataset, idxname))
write_pcs(p3d1,
imgL_o,
mask_input,
path='%s/%s/pc1-%s.ply' %
(args.outdir, args.dataset, idxname))
except:
pass
torch.cuda.empty_cache()
print(np.mean(ttime_all))
#Posibles casos:
# 'A' = Nuevo sector
# 'B' = Disco no se ha movido de sector
# 'C' = Disco fuera
if((drawSectors==True) and (caso is not 'C')):
colored_sector(frame, pos_D1)
if(caso is 'A'):
if(pos_D1[0]<=pos_D0[0]): #actual_sector[x] <= old_sector[x]
disc_advances = True
#Si quisiésemos pintar una flecha por dónde se mueve el disco:
pt1 = calcRobotPos_in_mm(pos_D0)
pt2 = calcRobotPos_in_mm(pos_D1)
cv2.arrowedLine(frame, pt1, pt2, (0, 100, 255), thickness=3)
elif(caso is 'C'):
pos_D0 = (-2,-2)
disc_Out = True
#--ROBOT SECTOR POSITION CONTROL ------------------------------------------------
if (disc_advances) and (center_ROBOT is not None):
caso, pos_R = update_actual_sector_position(
center_ROBOT[0],center_ROBOT[1],pos_R)
S_1 = calculoEstado(pos_D0, pos_D1, pos_R)
def draw_arrow(canvas,cord1,cord2):
cv.arrowedLine(canvas,cord1,cord2,(0,255,0),10)
import numpy as np
import cv2
#img = cv2.imread('hello/lena.jpg', 1)
img= np.zeros([600,600,3], np.uint8)
img= cv2.rectangle(img, (0,0), (510,510), (0,0,255), -1)
img= cv2.line(img, (0,0), (510,255), (0,255,255), 10,)
img= cv2.arrowedLine(img, (0,0), (255,510), (255,255,0), 2)
img= cv2.rectangle(img, (255,255), (510,510), (0,255,0), 5)
img= cv2.circle(img, (255,255), 150, (150,0,255), 10)
font= cv2.FONT_HERSHEY_SCRIPT_COMPLEX
img= cv2.putText(img, "Sample", (150,255), font, 3, (255,0,0), 5, cv2.LINE_8)
#img= cv2.ellipse(img, (300,300), )
cv2.imshow('image', img)
if cv2.waitKey(0) and 0xFF == ord('q'):
cv2.destroyAllWindows()
alpha = float(tmp[12])
xmin = int(bbox2D[0])
ymin = int(bbox2D[1])
xmax = int(bbox2D[0]) + int(bbox2D[2])
ymax = int(bbox2D[1]) + int(bbox2D[3])
x = (bbox2D[0] + bbox2D[2]) / 2
cv2.line(image, (xmin, ymin), (xmax, ymin), (0, 255, 0), 2, lineType=cv2.LINE_AA)
cv2.line(image, (xmax, ymin), (xmax, ymax), (0, 255, 0), 2, lineType=cv2.LINE_AA)
cv2.line(image, (xmin, ymax), (xmin, ymin), (0, 255, 0), 2, lineType=cv2.LINE_AA)
cv2.line(image, (xmax, ymax), (xmin, ymax), (0, 255, 0), 2, lineType=cv2.LINE_AA)
ct_x = xmin + bbox2D[2] / 2
ct_y = ymin + bbox2D[3] / 2
cv2.circle(image, (int(ct_x), int(ct_y)), 10, (0, 255, 0), -1)
end_x = ct_x + np.cos(rotation_y)*100
end_y = ct_y - np.sin(rotation_y)*100
cv2.arrowedLine(image, (int(ct_x),int(ct_y)), (int(end_x), int(end_y)), (0,255,0), 2) #초록
#alpha = rot_y2alpha(rotation_y, x, calib[0, 2], calib[0, 0])
alpha_x = ct_x + np.cos(alpha)*100
alpha_y = ct_y - np.sin(alpha)*100
cv2.arrowedLine(image, (int(ct_x), int(ct_y)), (int(alpha_x), int(alpha_y)), (255,0,0), 2) #파랑
# rotation_y = -rotation_y
theta = RotationMatrixToeulerAngles(Rtilt)
box_3d = compute_box_3d_world(dim, location, rotation_y, theta)
# print(cat_id, location, dim, rotation_y, bbox2D, alpha)
# ann에 axis 바꾼거를 집어넣기
sun_permutation = [0, 2, 1]
#19.12.26 추가된 부분
print("Rtilt")
print(Rtilt)
import cv2
import numpy as np
#img = cv2.imread('lena.jpg', 1)
# CREAR UNA IMAGEN VACIA CON NUMPY
img = np.zeros([512, 512, 3], np.uint8)
# EL ARGUMENTO COLOR ESTA ORDENADO POR AZUL VERDE Y ROJO
# PARA VER EL CODGIO DE COLORES PUEDO BUSCAR RGB COLOR PICKER
img = cv2.line(img, (0, 0), (255, 255), (147, 96, 44), 3)
img = cv2.arrowedLine(img, (0, 255), (255, 255), (147, 96, 44), 3)
# EL PONER -1 EN TICKNEES ME RELLENA EL RECTANGUO
# PRIMER PUNTO ES EL VERTICE SUPERIOR
# SEGUNDO PUNTO ES EL VERTICE INFERIOR
img = cv2.rectangle(img, (384, 0), (510, 128), (0, 0, 255), 1)
img = cv2.circle(img, (447, 63), 63, (0, 255, 0), -1)
font = cv2.FONT_HERSHEY_SIMPLEX
# PONER TEXTO EN UNA IMAGEN
img = cv2.putText(img, 'Hola', (10, 500), font, 4, (255, 255, 255), 10, cv2.LINE_AA)
cv2.imshow('image', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
minArea=2000,
filter=4,
cThr=[50, 50],
draw=False)
if len(conts) != 0:
for obj in conts2:
cv2.polylines(imgC, [obj[2]], True, (0, 255, 0), 2)
nPoints = utilis.reorder(obj[2])
nW = round((utilis.findDist(nPoints[0][0] // scale,
nPoints[1][0] // scale) / 10), 1)
nH = round((utilis.findDist(nPoints[0][0] // scale,
nPoints[2][0] // scale) / 10), 1)
cv2.arrowedLine(imgC, (nPoints[0][0][0], nPoints[0][0][1]),
(nPoints[1][0][0], nPoints[1][0][1]),
(255, 0, 255), 3, 8, 0, 0.05)
cv2.arrowedLine(imgC, (nPoints[0][0][0], nPoints[0][0][1]),
(nPoints[2][0][0], nPoints[2][0][1]),
(255, 0, 255), 3, 8, 0, 0.05)
x, y, w, h = obj[3]
cv2.putText(imgC, '{}cm'.format(nW), (x + 30, y - 10),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1.5, (255, 0, 255),
2)
cv2.putText(imgC, '{}cm'.format(nH), (x - 70, y + h // 2),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1.5, (255, 0, 255),
2)
cv2.imshow("cont in cont", imgC)
if cv2.waitKey(20) & 0xFF == ord('q'):
def find_train(self, mat, color_splits):
lab_split = color_splits['lab']
yuv_split = color_splits['yuv']
hls_split = color_splits['hls']
train_yuv_uthreshed = cv2.inRange(yuv_split[1], self.options['train_yuv_u_min'], self.options['train_yuv_u_max'])
train_lab_athreshed = cv2.inRange(lab_split[1], self.options['train_lab_a_min'], self.options['train_lab_a_max'])
train_hls_hthreshed = cv2.inRange(hls_split[0], self.options['train_hls_h_min'], self.options['train_hls_h_max'])
train_threshed = train_yuv_uthreshed & train_lab_athreshed & train_hls_hthreshed
train_eroded = cv2.erode(train_threshed, np.ones((10, 10)))
train_dilated = cv2.dilate(train_eroded, np.ones((100, 100)))
if self.options['debugging']:
self.post('train_lab_athreshed', train_lab_athreshed)
self.post('train_yuv_uthreshed', train_yuv_uthreshed)
self.post('train_hls_hthreshed', train_hls_hthreshed)
self.post('train_threshed', train_threshed)
self.post('train_eroded', train_eroded.copy())
self.post('train_dilated', train_dilated.copy())
_, train_threshed_contours, train_threshed_hierarchy = cv2.findContours(train_eroded, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if train_threshed_hierarchy is None:
print('Cannot find any train contours in the picture')
shm.recovery_results.train_heuristic_score.set(0)
return
_, train_dilated_contours, train_dilated_hierarchy = cv2.findContours(train_dilated, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
train_dilated_contour_areas = map(lambda (i, x): [cv2.contourArea(x), i, cv2.boundingRect(x), x],
enumerate(train_dilated_contours))
train_bounding_contour = max(train_dilated_contour_areas, key=lambda x: x[0])
train_threshed_contours = filter(lambda x: point_in_rect(center(cv2.boundingRect(x)), train_bounding_contour[2]), train_threshed_contours)
if self.options['debugging']:
all_contours_drawing = np.copy(mat)
cv2.drawContours(all_contours_drawing, [c for c in train_threshed_contours if cv2.contourArea(c)], -1,
(255, 255, 0), 2)
cv2.drawContours(all_contours_drawing, [c for c in train_dilated_contours if cv2.contourArea(c)], -1,
(255, 0, 0), 2)
self.post('train_threshed_contours', all_contours_drawing)
if self.options['debugging']:
all_contours_drawing = np.copy(mat)
cv2.drawContours(all_contours_drawing, [c for c in train_threshed_contours if cv2.contourArea(c)], -1,
(255, 255, 0), 2)
cv2.drawContours(all_contours_drawing, [c for c in train_dilated_contours if cv2.contourArea(c)], -1,
(255, 0, 0), 2)
self.post('train_threshed_contours', all_contours_drawing)
train_threshed_contour_areas = map(lambda (i, x): [cv2.contourArea(x), i, cv2.boundingRect(x), x],
enumerate(train_threshed_contours))
train_threshed_contour_areas.sort(key=lambda x: x[0], reverse=True)
train_threshed_contour_areas = train_threshed_contour_areas[:4]
train_threshed_contour_areas_by_x = sorted(train_threshed_contour_areas, key=lambda x: center(x[2])[0])
biggest_block_index_by_x, biggest_block = max(enumerate(train_threshed_contour_areas_by_x), key=lambda x: x[1][0])
biggest_block_rotated_rect = list(cv2.minAreaRect(biggest_block[3]))
if biggest_block_rotated_rect[1][0] > biggest_block_rotated_rect[1][1]:
biggest_block_rotated_rect[1] = biggest_block_rotated_rect[1][1], biggest_block_rotated_rect[1][0]
biggest_block_rotated_rect[2] += 90
train_threshed_contour_areas_by_y = sorted(train_threshed_contour_areas, key=lambda x: center(x[2])[1])
biggest_block_index_by_y, _ = max(enumerate(train_threshed_contour_areas_by_y), key=lambda x: x[1][0])
vertical = False
if len(train_threshed_contour_areas) == 4:
if biggest_block_index_by_x > 1:
biggest_block_rotated_rect[2] += 180
if biggest_block_index_by_y > 1:
vertical = True
elif len(train_threshed_contour_areas) == 3:
if biggest_block_index_by_x == 2:
biggest_block_rotated_rect[2] += 180
elif biggest_block_index_by_x == 1:
if train_threshed_contour_areas_by_x[0][0] < train_threshed_contour_areas_by_x[2][0]:
biggest_block_rotated_rect[2] += 180
if biggest_block_index_by_y == 2:
vertical = True
elif biggest_block_index_by_y == 1:
if train_threshed_contour_areas_by_y[0][0] < train_threshed_contour_areas_by_y[2][0]:
vertical = True
elif len(train_threshed_contour_areas) == 2:
if biggest_block_index_by_x == 1:
biggest_block_rotated_rect[2] += 180
if biggest_block_index_by_y == 1:
vertical = True
biggest_block_rotated_rect[2] %= 360
biggest_block_rotated_rect[2] = 360 - biggest_block_rotated_rect[2]
if 65 < biggest_block_rotated_rect[2] < 115 and not vertical:
biggest_block_rotated_rect[2] += 180
elif 245 < biggest_block_rotated_rect[2] < 294 and vertical:
biggest_block_rotated_rect[2] -= 180
if self.options['debugging']:
arrow_distance = 200
angle = 360 - biggest_block_rotated_rect[2]
arrow_x_dist = int(arrow_distance * cos(radians(angle)))
arrow_y_dist = int(arrow_distance * sin(radians(angle)))
arrow_start = tuple(map(int, biggest_block_rotated_rect[0]))
arrow_end = (arrow_start[0] + arrow_x_dist, arrow_start[1] + arrow_y_dist)
cv2.arrowedLine(all_contours_drawing, arrow_start, arrow_end, (255, 0, 0), 4)
shm.recovery_results.train_center_x.set(int(biggest_block_rotated_rect[0][0]))
shm.recovery_results.train_center_y.set(int(biggest_block_rotated_rect[0][1]))
shm.recovery_results.train_angle.set(biggest_block_rotated_rect[2])
shm.recovery_results.train_heuristic_score.set(train_bounding_contour[0])
if matches_to_use <= MIN_MATCHES:
print("Insufficient point number")
exit()
# apply ransac
src_pts = np.float32([kp1[match.queryIdx].pt for match in matches])
dst_pts = np.float32([kp2[match.trainIdx].pt for match in matches])
F, mask = cv2.findFundamentalMat(src_pts.reshape(-1, 1, 2),
dst_pts.reshape(-1, 1, 2), cv2.RANSAC,
RANSAC_REPROJ, RANSAC_ACCURACY)
match_mask = mask.ravel().tolist()
# draw matches
params = dict(matchesMask=match_mask)
img3 = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, **params)
# filter points
good_src = [src_pts[i] for i in range(len(src_pts)) if match_mask[i] == 1]
good_dst = [dst_pts[i] for i in range(len(dst_pts)) if match_mask[i] == 1]
print("Found {} good matches".format(len(good_src)))
# print arrows
arrows = img1.copy()
for i in range(len(good_src)):
# print(good_src[i])
cv2.arrowedLine(arrows, tuple(good_src[i]), tuple(good_dst[i]),
(0, 100, 255))
cv2.imshow('matches', arrows)
cv2.imshow('origin', img1)
cv2.waitKey()
cv2.destroyAllWindows()
cap.release()
if ok:
# Tracking success
p1 = (int(bbox[0]), int(bbox[1]))
p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
# note: colors are blue, green, red (lmao why)
# rectangle around target (blue)
cv2.rectangle(image, p1, p2, (255, 0, 0), 2, 1)
# from this information we can then derive distance from center and angle of correction using some fancy math.
target_center = (int(p1[0] + (bbox[2] / 2.0)),
int(p1[1] + (bbox[3] / 2.0)))
# target correction arrow (green)
cv2.arrowedLine(image, video_center, target_center, (0, 255, 0), 3)
# target reticule (red)
cv2.circle(image,
target_center,
radius, (0, 0, 255),
thickness=2,
lineType=8,
shift=0)
cv2.putText(image, "Tracking Successful", (100, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
else:
# Tracking failure
cv2.putText(image, "Tracking failure detected", (100, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
def main(yolo):
selectingObject = False #use detect not hand
initTracking = True
onTracking = False
duration = 0.01
pts = deque(maxlen=50)
bx2 = [0, 0, 0, 0]
tracker = KCFTracker(True, True, True)
#KF initialization
P = np.diag([3000.0, 3000.0, 3000.0, 3000.0])
I = np.eye(4)
H = np.matrix([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
ra = 0.15 # 厂商提供
R = np.matrix([[ra, 0.0], [0.0, ra]])
noise_ax = 0.3
noise_ay = 0.3
cv2.namedWindow('tracking')
#cap = cv2.VideoCapture(0)
cap = cv2.VideoCapture('./oc1.mp4')
while True:
ret, frame = cap.read()
if not ret:
break
if (selectingObject):
continue
elif (initTracking):
image = Image.fromarray(frame[..., ::-1]) #bgr to rgb
box, class_names = yolo.detect_image(image) #use yolo to predict
ix0 = int(box[0][0])
iy = int(box[0][1])
w = int(box[0][2])
h = int(box[0][3])
tracker.init([ix0, iy, w, h], frame)
#Lenovo camera
ixx0 = int((box[0][0]) + (box[0][2]) / 2)
iyy0 = int((box[0][1]) + (box[0][3]) / 2)
#GH4 camera
'''
D0 = 2*( 355.2/ w) # meter,qianhou
x_cen = 640
xd0 = ixx0 - x_cen
# geo similarity
X0 = D0 / 887.9 * xd0 # meter,zuoyou
'''
#xiaomi
D0 = (1978.9 / h) # meter,qianhou
x_cen = 703
xd0 = ixx0 - x_cen
# geo similarity
X0 = xd0 * (1.54 / h) # meter,zuoyou
#humw.append(0)
#humw.append(w)
#DJI
'''
D0 = (3006.5/h) # meter,qianhou
x_cen = 1361.8
xd0 = ixx0 - x_cen
# geo similarity
X0 = xd0*(1.75/h) # meter,zuoyou
'''
state = np.matrix([[X0, D0, 0.0, 0.0]]).T
state_2D = np.matrix([[ixx0, iyy0, 0.0, 0.0]]).T
initTracking = False
onTracking = True
elif (onTracking):
t0 = time()
boundingbox = tracker.update(frame)
x = boundingbox[0]
y = boundingbox[1]
w = boundingbox[2]
h = boundingbox[3]
cx = int(x + w / 2)
cy = int(y + h / 2)
boundingbox = list(map(int, boundingbox))
x1 = boundingbox[0]
y1 = boundingbox[1]
w1 = boundingbox[2]
h1 = boundingbox[3]
ix = int((boundingbox[0]) + (boundingbox[2]) / 2)
iy = int((boundingbox[1]) + (boundingbox[3]) / 2)
ht = boundingbox[3]
wt = boundingbox[2]
#GH$
'''
D = 2*( 355.2/ wt) # meter,qianhou
x_cen = 640
xd = ix - x_cen
# geo similarity
X = D / 887.9 * xd # meter,zuoyou
'''
#xiaomi
D = (1978.9 / ht) # meter,qianhou
x_cen = 703
xd0 = ix0 - x_cen
# geo similarity
X = xd0 * (1.52 / ht) # meter,zuoyou
'''
D = (3006.5/ht) # meter,qianhou
x_cen = 1361.8
xd0 = ix0 - x_cen
# geo similarity
X = xd0*(1.75/ht)
'''
#K = D/(D+60)
#if ():
#else:
# bx2[0] = int(x+w1/20)
# bx2[1] = int(y-h1/20)
#bx2[2] = int(w)
#bx2[3] = int(h)
td = time()
dt = td - t0
#print(dt) # Time step between Filters steps
F = np.matrix([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
dt_2 = dt * dt
dt_3 = dt_2 * dt
dt_4 = dt_3 * dt
Q = np.matrix(
[[0.25 * dt_4 * noise_ax, 0, 0.5 * dt_3 * noise_ax, 0],
[0, 0.25 * dt_4 * noise_ay, 0, 0.25 * dt_3 * noise_ay],
[dt_3 / 2 * noise_ax, 0, dt_2 * noise_ax, 0],
[0, dt_3 / 2 * noise_ay, 0, dt_2 * noise_ay]])
dX = X - state[0][0]
dD = D - state[1][0]
dVx = int(dX / dt)
dVd = int(dD / dt)
dX2 = cx - state_2D[0][0]
dD2 = cy - state_2D[1][0]
dVx2 = int(dX2 / dt)
dVd2 = int(dD2 / dt)
state = F * state # Project the state ahead
state_2D = F * state_2D
P = F * P * F.T + Q # Project the error covariance ahead
# Measurement Update (Correction)
# ==============================
S = H * P * H.T + R
K = (P * H.T) * np.linalg.pinv(S)
# Update the estimate via z
Z = ([[float('%.2f' % dVx)], [float('%.2f' % dVd)]]
) # 本身应该用速度传感器测量真值的，但是由于没有这个传感器所以只能通过视频中算出来的X，D除去对应的dt计算。
Z2 = ([[float('%.2f' % dVx2)], [float('%.2f' % dVd2)]])
ykf = Z - (H * state)
y2kf = Z2 - (H * state_2D)
state = state + (K * ykf)
state_2D = state_2D + (K * y2kf)
# update the error convariance
P = (I - (K * H)) * P
#draw the picture and give out the info of interested obj
#print(str(int(state_2D[2][0])))
#print(str(int(state_2D[3][0])))
if (abs(int(state_2D[2][0])) - abs(int(state_2D[3][0])) > 29):
if (int(state_2D[2][0]) > 0):
#right
bx2[0] = int(x + w / 15)
bx2[1] = int(y - h / 15)
bx2[2] = int(w)
bx2[3] = int(h)
cv2.rectangle(frame, (x1, y1), (x1 + w1, y1 + h1),
(0, 0, 255), 2)
cv2.rectangle(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(0, 0, 255), 2)
cv2.line(frame, (x1, y1), (bx2[0], bx2[1]), (255, 0, 255),
2)
cv2.line(frame, (x1 + w1, y1), (bx2[0] + bx2[2], bx2[1]),
(255, 0, 255), 2)
cv2.line(frame, (x1, y1 + h1), (bx2[0], bx2[1] + bx2[3]),
(255, 0, 255), 2)
cv2.line(frame, (x1 + w1, y1 + h1),
(bx2[0] + bx2[2], bx2[1] + bx2[3]), (255, 0, 255),
2)
cv2.line(frame, (x1, y1), (x1 + w1, y1 + h1),
(255, 255, 255), 1)
cv2.line(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(100, 100, 50), 1)
cv2.line(frame, (x1 + w1, y1), (x1, y1 + h1),
(255, 255, 0), 1)
cv2.line(frame, (bx2[0] + bx2[2], bx2[1]),
(bx2[0], bx2[1] + bx2[3]), (100, 75, 50), 1)
cv2.arrowedLine(frame, (int((x1 + bx2[0]) / 2 + w1),
int((y1 + bx2[1]) / 2 + h1)),
(int((x1 + bx2[0]) / 2 + w1 + 50),
int((y1 + bx2[1]) / 2 + h1)), (0, 0, 255),
3)
else:
#left
bx2[0] = int(x + w / 15)
bx2[1] = int(y - h / 15)
bx2[2] = int(w)
bx2[3] = int(h)
cv2.rectangle(frame, (x1, y1), (x1 + w1, y1 + h1),
(0, 0, 255), 2)
cv2.rectangle(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(0, 0, 255), 2)
cv2.line(frame, (x1, y1), (bx2[0], bx2[1]), (255, 0, 255),
2)
cv2.line(frame, (x1 + w1, y1), (bx2[0] + bx2[2], bx2[1]),
(255, 0, 255), 2)
cv2.line(frame, (x1, y1 + h1), (bx2[0], bx2[1] + bx2[3]),
(255, 0, 255), 2)
cv2.line(frame, (x1 + w1, y1 + h1),
(bx2[0] + bx2[2], bx2[1] + bx2[3]), (255, 0, 255),
2)
cv2.line(frame, (x1, y1), (x1 + w1, y1 + h1),
(255, 255, 255), 1)
cv2.line(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(100, 100, 50), 1)
cv2.line(frame, (x1 + w1, y1), (x1, y1 + h1),
(255, 255, 0), 1)
cv2.line(frame, (bx2[0] + bx2[2], bx2[1]),
(bx2[0], bx2[1] + bx2[3]), (100, 75, 50), 1)
cv2.arrowedLine(frame, (int(
(x1 + bx2[0]) / 2), int((y1 + bx2[1]) / 2 + h1)),
(int((x1 + bx2[0]) / 2) - 50,
int((y1 + bx2[1]) / 2 + h1)), (0, 0, 255),
3)
else:
if (int(state_2D[3][0]) < 0):
#back
bx2[0] = int(x + w / 5)
bx2[1] = int(y - h / 5)
bx2[2] = int(w)
bx2[3] = int(h)
cv2.rectangle(frame, (x1, y1), (x1 + w1, y1 + h1),
(0, 0, 255), 2)
cv2.rectangle(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(0, 0, 255), 2)
cv2.line(frame, (x1, y1), (bx2[0], bx2[1]), (255, 0, 255),
2)
cv2.line(frame, (x1 + w1, y1), (bx2[0] + bx2[2], bx2[1]),
(255, 0, 255), 2)
cv2.line(frame, (x1, y1 + h1), (bx2[0], bx2[1] + bx2[3]),
(255, 0, 255), 2)
cv2.line(frame, (x1 + w1, y1 + h1),
(bx2[0] + bx2[2], bx2[1] + bx2[3]), (255, 0, 255),
2)
cv2.line(frame, (x1, y1), (x1 + w1, y1 + h1),
(255, 255, 255), 1)
cv2.line(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(100, 100, 50), 1)
cv2.line(frame, (x1 + w1, y1), (x1, y1 + h1),
(255, 255, 0), 1)
cv2.line(frame, (bx2[0] + bx2[2], bx2[1]),
(bx2[0], bx2[1] + bx2[3]), (100, 75, 50), 1)
cv2.arrowedLine(
frame, (int(bx2[0] + bx2[2] / 2), bx2[1] + bx2[3]),
(int(bx2[0] + bx2[2] / 2 + 50), bx2[1] + bx2[3] - 50),
(0, 0, 255), 3)
else:
#front
bx2[0] = int(x + w / 5)
bx2[1] = int(y - h / 5)
bx2[2] = int(w)
bx2[3] = int(h)
cv2.rectangle(frame, (x1, y1), (x1 + w1, y1 + h1),
(0, 0, 255), 2)
cv2.rectangle(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(0, 0, 255), 2)
cv2.line(frame, (x1, y1), (bx2[0], bx2[1]), (255, 0, 255),
2)
cv2.line(frame, (x1 + w1, y1), (bx2[0] + bx2[2], bx2[1]),
(255, 0, 255), 2)
cv2.line(frame, (x1, y1 + h1), (bx2[0], bx2[1] + bx2[3]),
(255, 0, 255), 2)
cv2.line(frame, (x1 + w1, y1 + h1),
(bx2[0] + bx2[2], bx2[1] + bx2[3]), (255, 0, 255),
2)
cv2.line(frame, (x1, y1), (x1 + w1, y1 + h1),
(255, 255, 255), 1)
cv2.line(frame, (bx2[0], bx2[1]),
(bx2[0] + bx2[2], bx2[1] + bx2[3]),
(100, 100, 50), 1)
cv2.line(frame, (x1 + w1, y1), (x1, y1 + h1),
(255, 255, 0), 1)
cv2.line(frame, (bx2[0] + bx2[2], bx2[1]),
(bx2[0], bx2[1] + bx2[3]), (100, 75, 50), 1)
cv2.arrowedLine(frame, (int(x1 + w1 / 2), y1 + h1),
(int(x1 + w1 / 2 - 50), y1 + h1 + 50),
(0, 0, 255), 3)
center = (int(cx), int(cy))
pts.appendleft(center)
for i in range(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
cv2.line(frame, (pts[i - 1]), (pts[i]), (0, 255, 0), 2)
t1 = time()
duration = 0.8 * duration + 0.2 * (t1 - t0)
cv2.putText(frame, 'FPS: ' + str(1 / duration)[:4].strip('.'),
(8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
cv2.putText(
frame, 'Pedestrian status camera-coor: X,D:' +
str(state[0][0]) + str(state[1][0]), (8, 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
cv2.putText(
frame, 'Pedestrian status camera-coor: Vx,Vd:' +
str(state[2][0]) + str(state[3][0]), (8, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
#print('2D parameter')
#print(str(state_2D[2][0]))
#print(str(state_2D[3][0]))
cv2.imshow('tracking', frame)
c = cv2.waitKey(20) & 0xFF
if c == 27 or c == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def compute_optical_flow(self,
frame,
color=[0, 0, 255],
region=None,
ang_vel=[0., 0., 0.],
average=False):
# Zero out the mask
mask = np.zeros_like(frame)
# Extract region values
region_str = region.__str__()
if region == None:
region_x = 0
region_y = 0
region_width = self.default_size[0]
region_height = self.default_size[1]
else:
region_x = int(region[0])
region_y = int(region[1])
region_width = int(region[2])
region_height = int(region[3])
# Initialize prev data, if needed
if not region_str in self.regions_frame_buffer:
self.regions_frame_buffer[region_str] = FrameBuffer(
self.buffer_size)
self.regions_frame_buffer[region_str].fill(
get_gray(frame, img_type='rgba'))
# self.p0 = cv2.goodFeaturesToTrack(self.prev_gray, mask = None, **self.feature_params)
self.regions_p0[region_str] = get_grid([region_x, region_y],
region_width, region_height,
self.num_points)
# Get appropriate data for the region
p0 = self.regions_p0[region_str]
prev_gray = self.regions_frame_buffer[region_str].pop()
# Get gray images
gray = get_gray(frame, img_type='rgba')
# Calculate optical flow
p1, st, err = cv2.calcOpticalFlowPyrLK(prev_gray, gray, p0, None,
**self.lk_params)
# Select good points
good_new = p1[st == 1]
good_old = p0[st == 1]
self.regions_frame_buffer[region_str].add_frame(gray)
# self.p0 = good_new.reshape(-1,1,2)
# Get static optical flow
u = (good_new - good_old) / self.dt
# Cancel out flow from rotational motion
u_stable = self.remove_rotation(good_old, u, ang_vel)
scale = 1.0 / self.buffer_size
if average == True:
u_stable = np.average(u_stable, 0)
region_center = np.array(
[region_x + region_width / 2, region_y + region_height / 2])
a, b = region_center.ravel()
c, d = region_center + scale * u_stable
mask = cv2.arrowedLine(mask, (int(a), int(b)), (int(c), int(d)),
color, 2)
mask = cv2.rectangle(
mask, (region_x, region_y),
(region_x + region_width, region_y + region_height), color)
frame = cv2.circle(frame, (int(a), int(b)), 3, color, -1)
else:
for i, (pt, vec) in enumerate(zip(good_old, u_stable)):
a, b = pt.ravel()
c, d = pt + scale * vec
c, d = int(c), int(d)
# c,d = old.ravel()
mask = cv2.arrowedLine(mask, (a, b), (c, d), color, 1)
frame = cv2.circle(frame, (a, b), 2, color, -1)
if not self.display_init:
self.display = cv2.add(frame, mask)
self.display_init = True
else:
self.display = cv2.add(self.display, mask)
if average == True:
return u_stable
else:
return [u_stable, good_old]
import numpy as np
import cv2
# อ่านรูปภาพเข้ามา กำหนดค่า 1 คือค่าสีจะเหมือนค่าเดิมของภาพ, 0 คือ grayscale, -1 คือ ความโปรงแสง
img = cv2.imread("./images/pienza-town-italy-1080x720.jpg", 1)
# Drawing Line เส้นตรง
img = cv2.line(img, (0, 0), (255, 255), (255, 0, 0), 10)
# Drawing Arrow Line ลูกศร
img = cv2.arrowedLine(img, (0, 200), (255, 200), (255, 255, 100), 10)
# Drawing Rectangle สี่เหลี่ยม
img = cv2.rectangle(img, (384, 0), (510, 128), (0, 255, 0), 3)
# Drawing Circle วงกลม
img = cv2.circle(img, (447, 103), 63, (0, 0, 255), 0)
# Drawing Ellipse วงรี
img = cv2.ellipse(img, (256, 256), (100, 50), 0, 0, 180, 255, -1)
# Drawing Polygon รูปหลายเหลี่ยม
pts = np.array([[10, 5], [20, 30], [70, 20], [50, 10]], np.int32)
pts = pts.reshape((-1, 1, 2))
img = cv2.polylines(img, [pts], True, (0, 255, 255))
# Adding Text to Images
font = cv2.FONT_HERSHEY_SIMPLEX
img = cv2.putText(img, 'OpenCV By Noy', (10, 500), font, 4, (255, 255, 255), 2,
cv2.LINE_AA)
代表蓝色，第一个是蓝色通道，第二个是绿色通道，第三个是红色通道。对于灰度图只需要传入灰度值。
• thickness 线条的粗细。如果给一个闭合图形 置为 -1 那么这个图形
就会被填充。 默认值是 1.
• linetype 线条的类型， 8 连接，抗锯齿等。 默认情况是8 连接。cv2.LINE_AA
为抗锯齿 这样看起来会非常平滑。
'''
# Create a black image
img = np.zeros((512, 512, 3), np.uint8)
# Draw a diagonal blue line with thickness of 5 px
cv2.line(img, pt1=(0, 0), pt2=(511, 511), color=(255, 0, 0), thickness=5) # pt1, pt2, color, thickness=
# cv2.polylines() 可以 用来画很多条线。只需要把想 画的线放在一 个列表中， 将 列表传给函数就可以了。每条线 会被独立绘制。 这会比用 cv2.line() 一条一条的绘制 要快一些。
# cv2.polylines(img, pts, isClosed, color, thickness=None, lineType=None, shift=None)
cv2.arrowedLine(img,pt1=(21, 13), pt2=(151, 401), color=(255, 0, 0), thickness=5)
cv2.rectangle(img, (384, 0), (510, 128), (0, 255, 0), 3)
cv2.circle(img, center=(447, 63), radius=63, color=(0, 0, 255), thickness=-1) # center, radius, color, thickness=None
# 一个参数是中心点的位置坐标。 下一个参数是长轴和短轴的长度。椭圆沿逆时针方向旋转的角度。
# 椭圆弧演顺时针方向起始的角度和结束角度 如果是 0 很 360 就是整个椭圆
cv2.ellipse(img, center=(256, 256), axes=(100, 50), angle=0, startAngle=0, endAngle=180, color=255,
thickness=-1) # center, axes, angle, startAngle, endAngle, color, thickness=
pts = np.array([[10, 5], [20, 30], [70, 20], [50, 10]], np.int32)
pts = pts.reshape((-1, 1, 2))
# 这里 reshape 的第一个参数为-1, 表明这一维的长度是根据后面的维度的计算出来的。
# 注意 如果第三个参数是 False 我们得到的多边形是不闭合的 ，首 尾不相 连 。
filter=4,
cThresh=[50, 50],
draw=False)
if len(Contours1) != 0:
for obj in Contours2:
cv2.polylines(imgContours2, [obj[2]], True, (0, 255, 0), 2)
newPoints = utils.reorder(obj[2])
#Divide no of pixels by the scale value
newWidth = round((utils.findDist(
newPoints[0][0] // scale, newPoints[1][0] // scale) / 10),
1)
newHeight = round((utils.findDist(
newPoints[0][0] // scale, newPoints[2][0] // scale) / 10),
1)
cv2.arrowedLine(imgContours2,
(newPoints[0][0][0], newPoints[0][0][1]),
(newPoints[1][0][0], newPoints[1][0][1]),
(255, 0, 255), 3, 8, 0, 0.05)
cv2.arrowedLine(imgContours2,
(newPoints[0][0][0], newPoints[0][0][1]),
(newPoints[2][0][0], newPoints[2][0][1]),
(255, 0, 255), 3, 8, 0, 0.05)
x, y, w, h = obj[3]
cv2.putText(imgContours2, '{}cm'.format(newWidth),
(x + 30, y - 10), cv2.FONT_HERSHEY_COMPLEX_SMALL,
1.5, (255, 0, 255), 2)
cv2.putText(imgContours2, '{}cm'.format(newHeight),
(x - 70, y + h // 2),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1.5, (255, 0, 255),
2)
cv2.imshow('A4', imgContours2)