def get_test_bounding_boxes():
Helipad = BoundingBox()
Helipad.probability = 0.5
Helipad.xmin = 312
Helipad.ymin = 120
Helipad.xmax = 337
Helipad.ymax = 148
Helipad.id = 2
Helipad.Class = "Helipad"
H = BoundingBox()
H.probability = 0.5
H.xmin = 320
H.ymin = 128
H.xmax = 330
H.ymax = 138
H.id = 0
H.Class = "H"
Arrow = BoundingBox()
Arrow.probability = 0.5
Arrow.xmin = 333
Arrow.ymin = 140
Arrow.xmax = 335
Arrow.ymax = 143
Arrow.id = 1
Arrow.Class = "Arrow"
bbs = BoundingBoxes()
bbs.bounding_boxes = [Helipad, H, Arrow]
return bbs
def callback(self, data):
print('\n', 'Predicting')
# try:
# cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
# except CvBridgeError as e:
# print(e)
# cv_image = cv2.copyMakeBorder(cv_image, 100, 100, 100, 100, cv2.BORDER_CONSTANT, value=[85, 120, 68])
# cv_image = cv2.imread("pic_buffer/R.png")
# cv2.imwrite("pic_buffer/R.png", cv_image)
# im = array_to_image(cv_image)
# dn.rgbgr_image(im)
# # print(dn.detect(net, meta, im))
# print("===")
r = []
img = cv2.imread("pic_buffer/1_R.png")
sp = img.shape
r += self.subpredict(0, int(sp[0] / 2), 0, sp[1])
r += self.subpredict(int(sp[0] / 2), sp[0], 0, sp[1])
r += self.subpredict(int(sp[0] / 4), int(sp[0] * 3 / 4), 0, sp[1])
r += self.subpredict(0, sp[0], 0, int(sp[1] / 2))
r += self.subpredict(0, sp[0], int(sp[1] / 2), sp[1])
r += self.subpredict(0, sp[0], int(sp[1] / 3), int(sp[1] * 2 / 3))
r += self.subpredict(0, sp[0], 0, int(sp[1] * 2 / 3))
r += self.subpredict(0, sp[0], int(sp[1] / 3), sp[1])
r += self.subpredict(0, int(sp[0] * 2 / 3), 0, sp[1])
r += self.subpredict(int(sp[0] / 3), sp[0], 0, sp[1])
r = self.removeDuplicate(r)
print()
img = self.drawCrossings(img, r)
cv2.imwrite("pic_buffer/2_D.png", img)
boxes = BoundingBoxes()
print('\n', 'Predict result:')
for i in range(len(r)):
box = BoundingBox()
print(' ', r[i][0], r[i][1] * 100, '%')
box.Class = r[i][0].decode('utf-8')
box.probability = r[i][1]
box.id = i
box.xmin = int(r[i][2][0])
box.ymin = int(r[i][2][1])
box.xmax = int(r[i][2][2])
box.ymax = int(r[i][2][3])
boxes.bounding_boxes.append(box)
# if b'endpoint' == r[i][0]:
# print('\t', r[i][0], r[i][1]*100, '%')
print(' ', int(r[i][2][0]), int(r[i][2][1]), int(r[i][2][2]),
int(r[i][2][3]))
# if b'cross' == r[i][0]:
# print('\t', r[i][0], r[i][1]*100, '%')
# # print('\t', r[i][2])
print('\n', 'Darknet waiting for rgb_img')
time.sleep(0.5)
try:
self.boxes_pub.publish(boxes)
except CvBridgeError as e:
print(e)
def create_d_msgs_box(self, track):
one_box = BoundingBox()
one_box.id = int(track.id[:3], 16)
one_box.class_id = "face"
one_box.probability = float(track.score)
one_box.xmin = int(track.box[0])
one_box.ymin = int(track.box[1])
one_box.xmax = int(track.box[2])
one_box.ymax = int(track.box[3])
return one_box
def GetBBoxesMsg(self, label_file: str):
output_bboxes_msg = BoundingBoxes()
with open(label_file, 'r') as file:
id_ = 0
for line in file:
gt = line.split()
bbox_msg = BoundingBox()
#print(gt)
bbox_msg.xmin = int(gt[5])
bbox_msg.ymin = int(gt[6])
bbox_msg.xmax = int(gt[7])
bbox_msg.ymax = int(gt[8])
bbox_msg.id = id_
bbox_msg.Class = gt[0]
id_ += 1
output_bboxes_msg.bounding_boxes.append(bbox_msg)
return output_bboxes_msg
def boxcallback(self, msg):
dets = []
for i in range(len(msg.bounding_boxes)):
bbox = msg.bounding_boxes[i]
dets.append(
np.array([
bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax,
bbox.probability
]))
dets = np.array(dets)
start_time = time.time()
trackers = self.update(dets)
cycle_time = time.time() - start_time
print(cycle_time)
r = BoundingBoxes()
rb = BoundingBox()
for d in range(len(trackers)):
rb.probability = 1
rb.xmin = trackers[d, 0]
rb.ymin = trackers[d, 1]
rb.xmax = trackers[d, 2]
rb.ymax = trackers[d, 3]
rb.id = trackers[d, 4]
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if self.img_in == 1 and self.display:
res = trackers[d].astype(np.int32)
rgb = self.colours[res[4] % 32, :] * 255
cv2.rectangle(self.img, (res[0], res[1]), (res[2], res[3]),
(rgb[0], rgb[1], rgb[2]), 2)
cv2.putText(self.img, "ID : %d" % (res[4]), (res[0], res[1]),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (200, 85, 200), 2)
if self.img_in == 1 and self.display:
try:
self.image = self.bridge.cv2_to_imgmsg(self.img, "bgr8")
self.image.header.stamp = rospy.Time.now()
self.pubimage.publish(self.image)
except CvBridgeError as e:
pass
r.header.stamp = rospy.Time.now()
self.pubb.publish(r)
return
start_time = time.time()
if mot_tracker.bbox_checkin==1:
trackers = mot_tracker.update(mot_tracker.dets)
mot_tracker.bbox_checkin=0
else:
trackers = mot_tracker.update(np.empty((0,5)))
r = BoundingBoxes()
for d in range(len(trackers)):
rb = BoundingBox()
rb.probability=1
rb.xmin = int(trackers[d,0])
rb.ymin = int(trackers[d,1])
rb.xmax = int(trackers[d,2])
rb.ymax = int(trackers[d,3])
rb.id = int(trackers[d,4])
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if mot_tracker.img_in==1 and mot_tracker.display:
res = trackers[d].astype(np.int32)
rgb=colours[res[4]%32,:]*255
cv2.rectangle(mot_tracker.img, (res[0],res[1]), (res[2],res[3]), (rgb[0],rgb[1],rgb[2]), 6)
cv2.putText(mot_tracker.img, "ID : %d"%(res[4]), (res[0],res[1]), cv2.FONT_HERSHEY_SIMPLEX, 1.6, (200,85,200), 6)
if mot_tracker.img_in==1 and mot_tracker.display:
try :
mot_tracker.image = mot_tracker.bridge.cv2_to_imgmsg(mot_tracker.img, "bgr8")
mot_tracker.image.header.stamp = rospy.Time.now()
mot_tracker.pubimage.publish(mot_tracker.image)
except CvBridgeError as e:
pass
def __raw_image_callback(self, data):
self.image_header = data.header
raw_image = self.bridge.imgmsg_to_cv2(data).astype(np.uint8)
if len(raw_image.shape) <= 2:
return
self.image = raw_image
# 1. get SORT bounding boxes
new_bboxes_sort = []
if len(self.new_bboxes) != 0:
new_bboxes = copy.deepcopy(self.new_bboxes)
self.new_bboxes = []
for bbox in new_bboxes:
new_bboxes_sort.append(
np.asarray((bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax,
bbox.probability)))
new_bboxes_sort = np.asarray(new_bboxes_sort)
# 2. update tracker
trackers = self.mot_tracker.update(new_bboxes_sort)
matched, _, _ = associate_detections_to_trackers(
new_bboxes_sort, trackers, 0.3)
else:
# 2. update tracker
trackers = self.mot_tracker.update()
if len(self.bboxes) == 0:
matched = np.empty((0, 2), dtype=int)
else:
new_bboxes = copy.deepcopy(self.bboxes)
for bbox in new_bboxes:
new_bboxes_sort.append(
np.asarray((bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax,
bbox.probability)))
new_bboxes_sort = np.asarray(new_bboxes_sort)
matched, _, _ = associate_detections_to_trackers(
new_bboxes_sort, trackers, 0.3)
# 3. update current bounding boxes & extract tracking trace
# 1). id is the id from tracker;
# 2). class is the latest class from detection;
# 3). probability is the latest probability from detection;
self.bboxes = []
ids = []
if trackers.shape[0] != 0:
for tracker in trackers:
bbox = BoundingBox()
print(tracker)
bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax = int(
tracker[0]), int(tracker[1]), int(tracker[2]), int(
tracker[3])
bbox.id = int(tracker[4])
self.bboxes.append(bbox)
id = int(bbox.id)
center = (int((bbox.xmin + bbox.xmax) / 2.),
int((bbox.ymin + bbox.ymax) / 2.))
if self.traces.get(id) == None:
self.traces[id] = [center]
else:
self.traces[id].append(center)
ids.append(id)
if matched.shape[0] != 0:
for pair in matched:
original_id = pair[0]
new_id = pair[1]
original_bbox = new_bboxes[original_id]
self.bboxes[new_id].Class = original_bbox.Class
self.bboxes[new_id].probability = original_bbox.probability
del_ids = []
for k in self.traces:
if k not in ids:
del_ids.append(k)
for k in del_ids:
del self.traces[k]
# 4. publish current bounding boxes
self.__publish_bbox()
# 5. publish tracking image
self.__publish_tracking_image(self.image, self.bboxes)
start_time = time.time()
if mot_tracker.bbox_checkin == 1:
trackers = mot_tracker.update(mot_tracker.dets)
mot_tracker.bbox_checkin = 0
else:
trackers = mot_tracker.update(np.empty((0, 5)))
r = BoundingBoxes()
for d in range(len(trackers)):
rb = BoundingBox()
rb.probability = 1
rb.xmin = trackers[d, 0]
rb.ymin = trackers[d, 1]
rb.xmax = trackers[d, 2]
rb.ymax = trackers[d, 3]
rb.id = trackers[d, 4]
rb.Class = 'tracked'
r.bounding_boxes.append(rb)
if mot_tracker.img_in == 1 and mot_tracker.display:
res = trackers[d].astype(np.int32)
rgb = colours[res[4] % 32, :] * 255
cv2.rectangle(mot_tracker.img, (res[0], res[1]),
(res[2], res[3]), (rgb[0], rgb[1], rgb[2]),
6)
cv2.putText(mot_tracker.img, "ID : %d" % (res[4]),
(res[0], res[1]), cv2.FONT_HERSHEY_SIMPLEX,
1.6, (200, 85, 200), 6)
if mot_tracker.img_in == 1 and mot_tracker.display:
try:
mot_tracker.image = mot_tracker.bridge.cv2_to_imgmsg(
mot_tracker.img, "bgr8")