def yoloRecognitionCallback(self, bbs):
rospy.loginfo('Image ID: ' + str(bbs.image_header.seq))
bbs_l = bbs.bounding_boxes
objects = []
people = []
for bb in bbs_l:
if bb.Class in dictionary.keys():
reference_model = bb.Class
bb.Class = dictionary[bb.Class]
if 'people' in self.possible_classes and bb.Class in self.possible_classes[
'people'] and bb.probability >= self.threshold:
person = Description()
person.label_class = 'people' + '/' + bb.Class
person.reference_model = reference_model
person.probability = bb.probability
person.bounding_box.minX = bb.xmin
person.bounding_box.minY = bb.ymin
person.bounding_box.width = bb.xmax - bb.xmin
person.bounding_box.height = bb.ymax - bb.ymin
people.append(person)
elif bb.Class in [
val for sublist in self.possible_classes.values()
for val in sublist
] and bb.probability >= self.threshold:
object_d = Description()
index = 0
i = 0
for value in self.possible_classes.values():
if bb.Class in value:
index = i
i += 1
object_d.reference_model = reference_model
object_d.label_class = list(
self.possible_classes.keys())[index] + '/' + bb.Class
object_d.probability = bb.probability
object_d.bounding_box.minX = bb.xmin
object_d.bounding_box.minY = bb.ymin
object_d.bounding_box.width = bb.xmax - bb.xmin
object_d.bounding_box.height = bb.ymax - bb.ymin
objects.append(object_d)
objects_msg = Recognitions()
people_msg = Recognitions()
if len(objects) > 0:
objects_msg.header = bbs.header
objects_msg.image_header = bbs.image_header
objects_msg.descriptions = objects
self.recognized_objects_pub.publish(objects_msg)
if len(people) > 0:
people_msg.header = bbs.header
people_msg.image_header = bbs.image_header
people_msg.descriptions = people
self.recognized_people_pub.publish(people_msg)
def instanceSegmentationCallback(self, instance_results):
rospy.loginfo('Image ID: ' + str(instance_results.header.seq))
objects = []
people = []
for i, (bb, class_id, class_name, score, mask) in enumerate(
zip(instance_results.boxes, instance_results.class_ids,
instance_results.class_names, instance_results.scores,
instance_results.masks)):
if class_name in self.possible_classes[
'people'] and score >= self.threshold:
person = Description()
person.label_class = "person/{}".format(class_name)
person.probability = score
person.bounding_box.minX = bb.x_offset
person.bounding_box.minY = bb.y_offset
person.bounding_box.width = bb.width
person.bounding_box.height = bb.height
person.mask = mask
people.append(person)
elif class_name in [
val for sublist in self.possible_classes.values()
for val in sublist
] and score >= self.threshold:
object_d = Description()
index = 0
i = 0
for value in self.possible_classes.values():
if class_name in value:
index = i
i += 1
object_d.label_class = list(
self.possible_classes.keys())[index] + '/' + class_name
object_d.probability = score
object_d.bounding_box.minX = bb.x_offset
object_d.bounding_box.minY = bb.y_offset
object_d.bounding_box.width = bb.width
object_d.bounding_box.height = bb.height
objects.append(object_d)
objects_msg = Recognitions()
people_msg = Recognitions()
if len(objects) > 0:
objects_msg.header = instance_results.header
objects_msg.image_header = instance_results.header
objects_msg.descriptions = objects
self.recognized_objects_pub.publish(objects_msg)
if len(people) > 0:
people_msg.header = instance_results.header
people_msg.image_header = instance_results.header
people_msg.descriptions = people
self.recognized_people_pub.publish(people_msg)
def recognitionProcess(self, ros_msg):
rospy.loginfo('Image ID: {}'.format(ros_msg.header.seq))
bgr_image = BRIDGE.imgmsg_to_cv2(ros_msg, desired_encoding="bgr8")
self.image_height = bgr_image.shape[0]
self.image_width = bgr_image.shape[1]
face_rects = self.detectFaces(bgr_image)
if len(face_rects) == 0:
rospy.loginfo("Recognition FPS: {:.2f} Hz.".format(
(1 / (rospy.get_rostime().to_sec() - self.last_recognition))))
self.last_recognition = rospy.get_rostime().to_sec()
return None
faces_description = []
for face_rect in face_rects:
face_description = Description()
aligned_face = self.alignFace(bgr_image, face_rect)
features_array = self.extractFeatures(aligned_face)
classification = self.classify(features_array)
label_class, confidence = classification
bounding_box = self.dlibRectangle2RosBoundingBox(face_rect)
features = self.numpyArray2RosVector(features_array)
face_description.label_class = label_class
face_description.probability = confidence
face_description.bounding_box = bounding_box
faces_description.append(face_description)
recognized_faces = Recognitions()
recognized_faces.image_header = ros_msg.header
recognized_faces.header.stamp = rospy.get_rostime()
recognized_faces.header.frame_id = "face_recognition"
recognized_faces.descriptions = faces_description
rospy.loginfo("Recognition FPS: {:.2f} Hz.".format(
(1 / (rospy.get_rostime().to_sec() - self.last_recognition))))
self.last_recognition = rospy.get_rostime().to_sec()
return recognized_faces
def peopleDetectionCallBack(recognition):
frame_id = recognition.image_header.seq
req = image_request_client(frame_id)
frame = req.rgbd_image.rgb
descriptions = recognition.descriptions
cv_frame = BRIDGE.imgmsg_to_cv2(frame, desired_encoding='rgb8')
frame_rgb = cv2.cvtColor(cv_frame, cv2.COLOR_BGR2RGB)
people_tracking.setFrame(recognition.image_header, recognition.header,
frame_id, frame_rgb.copy())
img_size = frame.height * frame.width
for description in descriptions:
area = description.bounding_box.width * description.bounding_box.height
if ((description.probability < probability_threshold)
or (area < img_size * boundingBox_threshold)):
descriptions.remove(description)
tracker, detections = people_tracking.track(descriptions)
if people_tracking.trackingPerson is not None:
if people_tracking.trackingPerson.is_confirmed(
) and people_tracking.trackingPerson.time_since_update <= 1:
response = Recognitions()
response.image_header = recognition.image_header
response.header = recognition.header
response.descriptions = [Description()]
response.descriptions[0].label_class = 'tracked_person'
BBbox = BoundingBox()
BBbox.minX = people_tracking.trackingPerson.to_tlwh()[0]
BBbox.minY = people_tracking.trackingPerson.to_tlwh()[1]
BBbox.width = people_tracking.trackingPerson.to_tlwh()[2]
BBbox.height = people_tracking.trackingPerson.to_tlwh()[3]
response.descriptions[0].bounding_box = BBbox
tracker_publisher.publish(response)
else:
people_tracking.findPerson()
debug(frame_rgb.copy(), tracker, detections,
people_tracking.trackingPerson)