def __init__(self):
super(PeopleObjectTrackerNode, self).__init__()
# init the node
rospy.init_node('people_object_tracker', anonymous=False)
# Get the parameters
(detection_topic, tracker_topic, cost_threshold, \
max_age, min_hits) = \
self.get_parameters()
self._bridge = CvBridge()
self.tracker = sort.Sort(max_age=max_age, min_hits=min_hits)
self.cost_threshold = cost_threshold
# Advertise the result of Object Tracker
self.pub_trackers = rospy.Publisher(tracker_topic, \
DetectionArray, queue_size=1)
self.sub_detection = rospy.Subscriber(detection_topic, \
DetectionArray,\
self.detection_callback)
# spin
rospy.spin()
def main():
global tracker
global msg
msg = IntList()
global inverse_cam_matrix, cam_to_quad, tcam
#Initialize ROS node
rospy.init_node('sort_tracker', anonymous=False)
rate = rospy.Rate(10)
# Get the parameters
global camera_parameters
(camera_topic, detection_topic, tracker_topic, cost_threshold, max_age, min_hits,camera_parameters) = get_parameters()
inverse_cam_matrix = np.linalg.inv(np.reshape(np.array(camera_parameters['camera_matrix']['data']), (camera_parameters['camera_matrix']['rows'],camera_parameters['camera_matrix']['cols'])))
cam_to_quad = np.linalg.inv(np.reshape(np.array(camera_parameters['rotation']),(3,3)))
tcam = np.reshape(np.array(camera_parameters['translation']),(3,1))
tracker = sort.Sort(max_age=max_age, min_hits=min_hits) #create instance of the SORT tracker
cost_threshold = cost_threshold
#Subscribe to image topic
image_sub = rospy.Subscriber(camera_topic,Image,callback_image)
#Subscribe to darknet_ros to get BoundingBoxes from YOLOv3
sub_detection = rospy.Subscriber(detection_topic, BoundingBoxes , callback_det)
#Publish results of object tracking
pub_trackers = rospy.Publisher(tracker_topic, IntList, queue_size=10)
#print(msg) #Testing msg that is published
#pub_trackers.publish(msg)
sub_odometry = rospy.Subscriber("Odometry",Odometry,callback_odom)
global pub_bbposes
pub_bbposes = rospy.Publisher("BBposes",BBPoses,queue_size=10)
rospy.spin()
def SORT(self, predictions):
qtt_frames = len(predictions)
tracker = sort.Sort()
tracked_vehicles_trajectory = {} # Trajetória de cada ID identificado
vehicles_on_the_frame = {} # Veículos que estão presentes no frame X
# Ids e bounding boxes preditas pelo tracking
mot_labels = [[0, 0, 0, 0, 0, 0, 0] for _ in range(qtt_frames + 1)]
for frame_number in range(1, qtt_frames + 1):
bboxes_atual = predictions[frame_number][:]
# Formatar a lista para alimentar o Sort
# np.array( [ [x1,y1,x2,y2,score1], [x3,y3,x4,y4,score2], ... ] )
if len(bboxes_atual) == 0:
bboxes_atual = np.zeros(
(0, 5)) # Requerido pelo Algoritmo Sort
else:
for idx in range(len(bboxes_atual)):
x1, y1, w, h, classe = bboxes_atual[idx]
x2 = x1 + w
y2 = y1 + h
score = np.random.randint(
50,
100) / 100 # Temporariamente setar score como random.
bboxes_atual[idx] = [x1, y1, x2, y2, score, classe]
# Numpy array requerido pelo Sort
bboxes_atual = np.array(bboxes_atual)
# Analisar o frame atual e identificar os bounding boxes id (update SORT)
track_bbs_ids = tracker.update(bboxes_atual[:, :-1])
this_frame_ids = track_bbs_ids[:, -1]
# Passar as coordenadas para o padrão: [frame,x,y,w,h,idx]
newboxes_list = [[0, 0, 0, 0, 0, 0, 0]
for _ in range(len(track_bbs_ids))]
for i, newbox in enumerate(track_bbs_ids):
x1, y1, x2, y2, idx = newbox
x, y, w, h = x1, y1, abs(x2 - x1), abs(y2 - y1)
x, y, w, h, idx = int(x), int(y), int(w), int(h), int(idx)
newboxes_list[i] = [frame_number, x, y, w, h, classe, idx]
# Guardar a trajetória do centro do veículo IDx
xc, yc = int(x + w / 2), int(y + h / 2)
if idx in tracked_vehicles_trajectory:
tracked_vehicles_trajectory[idx].append(
(frame_number, xc, yc))
else:
tracked_vehicles_trajectory[idx] = [(frame_number, xc,
yc)]
# Atualizar as variáveis
vehicles_on_the_frame[frame_number] = this_frame_ids
mot_labels[frame_number] = newboxes_list[:]
return mot_labels, tracked_vehicles_trajectory, vehicles_on_the_frame
def main():
global tracker
global msg
msg = IntList()
while not rospy.is_shutdown():
#Initialize ROS node
rospy.init_node('sort_tracker', anonymous=False)
rate = rospy.Rate(10)
# Get the parameters
(camera_topic, detection_topic, tracker_topic, cost_threshold, max_age,
min_hits) = get_parameters()
tracker = sort.Sort(
max_age=max_age,
min_hits=min_hits) #create instance of the SORT tracker
cost_threshold = cost_threshold
#Subscribe to image topic
image_sub = rospy.Subscriber(camera_topic, Image, callback_image)
#Subscribe to darknet_ros to get BoundingBoxes from YOLOv3
sub_detection = rospy.Subscriber(detection_topic, BoundingBoxes,
callback_det)
#Publish results of object tracking
pub_trackers = rospy.Publisher(tracker_topic, IntList, queue_size=10)
#print(msg) #Testing msg that is published
#pub_trackers.publish(msg)
rate.sleep()
rospy.spin()
def main():
global tracker
global msg
global detections
global total_time
global total_frames
global start_time
global cycle_time
global tracker_id
global iou_threshold
global display
global detection_event
display = False
total_frames = 0
detection_event = False
# holds tracker id
counter = IntList()
while not rospy.is_shutdown():
#Initialize ROS node
# frame +=1
rospy.init_node('sort_tracker', anonymous=False)
# Get the parameters
(queue_size, camera_topic, detection_topic, tracker_topic,
cost_threshold, iou_threshold, display, max_age, min_hits,
fps) = get_parameters()
cost_threshold = cost_threshold
# loop twice per frame
rate = rospy.Rate(3)
tracker = sort.Sort(
max_age=max_age,
min_hits=min_hits) #create instance of the SORT tracker
#Subscribe to darknet_ros to get BoundingBoxes from yolov3-detections
sub_detection = rospy.Subscriber(detection_topic, BoundingBoxes,
callback_det)
#Subscribe to image topic
image_sub = rospy.Subscriber(camera_topic, Image, callback_image)
#Publish results of object tracking
# pub_trackers = rospy.Publisher(tracker_topic, IntList, queue_size=queue_size)
# print(msg)
# 1.5 = 2
total_frames = math.ceil(total_frames + 1 / 2)
#pub_trackers.publish(msg)
rate.sleep()
rospy.spin()
def __init__(self):
# Gets the necessary parameters from .yaml file
self.loop_rate = rospy.get_param("~loop_rate")
self.show_image = rospy.get_param("~show_image")
self.camera_topic = rospy.get_param("~camera_topic")
self.detection_topic = rospy.get_param("~detection_topic")
self.tracker_topic = rospy.get_param('~tracker_topic')
self.cost_threshold = rospy.get_param('~cost_threhold')
self.min_hits = rospy.get_param('~min_hits')
self.max_age = rospy.get_param('~max_age')
self.msg = TrackedBoundingBoxes()
global tracker
tracker = sort.Sort(max_age=self.max_age, min_hits=self.min_hits)
# Subscribe to image topic
self.image_sub = rospy.Subscriber(self.camera_topic, Image, self.callback_image)
# Subscribe to darknet_ros to get BoundingBoxes from YOLOv3
self.sub_detection = rospy.Subscriber(self.detection_topic, BoundingBoxes, self.callback_det)
# Publish results of object tracking
self.pub_trackers = rospy.Publisher(self.tracker_topic, TrackedBoundingBoxes, queue_size=10)
# keep process alive
rospy.spin()
import time
from sort import sort
import numpy as np
# create instance of SORT
mot_tracker = sort.Sort()
total_frames = 0
total_time = 0
# get detections
# dets - a numpy array of detections in the format [[x1,y1,x2,y2,score],[x1,y1,x2,y2,score],...]
seq_dets = np.loadtxt('det_sort.csv', delimiter=',') # load detections
with open('out_sort.csv', 'w') as out_file:
print("Processing detections...")
for frame in range(int(seq_dets[:, 0].max())):
frame += 1 # detection and frame numbers begin at 1
dets = seq_dets[seq_dets[:, 0] == frame, 2:7]
dets[:, 2:4] += dets[:, 0:2] # convert to [x1,y1,w,h] to [x1,y1,x2,y2]
total_frames += 1
start_time = time.time()
trackers = mot_tracker.update(dets)
cycle_time = time.time() - start_time
total_time += cycle_time
for d in trackers:
print('%d,%d,%.2f,%.2f,%.2f,%.2f' %
(frame, d[4], d[0], d[1], d[2] - d[0], d[3] - d[1]),
file=out_file)
# update SORT
def run():
# declare global variables that can be updated globally
global counter
global _current_image
global _bridge
global _detected_bbox
global coordinates
global object_class
global dets
global marked_image
global _frame_seq
global _current_image_header_seq
global _detected_image_header_seq
global _current_detected_image
global tracker
global trackers # contains tracker id, x1, y1, x2, y2, probabilitiy
global _old_detection
global _current_detected_image_seq
trackers = [] # this is going to hold the state estimates of all of the trackers.
tracker_ids = []
_old_detection = None
_current_detected_image_seq = -1
_detected_image_header_seq = 0
_current_image_header_seq = -1
temp_detected_image = -2
counter = 0
# initilize current image and detected boxes
_current_image_buffer = []
_detected_image_buffer = []
_detected_image_header_seq_buffer = []
_current_image_header_seq_buffer = []
marked_image = None
_current_image = None
_current_detected_image = None
# _detected_bbox_buffer = []
_detected_bbox = None
# setup cv2 bridge
_bridge = CvBridge()
# image and point cloud subscribers
image_topic, detection_topic, display = getParam()
# subscribe to raw image feed
# rospy.Subscriber(image_topic, Image, image_callback, queue_size=100)
# subscribe to detections
rospy.Subscriber(detection_topic, BoundingBoxes, detector_callback, queue_size=1)
rospy.Subscriber("/darknet_ros/detection_image", Image, detected_image_callback, queue_size=1)
# publish image topic with bounding boxes
_imagepub = rospy.Publisher('~labeled_image', Image, queue_size=1)
# TODO publish original unmarked frame
# _imagepub = rospy.Publisher('~original_frames', Image, queue_size=10)
# TODO publish marked image with sort detections
rospy.loginfo("ready to detect")
# adjust frame rate
r = rospy.Rate(6)
# max age is the maximum number of frames a tracker can exist by making max_age > 1
# you can allow it to survive without a detection, for instance if there are skip frames
# in this there is an expected number of skip frames, by making max_age = n, you are allowing
# for n skip frames.
# min hits is the minimum number of times a tracker must be detected to survive
tracker = sort.Sort(max_age = 8, min_hits=1) #create instance of the SORT tracker
# counter = 0
frames = 1
_old_detection = rospy.wait_for_message(detection_topic, BoundingBoxes).image_header.seq
# rospy.wait_for_message("/darknet_ros/detection_image", Image)
while not rospy.is_shutdown():
# _old_detection = _detected_bbox.image_header.seq
# rospy.wait_for_message("/darknet_ros/detection_image", Image)
boxes = []
class_ids = []
detections = 0
dets = []
# if frames == 1:
# _old_detection = _detected_bbox.seq
# if frames == 1:
# _old_detection = _detected_bbox
# in the form: dets.append([x_min, y_min, x_max, y_max, probability])
# dets = np.empty((0,5))
# only run if there is an image
if new_detection(_detected_bbox) and _current_detected_image is not None:
# if frames == 1:
# _old_detection = _detected_bbox.image_header.seq
print("current image seq: %s", _current_detected_image_seq)
# print("frames processed %s: " %frames)
print("detected frame number: %s" % _detected_image_header_seq)
# if frame < 3 :
# rospy.spin()
# check to see if curr detection is on the current frame
# if _current_image_header_seq == _detected_image_header_seq:
# print("image seq are the same")
try:
# image received
# convert image from the subscriber into an OpenCV image
# initialize the detected image as the current frame
# initialize our current frame
# marked_image = _bridge.imgmsg_to_cv2(_current_image,
# 'rgb8')
marked_image = _bridge.imgmsg_to_cv2(_current_detected_image,
'rgb8')
# _imagepub.publish(_bridge.cv2_to_imgmsg(marked_image, 'rgb8')) # publish detection results
# marked_image, objects = self._detector.from_image(scene) # detect objects
# _imagepub.publish(_bridge.cv2_to_imgmsg(scene, 'rgb8')) # publish detection results
# what if there is no bounding boxes?
# go through all the detections in each frame
if _detected_bbox is not None:
frames +=1
for box in _detected_bbox.bounding_boxes:
# xmin, ymin, xmax, ymax = box.xmin, box.ymin, box.xmax, box.ymax
obj_class = box.Class
# update the trackers one at a time based on detection boxes ?
# collect all boxes from the relevant class
if box.Class == "plant":
# rospy.loginfo("cycle_time %s", cycle_time)
# [x1,y1,x2,y2] - for each object
# store the class ids
dets.append([box.xmin, box.ymin, box.xmax, box.ymax, box.probability])
# class_ids.append(obj_class)
# detections +=
else: pass # no plants in the image
# there are no detections, you still need to update you trackers
dets = np.array(dets) # convert for the n dimensional array
# whether or not the detections are none, the trackers still need to be updated
# if len(dets) == 0:
# rospy.loginfo("no targets detected!")
# # trackers = tracker.update(dets)
# else:
# rospy.loginfo("targets detected")
# trackers = tracker.update(dets)
# rospy.loginfo("trackers updated based on current detections")
trackers = tracker.update(dets)
# print(trackers)
# iterate through all the trackers
for t in trackers:
# rospy.loginfo("tracker ID: %s", d[4])
# rospy.loginfo("tracker id: " + str(t[4]) + ": " + "info: " + str(t))
# _tracker_ids.append(str(t[4]))
str_n = str(int(t[4]))
if (str_n in tracker_ids) == False:
# print("unique id found")
# unique id
tracker_ids.append(str_n)
counter +=1
else: pass
box_t = [t[0], t[1], t[2], t[3]]
# draw every tracker
# draw_detections(box, obj_class, tracker_id, im):
# draw_detections(box_t, obj_class, t[4], marked_image)
# draw_detections(marked_image, t[4], box_t)
draw_detections(box_t, "plant", t[4], marked_image)
print("plant count:%s"%str(counter))
# the default case is the current frame with no writting
_imagepub.publish(_bridge.cv2_to_imgmsg(marked_image, 'rgb8')) # publish detection results
_old_detection = _detected_bbox.image_header.seq
except CvBridgeError as e:
print(e)
r.sleep() # best effort to maintain loop rate r for each frame