def on_new_frame(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, "bgr8")
frame = np.array(frame, dtype=np.uint8)
result_raw = self.find(frame)
boundingBox = BoundingBox2D()
boundingBox.center.x = result_raw[0]
boundingBox.center.y = result_raw[1]
boundingBox.size_x = result_raw[2]
boundingBox.size_y = result_raw[2]
self.result_pub.publish(boundingBox)
debug_frames = result_raw[3]
while len(self.debug_pubs) < len(debug_frames):
self.debug_pubs.append(
rospy.Publisher("debug/channel{}".format(len(self.debug_pubs)),
Image,
queue_size=10))
for i, frame in enumerate(debug_frames):
colored = len(frame.shape) == 3
encoding = 'bgr8' if colored else 'passthrough'
self.debug_pubs[i].publish(
self.bridge.cv2_to_imgmsg(frame, encoding))
def listener_callback(self, data):
self.get_logger().info("Received an image! ")
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
self.timer.start()
boxes, labels, probs = self.predictor.predict(image, 10, 0.4)
interval = self.timer.end()
print('Time: {:.2f}s, Detect Objects: {:d}.'.format(
interval, labels.size(0)))
detection_array = Detection2DArray()
for i in range(boxes.size(0)):
box = boxes[i, :]
label = f"{self.class_names[labels[i]]}: {probs[i]:.2f}"
print("Object: " + str(i) + " " + label)
cv2.rectangle(cv_image, (box[0], box[1]), (box[2], box[3]),
(255, 255, 0), 4)
# Definition of 2D array message and ading all object stored in it.
object_hypothesis_with_pose = ObjectHypothesisWithPose()
object_hypothesis_with_pose.id = str(self.class_names[labels[i]])
object_hypothesis_with_pose.score = float(probs[i])
bounding_box = BoundingBox2D()
bounding_box.center.x = float((box[0] + box[2]) / 2)
bounding_box.center.y = float((box[1] + box[3]) / 2)
bounding_box.center.theta = 0.0
bounding_box.size_x = float(2 * (bounding_box.center.x - box[0]))
bounding_box.size_y = float(2 * (bounding_box.center.y - box[1]))
detection = Detection2D()
detection.header = data.header
detection.results.append(object_hypothesis_with_pose)
detection.bbox = bounding_box
detection_array.header = data.header
detection_array.detections.append(detection)
cv2.putText(
cv_image,
label,
(box[0] + 20, box[1] + 40),
cv2.FONT_HERSHEY_SIMPLEX,
1, # font scale
(255, 0, 255),
2) # line type
# Displaying the predictions
cv2.imshow('object_detection', cv_image)
# Publishing the results onto the the Detection2DArray vision_msgs format
self.detection_publisher.publish(detection_array)
ros_image = self.bridge.cv2_to_imgmsg(cv_image)
ros_image.header.frame_id = 'camera_frame'
self.result_publisher.publish(ros_image)
cv2.waitKey(1)
def detect(self, image):
# Convert to grayscale if needed
if image.ndim == 3:
image = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
image_height, image_width = image.shape
image_area = image_height * image_width
# Apply (inverse) binary threshold to input image
mask = cv.threshold(image, THRESHOLD, THRESHOLD_MAX,
cv.THRESH_BINARY_INV)[1]
# Dilate mask to find more reliable contours
# kernel = np.ones((5, 5), np.uint8)
# mask_dilated = cv.dilate(mask, kernel, iterations=1)
# Find external approximate contours in dilated mask
contours, hierarchy = cv.findContours(mask, cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_SIMPLE)
# Filter out contours that don't qualify as a detection
detections = []
for contour in contours:
# Filer out if the contour touches the image border
x, y, w, h = cv.boundingRect(contour)
if x == 0 or y == 0 or x + w == image_width or y + h == image_height:
continue
# Filter out if the contour is too small
if cv.contourArea(contour) < 1e-4 * image_area:
continue
detections.append((x, y, w, h))
# Fill detections msg
detection_array_msg = Detection2DArray()
for detection in detections:
x, y, w, h = detection
center_x = x + w / 2.
center_y = y + h / 2.
bbox = BoundingBox2D()
bbox.center = Pose2D(x=center_x, y=center_y, theta=0)
bbox.size_x = w
bbox.size_y = h
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = 0
object_hypothesis.score = 1.0
detection_msg = Detection2D()
detection_msg.bbox = bbox
detection_msg.results.append(object_hypothesis)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
def _handle_yolo_detect(self, req):
cv_image = None
detection_array = Detection2DArray()
detections = []
boxes = None
try:
cv_image = self.bridge.imgmsg_to_cv2(req.image, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
try:
boxes = self.yolo.predict(cv_image)
except SystemError:
pass
# rospy.loginfo('Found {} boxes'.format(len(boxes)))
for box in boxes:
detection = Detection2D()
results = []
bbox = BoundingBox2D()
center = Pose2D()
detection.header = Header()
detection.header.stamp = rospy.get_rostime()
# detection.source_img = deepcopy(req.image)
labels = box.get_all_labels()
for i in range(0,len(labels)):
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = i
object_hypothesis.score = labels[i]
results.append(object_hypothesis)
detection.results = results
x, y = box.get_xy_center()
center.x = x
center.y = y
center.theta = 0.0
bbox.center = center
size_x, size_y = box.get_xy_extents()
bbox.size_x = size_x
bbox.size_y = size_y
detection.bbox = bbox
detections.append(detection)
detection_array.header = Header()
detection_array.header.stamp = rospy.get_rostime()
detection_array.detections = detections
return YoloDetectResponse(detection_array)
def got_image(self, rgb_msg):
color_image = self._bridge.imgmsg_to_cv2(rgb_msg)
if self._is_first_frame and self._inital_bbox is not None:
rospy.loginfo("Initializing tracker")
self._last_bbox = self._inital_bbox
self._is_first_frame = False
self.prev_img = color_image
elif not self._is_first_frame:
self.model.eval()
self.curr_img = color_image
sample = self.transform(self._get_sample())
self._last_bbox = self.get_rect(sample)
self.prev_img = self.curr_img
if self._last_bbox is not None:
bbox_message = BoundingBox2D()
bbox_message.size_x = self._last_bbox[2]
bbox_message.size_y = self._last_bbox[3]
bbox_message.center.theta = 0
bbox_message.center.x = self._last_bbox[0] + self._last_bbox[2] / 2
bbox_message.center.y = self._last_bbox[1] + self._last_bbox[3] / 2
self._pub_bbox.publish(bbox_message)
status_message = Int8()
status_message.data = self._current_status
self._pub_status.publish(status_message)
if self.publish_result_img:
self._last_bbox = tuple([int(i) for i in self._last_bbox])
if self._current_status == 1:
cv2.rectangle(color_image,
(self._last_bbox[0], self._last_bbox[1]),
(self._last_bbox[0] + self._last_bbox[2],
self._last_bbox[1] + self._last_bbox[3]),
(0, 0, 255), 2)
else:
cv2.rectangle(color_image,
(self._last_bbox[0], self._last_bbox[1]),
(self._last_bbox[0] + self._last_bbox[2],
self._last_bbox[1] + self._last_bbox[3]),
(255, 0, 0), 2)
imgmsg = self._bridge.cv2_to_imgmsg(color_image,
encoding="mono8")
self._pub_result_img.publish(imgmsg)
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
if cols > 60 and rows > 60:
cv2.circle(cv_image, (50, 50), 10, 255)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
try:
small_image = cv2.resize(cv_image, None, fx=0.5, fy=0.5)
hsv = cv2.cvtColor(small_image, cv2.COLOR_BGR2HSV)
lower_color = np.array([self.lower_h, self.lower_s, self.lower_v])
upper_color = np.array(
[self.higher_h, self.higher_s, self.higher_v])
mask = cv2.inRange(hsv, lower_color, upper_color)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = None
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
if radius > 10:
bounding_box_msg = BoundingBox2D()
cv2.circle(mask, (int(x), int(y)), int(radius), 100, 2)
cv2.circle(mask, center, 5, 250, -1)
x, y, w, h = cv2.boundingRect(c)
cv2.rectangle(mask, (x, y), (x + w, y + h), 200, 10)
bounding_box_msg.center.x = int(x)
bounding_box_msg.center.y = int(y)
bounding_box_msg.size_x = int(w)
bounding_box_msg.size_y = int(h)
self.bounding_box_pub.publish(bounding_box_msg)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(mask, "mono8"))
except CvBridgeError as e:
print(e)
def build_bounding_box_2d(candidate):
"""
Builds a BoundingBox2D message out of a vision Candidate
:param candidate: A vision Candidate
:return: BoundingBox2D message
"""
center = Pose2D()
center.position.x = float(candidate.get_center_x())
center.position.y = float(candidate.get_center_y())
bb_msg = BoundingBox2D()
bb_msg.size_x = float(candidate.get_width())
bb_msg.size_y = float(candidate.get_height())
bb_msg.center = center
return bb_msg
def track_video(self, rgb_msg):
color_image = self._bridge.imgmsg_to_cv2(rgb_msg)
if self._is_first_frame and self._inital_bbox is not None: # init
print('init')
target_pos, target_sz = rect_2_cxy_wh(self._inital_bbox)
self.state = SiamRPN_init(color_image, target_pos, target_sz,
self.model, device) # init tracker
location = cxy_wh_2_rect(self.state['target_pos'],
self.state['target_sz'])
self._last_bbox = self._inital_bbox
self._is_first_frame = False
elif not self._is_first_frame: # tracking
self.state = SiamRPN_track(self.state, color_image,
device) # track
location = cxy_wh_2_rect(self.state['target_pos'] + 1,
self.state['target_sz'])
self._last_bbox = rect_2_cxy_wh(location)
if self._last_bbox is not None:
bbox_message = BoundingBox2D()
bbox_message.size_x = self._last_bbox[1][0]
bbox_message.size_y = self._last_bbox[1][1]
bbox_message.center.theta = 0
bbox_message.center.x = self._last_bbox[0][
0] + self._last_bbox[1][0] / 2
bbox_message.center.y = self._last_bbox[0][
1] + self._last_bbox[1][1] / 2
self._pub_bbox.publish(bbox_message)
status_message = Int8()
status_message.data = self._current_status
self._pub_status.publish(status_message)
location = [int(l) for l in location] #
cv2.rectangle(
color_image, (location[0], location[1]),
(location[0] + location[2], location[1] + location[3]),
(0, 255, 255), 3)
cv2.putText(color_image, str("DaSiamRPN"), (40, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
imgmsg = self._bridge.cv2_to_imgmsg(color_image, encoding="mono8")
self._pub_result_img.publish(imgmsg)
def get_faces(raw_img):
bridge = CvBridge()
cv2_img = bridge.imgmsg_to_cv2(raw_img, "8UC3")
faces = detect_faces((cv2_img))
face_pub = rospy.Publisher("face_publisher", BoundingBox2D, queue_size=1)
rate = rospy.Rate(60) #fps
if len(faces) > 0: #stupid stupid line of stupid code STUPIDDDD
rospy.loginfo(faces)
for (x, y, w, h) in faces:
centered = Pose2D((x + w) / 2, (y + h) / 2, 0.0)
bbox = BoundingBox2D(center=centered, size_x=w, size_y=h)
face_pub.publish(bbox)
else:
rospy.loginfo("No faces detected")
rate.sleep()
def corners_to_detection_2d(corners):
# Find min/max bounding box points
min_x = corners[:, 0].min()
max_x = corners[:, 0].max()
min_y = corners[:, 1].min()
max_y = corners[:, 1].max()
# Find size of bounding box in x and y direction
size_x = max_x - min_x
size_y = max_y - min_y
# Find center coordinates
center_x = min_x + (size_x / 2.0)
center_y = min_y + (size_y / 2.0)
# Construct detection message
center = Pose2D(x=center_x, y=center_y)
bounding_box = BoundingBox2D(center=center, size_x=size_x, size_y=size_y)
return Detection2D(bbox=bounding_box)
def callback(self, data):
bridge = CvBridge()
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(
data, desired_encoding="passthrough")
except CvBridgeError as e:
rospy.logerr(e)
results = self.engine.DetectWithImage(PIL.Image.fromarray(cv_image),
top_k=1,
threshold=self.threshold,
keep_aspect_ratio=True,
relative_coord=True)
detections = Detection2DArray()
now = rospy.get_rostime()
for detection in results:
top_left, bottom_right = detection.bounding_box
min_x, min_y = top_left
max_x, max_y = bottom_right
imheight, imwidth, _ = cv_image.shape
min_x *= imwidth
max_x *= imwidth
min_y *= imheight
max_y *= imheight
centre_x = (max_x + min_x) / 2.0
centre_y = (max_y + min_y) / 2.0
height = max_y - min_y
width = max_x - min_x
if height <= 0 or width <= 0:
continue
bbox = BoundingBox2D()
bbox.center.x = centre_x
bbox.center.y = centre_y
bbox.size_x = width
bbox.size_y = height
hypothesis = ObjectHypothesisWithPose()
# hypothesis.id = str(detection.label_id)
hypothesis.score = detection.score
hypothesis.pose.pose.position.x = centre_x
hypothesis.pose.pose.position.y = centre_y
# update the timestamp of the object
object = Detection2D()
object.header.stamp = now
object.header.frame_id = data.header.frame_id
object.results.append(hypothesis)
object.bbox = bbox
object.source_img.header.frame_id = data.header.frame_id
object.source_img.header.stamp = now
object.source_img.height = int(height)
object.source_img.width = int(width)
object.source_img.encoding = "rgb8"
object.source_img.step = int(width * 3)
# object.source_img.data = cv_image[int(min_y):int(max_y), int(min_x):int(max_x)].tobytes()
detections.detections.append(object)
if len(results) > 0:
self.detection_pub.publish(detections)
rospy.logdebug("%.2f ms" % self.engine.get_inference_time())
def got_image(self, rgb_msg, depth_msg):
depth_image = self._bridge.imgmsg_to_cv2(depth_msg,
desired_encoding="32FC1")
color_image = self._bridge.imgmsg_to_cv2(rgb_msg)
final_bbox = None
if self._is_first_frame and self._inital_bbox is not None:
rospy.loginfo("Initializing tracker")
current_bbox = self._inital_bbox
bbox_center = self.calculate_bbox_center(current_bbox)
if depth_image[bbox_center[1]][bbox_center[0]] > 0:
if self._original_distance == -1:
self._original_distance = depth_image[bbox_center[1]][
bbox_center[0]]
current_distance = depth_image[bbox_center[1]][bbox_center[0]]
self._tracker.init(color_image, current_bbox)
self._is_first_frame = False
final_bbox = current_bbox
elif not self._is_first_frame:
width = int(color_image.shape[1] * self._scale)
height = int(color_image.shape[0] * self._scale)
color_image = cv2.resize(color_image, (width, height))
depth_image = cv2.resize(depth_image, (width, height))
if self._has_scale_changed:
self._has_scale_changed = False
self._tracker = cv2.TrackerCSRT_create()
if self._scale == self._fallback_scale:
bbox_scaled = tuple(
[self._scale * i for i in self._last_bbox])
self._inital_bbox = self.scale_bbox(
self._inital_bbox, self._scale)
self._tracker.init(color_image, bbox_scaled)
self.tracker_suggested_bbox = bbox_scaled
else:
bbox_scaled = tuple([
self._scale / self._fallback_scale * i
for i in self._last_bbox
])
self._inital_bbox = self.scale_bbox(
self._inital_bbox, self._scale / self._fallback_scale)
self._tracker.init(color_image, bbox_scaled)
self.tracker_suggested_bbox = bbox_scaled
ok = True
else:
ok, self.tracker_suggested_bbox = self._tracker.update(
color_image)
if ok:
bbox_center = self.calculate_bbox_center(
self.tracker_suggested_bbox)
split_array = np.array([
int(self.tracker_suggested_bbox[0]),
int(self.tracker_suggested_bbox[0] +
self.tracker_suggested_bbox[2]),
])
split_array = [0 if x < 0 else x for x in split_array]
bbox_content = np.hsplit(
depth_image[int(self.tracker_suggested_bbox[1]
):int(self.tracker_suggested_bbox[1] +
self.tracker_suggested_bbox[3])],
split_array,
)[1]
mask = (bbox_content >=
(self.min_depth * self.depth_scale_factor)) & (
bbox_content <=
(self.max_depth * self.depth_scale_factor)) & (
bbox_content != 9.0)
median = np.nanmedian(bbox_content[mask])
if self.publish_median_depth:
bbox_depth_msg = Float32()
bbox_depth_msg.data = median / self.depth_scale_factor
self._pub_median_depth.publish(bbox_depth_msg)
if median > 0:
if self._original_distance == -1:
self._original_distance = median
current_distance = median
depth_scale = self._original_distance / current_distance
tracker_scale = self.get_bbox_scale(
self._inital_bbox, self.tracker_suggested_bbox)
scaled_tracker_bbox = self.scale_bbox(
self.tracker_suggested_bbox,
depth_scale / tracker_scale)
scaled_tracker_bbox = tuple(
[int(x) for x in scaled_tracker_bbox])
final_bbox = scaled_tracker_bbox
elif np.isnan(median):
self._current_status = 0
self.tracker_suggested_bbox = [
int(x) for x in self.tracker_suggested_bbox
]
self.tracker_suggested_bbox = (
self.tracker_suggested_bbox[0],
self.tracker_suggested_bbox[1],
self.tracker_suggested_bbox[2],
self.tracker_suggested_bbox[3],
)
if final_bbox is None:
final_bbox = self.tracker_suggested_bbox
if final_bbox is not None:
self._last_bbox = final_bbox
width_ratio = float(final_bbox[2]) / float(color_image.shape[1])
height_ratio = float(final_bbox[3]) / float(color_image.shape[0])
if (width_ratio > self.max_bbox_ratio
or height_ratio > self.max_bbox_ratio
) and self._scale != self._fallback_scale:
rospy.loginfo("Scaling down...")
self._scale = self._fallback_scale
self._has_scale_changed = True
elif (width_ratio < self.max_bbox_ratio and height_ratio <
self.max_bbox_ratio) and self._scale == self._fallback_scale:
rospy.loginfo("Scaling back up...")
self._scale = 1.0
self._has_scale_changed = True
center = self.calculate_bbox_center(final_bbox)
if self.check_point_oob(center, color_image, oob_threshold):
self._current_status = 0
bbox_message = BoundingBox2D()
bbox_message.size_x = final_bbox[2]
bbox_message.size_y = final_bbox[3]
bbox_message.center.theta = 0
bbox_message.center.x = final_bbox[0] + final_bbox[2] / 2
bbox_message.center.y = final_bbox[1] + final_bbox[3] / 2
self._pub_bbox.publish(bbox_message)
status_message = Int8()
status_message.data = self._current_status
self._pub_status.publish(status_message)
if self.publish_result_img:
final_bbox = tuple([int(i) for i in final_bbox])
if self._current_status == 1:
cv2.rectangle(color_image, final_bbox, (0, 0, 255), 2)
else:
cv2.rectangle(color_image, final_bbox, (255, 0, 0), 2)
cv2.circle(color_image, center, 3, (255, 0, 0), 2)
imgmsg = self._bridge.cv2_to_imgmsg(color_image,
encoding="rgb8")
self._pub_result_img.publish(imgmsg)
def osd_sink_pad_buffer_probe(self,pad,info,u_data):
frame_number=0
#Intializing object counter with 0.
obj_counter = {
PGIE_CLASS_ID_VEHICLE:0,
PGIE_CLASS_ID_BICYCLE:0,
PGIE_CLASS_ID_PERSON:0,
PGIE_CLASS_ID_ROADSIGN:0
}
num_rects=0
gst_buffer = info.get_buffer()
if not gst_buffer:
print("Unable to get GstBuffer ")
return
# Retrieve batch metadata from the gst_buffer
# Note that pyds.gst_buffer_get_nvds_batch_meta() expects the
# C address of gst_buffer as input, which is obtained with hash(gst_buffer)
batch_meta = pyds.gst_buffer_get_nvds_batch_meta(hash(gst_buffer))
l_frame = batch_meta.frame_meta_list
while l_frame is not None:
try:
# Note that l_frame.data needs a cast to pyds.NvDsFrameMeta
# The casting is done by pyds.NvDsFrameMeta.cast()
# The casting also keeps ownership of the underlying memory
# in the C code, so the Python garbage collector will leave
# it alone.
frame_meta = pyds.NvDsFrameMeta.cast(l_frame.data)
except StopIteration:
break
frame_number=frame_meta.frame_num
num_rects = frame_meta.num_obj_meta
l_obj=frame_meta.obj_meta_list
# Message for output of detection inference
msg = Detection2DArray()
while l_obj is not None:
try:
# Casting l_obj.data to pyds.NvDsObjectMeta
obj_meta=pyds.NvDsObjectMeta.cast(l_obj.data)
l_classifier = obj_meta.classifier_meta_list
# If object is a car (class ID 0), perform attribute classification
if obj_meta.class_id == 0 and l_classifier is not None:
# Creating and publishing message with output of classification inference
msg2 = Classification2D()
while l_classifier is not None:
result = ObjectHypothesis()
try:
classifier_meta = pyds.glist_get_nvds_classifier_meta(l_classifier.data)
except StopIteration:
print('Could not parse MetaData: ')
break
classifier_id = classifier_meta.unique_component_id
l_label = classifier_meta.label_info_list
label_info = pyds.glist_get_nvds_label_info(l_label.data)
classifier_class = label_info.result_class_id
if classifier_id == 2:
result.id = class_color[classifier_class]
elif classifier_id == 3:
result.id = class_make[classifier_class]
else:
result.id = class_type[classifier_class]
result.score = label_info.result_prob
msg2.results.append(result)
l_classifier = l_classifier.next
self.publisher_classification.publish(msg2)
except StopIteration:
break
obj_counter[obj_meta.class_id] += 1
# Creating message for output of detection inference
result = ObjectHypothesisWithPose()
result.id = str(class_obj[obj_meta.class_id])
result.score = obj_meta.confidence
left = obj_meta.rect_params.left
top = obj_meta.rect_params.top
width = obj_meta.rect_params.width
height = obj_meta.rect_params.height
bounding_box = BoundingBox2D()
bounding_box.center.x = float(left + (width/2))
bounding_box.center.y = float(top - (height/2))
bounding_box.size_x = width
bounding_box.size_y = height
detection = Detection2D()
detection.results.append(result)
detection.bbox = bounding_box
msg.detections.append(detection)
try:
l_obj=l_obj.next
except StopIteration:
break
# Publishing message with output of detection inference
self.publisher_detection.publish(msg)
# Acquiring a display meta object. The memory ownership remains in
# the C code so downstream plugins can still access it. Otherwise
# the garbage collector will claim it when this probe function exits.
display_meta=pyds.nvds_acquire_display_meta_from_pool(batch_meta)
display_meta.num_labels = 1
py_nvosd_text_params = display_meta.text_params[0]
# Setting display text to be shown on screen
# Note that the pyds module allocates a buffer for the string, and the
# memory will not be claimed by the garbage collector.
# Reading the display_text field here will return the C address of the
# allocated string. Use pyds.get_string() to get the string content.
py_nvosd_text_params.display_text = "Frame Number={} Number of Objects={} Vehicle_count={} Person_count={}".format(frame_number, num_rects, obj_counter[PGIE_CLASS_ID_VEHICLE], obj_counter[PGIE_CLASS_ID_PERSON])
# Now set the offsets where the string should appear
py_nvosd_text_params.x_offset = 10
py_nvosd_text_params.y_offset = 12
# Font , font-color and font-size
py_nvosd_text_params.font_params.font_name = "Serif"
py_nvosd_text_params.font_params.font_size = 10
# set(red, green, blue, alpha); set to White
py_nvosd_text_params.font_params.font_color.set(1.0, 1.0, 1.0, 1.0)
# Text background color
py_nvosd_text_params.set_bg_clr = 1
# set(red, green, blue, alpha); set to Black
py_nvosd_text_params.text_bg_clr.set(0.0, 0.0, 0.0, 1.0)
# Using pyds.get_string() to get display_text as string
pyds.nvds_add_display_meta_to_frame(frame_meta, display_meta)
try:
l_frame=l_frame.next
except StopIteration:
break
return Gst.PadProbeReturn.OK
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and OBJECTS detected'''
#### direct conversion to CV2 ####
original_img = self.bridge.imgmsg_to_cv2(ros_data,
desired_encoding="bgr8")
print(original_img.shape)
to_square = original_img.shape[1] - original_img.shape[0]
cv_image = cv2.copyMakeBorder(original_img, 0, to_square, 0, 0,
cv2.BORDER_CONSTANT)
#
#cv_image = cv2.resize(original_img, (640, 640), interpolation = cv2.INTER_AREA)
# Uncomment thefollowing block in order to collect training data
#cv2.imwrite("/home/atas/MASKRCNN_REAL_DATASET/" + str(self.counter) + ".png", original_img)
#self.counter = self.counter + 1
#sec = input('PRESS KEY FOR NEXT.\n')
cuda_tensor_of_original_image = image_loader(cv_image)
# Get detections
with torch.no_grad():
detections = model(cuda_tensor_of_original_image)
detections = non_max_suppression(detections, opt.conf_thres,
opt.nms_thres)
detection_array = Detection2DArray()
if len(detections) > 0:
# Rescale boxes to original image
#detections = rescale_boxes(detections, opt.img_size, cv_image.shape[:2])
# print(detections)
for x1, y1, x2, y2, conf, cls_conf, cls_pred in detections[0]:
#print("\t+ Label: %s, Conf: %.5f" % (classes[int(cls_pred)], cls_conf.item()))
k = original_img.shape[1] / imsize
# Create a Rectangle patch
cv2.rectangle(
original_img, (x1 * k, y1 * k), (x2 * k, y2 * k),
(random.randint(0, 255), random.randint(0, 255), 55), 2)
bbx = BoundingBox2D()
bbx.center.x = (x1 + x2) / 2 * k
bbx.center.y = (y1 + y2) / 2 * k
bbx.center.theta = 0
bbx.size_x = (x2 - x1) * k
bbx.size_y = (y2 - y1) * k
bbx_for_this_detection = Detection2D()
bbx_for_this_detection.header.stamp = rospy.Time.now()
bbx_for_this_detection.bbox = bbx
detection_array.detections.append(bbx_for_this_detection)
detection_array.header.stamp = rospy.Time.now()
self.yolo_detection_pub.publish(detection_array)
# Add the bbox to the plot
# Add label
#### PUBLISH SEGMENTED IMAGE ####
msg = self.bridge.cv2_to_imgmsg(original_img, "bgr8")
msg.header.stamp = rospy.Time.now()
self.image_pub.publish(msg)
self.counter += 1
if (time.time() - self.start_time) > self.x:
print("FPS: ", self.counter / (time.time() - self.start_time))
self.counter = 0
self.start_time = time.time()
def callback(self, imageMsg):
image = self.bridge.imgmsg_to_cv2(imageMsg, "bgr8")
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
image = image[:, :, ::-1].copy()
# copy to draw on
draw = image.copy()
draw = cv2.cvtColor(draw, cv2.COLOR_BGR2RGB)
# Image formatting specific to Retinanet
image = preprocess_image(image)
image, scale = resize_image(image)
# Run the inferencer
try:
with self.session.as_default():
with self.session.graph.as_default():
boxes, scores, labels = self.model.predict_on_batch(np.expand_dims(image, axis=0))
except Exception as e:
rospy.logerr(e)
rospy.logwarn("WARNING: Has your model been converted to an inference model yet? "
"see https://github.com/fizyr/keras-retinanet#converting-a-training-model-to-inference-model")
return
# correct for image scale
boxes /= scale
# Construct the detection message
header = Header(frame_id=imageMsg.header.frame_id)
detections_message_out = Detection2DArray()
detections_message_out.header = header
detections_message_out.detections = []
# visualize detections
for box, score, label in zip(boxes[0], scores[0], labels[0]):
# scores are sorted so we can break
if score < self.confidence_cutoff:
break
# Add boxes and captions
b = np.array(box).astype(int)
cv2.rectangle(draw, (b[0], b[1]), (b[2], b[3]), self.color, self.thickness, cv2.LINE_AA)
if (label > len(self.labels_to_names)):
print("WARNING: Got unknown label, using 'detection' instead")
caption = "Detection {:.3f}".format(score)
else:
caption = "{} {:.3f}".format(self.labels_to_names[label], score)
cv2.putText(draw, caption, (b[0], b[1] - 10), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 0), 2)
cv2.putText(draw, caption, (b[0], b[1] - 10), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1)
#Construct the output detection message
bb = BoundingBox2D()
det = Detection2D(header=header)
hyp = ObjectHypothesisWithPose()
center = Pose2D()
hyp.id = label
hyp.score = score
bb.size_x = b[2] - b[0]
bb.size_y = b[3] - b[1]
center.x = float(b[2] + b[0])/2
center.y = float(b[3] + b[1])/2
bb.center = center
det.results.append(hyp)
det.bbox = bb
detections_message_out.detections.append(det)
self.detections_pub.publish(detections_message_out)
# Write out image
image_message_out = self.bridge.cv2_to_imgmsg(draw, encoding="rgb8")
self.img_pub.publish(image_message_out)
label_list = labels[index]
except KeyError:
print('Image {} has no label. Skipping.'.format(index),
file=sys.stderr)
for label in label_list:
_, center_x, center_y, width, height = label
# Scale by image height and width
center_x *= image_width
center_y *= image_height
width *= image_width
height *= image_height
center = Pose2D(x=center_x, y=center_y)
bbox = BoundingBox2D(center=center, size_x=width, size_y=height)
detection = Detection2D(bbox=bbox)
detection_array_msg.detections.append(detection)
outbag.write(args.detection_topic, detection_array_msg, t)
index += 1
# Write everything to outbag
outbag.write(topic, msg, t)
bag.close()
outbag.close()
def __init__(self):
self.current_yaw = 0
self.initial_yaw = 0
self.gate_pos = BoundingBox2D()
self.flare_pos = BoundingBox2D()
def detect_multi(self, images):
image_shape = images[0].shape
images_list = []
for image in images:
image = cv.resize(image,
self.image_size[::-1]) # cv uses width x height!
image = image[..., np.newaxis]
if not self.grayscale and image.shape[-1] == 1:
image = cv.cvtColor(image, cv.COLOR_GRAY2RGB)
image_torch = self.transform(image)[np.newaxis, ...]
images_list.append(image_torch)
images_torch = torch.cat(images_list, 0)
if self.half:
images_torch = images_torch.half()
if self.has_gpu and self.use_gpu:
images_torch = images_torch.cuda()
with torch.no_grad():
# Raw detection is a (N x 8190 x num_classes) tensor
start_time_inf = self.time_synchronized()
raw_detections, _ = self.model(images_torch)
elapsed_time_inf = self.time_synchronized() - start_time_inf
if self.verbose:
print('time (inf): {:.4f}s'.format(elapsed_time_inf))
if self.half:
raw_detections = raw_detections.float()
start_time_nms = self.time_synchronized()
detections_torch = self.non_max_suppression(
raw_detections.float(),
conf_thres=self.confidence_threshold,
iou_thres=self.iou_threshold)
elapsed_time_nms = self.time_synchronized() - start_time_nms
if self.verbose:
print('time (nms): {:.4f}s'.format(elapsed_time_nms))
if self.verbose:
print('time (tot): {:.4f}s\n'.format(elapsed_time_inf +
elapsed_time_nms))
detection_array_list = []
for detection_torch in detections_torch:
detection_array_msg = Detection2DArray()
if detection_torch is not None:
# Rescale bounding boxes back to original (old) image size
# shape_old: Shape of images as they come in
# shape_new: Shape of images used for inference
detection_torch = rescale_boxes(detection_torch,
shape_old=image_shape[:2],
shape_new=self.image_size)
detections = detection_torch.cpu().numpy()
for i, detection in enumerate(detections):
x1, y1, x2, y2, object_conf, class_pred = detection
class_id = int(class_pred)
# class_name = self.classes[class_id]
size_x = (x2 - x1)
size_y = (y2 - y1)
x = x1 + (size_x / 2.0)
y = y1 + (size_y / 2.0)
bbox = BoundingBox2D()
bbox.center = Pose2D(x=x, y=y, theta=0)
bbox.size_x = size_x
bbox.size_y = size_y
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = class_id
object_hypothesis.score = object_conf
detection_msg = Detection2D()
detection_msg.bbox = bbox
detection_msg.results.append(object_hypothesis)
detection_array_msg.detections.append(detection_msg)
detection_array_list.append(detection_array_msg)
return detection_array_list
def tiler_sink_pad_buffer_probe(self, pad, info, u_data):
frame_number = 0
num_rects = 0
gst_buffer = info.get_buffer()
if not gst_buffer:
print("Unable to get GstBuffer ")
return
# Retrieve batch metadata from the gst_buffer
# Note that pyds.gst_buffer_get_nvds_batch_meta() expects the
# C address of gst_buffer as input, which is obtained with hash(gst_buffer)
batch_meta = pyds.gst_buffer_get_nvds_batch_meta(hash(gst_buffer))
l_frame = batch_meta.frame_meta_list
while l_frame is not None:
try:
# Note that l_frame.data needs a cast to pyds.NvDsFrameMeta
# The casting is done by pyds.NvDsFrameMeta.cast()
# The casting also keeps ownership of the underlying memory
# in the C code, so the Python garbage collector will leave
# it alone.
frame_meta = pyds.NvDsFrameMeta.cast(l_frame.data)
except StopIteration:
break
frame_number = frame_meta.frame_num
l_obj = frame_meta.obj_meta_list
num_rects = frame_meta.num_obj_meta
is_first_obj = True
save_image = False
obj_counter = {
PGIE_CLASS_ID_VEHICLE: 0,
PGIE_CLASS_ID_BICYCLE: 0,
PGIE_CLASS_ID_PERSON: 0,
PGIE_CLASS_ID_ROADSIGN: 0
}
# Message for output of detection inference
msg = Detection2DArray()
while l_obj is not None:
try:
# Casting l_obj.data to pyds.NvDsObjectMeta
obj_meta = pyds.NvDsObjectMeta.cast(l_obj.data)
l_classifier = obj_meta.classifier_meta_list
# If object is a car (class ID 0), perform attribute classification
if obj_meta.class_id == 0 and l_classifier is not None:
# Creating and publishing message with output of classification inference
msg2 = Classification2D()
while l_classifier is not None:
result = ObjectHypothesis()
try:
classifier_meta = pyds.glist_get_nvds_classifier_meta(
l_classifier.data)
except StopIteration:
print('Could not parse MetaData: ')
break
classifier_id = classifier_meta.unique_component_id
l_label = classifier_meta.label_info_list
label_info = pyds.glist_get_nvds_label_info(
l_label.data)
classifier_class = label_info.result_class_id
if classifier_id == 2:
result.id = class_color[classifier_class]
elif classifier_id == 3:
result.id = class_make[classifier_class]
else:
result.id = class_type[classifier_class]
result.score = label_info.result_prob
msg2.results.append(result)
l_classifier = l_classifier.next
self.publisher_classification.publish(msg2)
except StopIteration:
break
obj_counter[obj_meta.class_id] += 1
# Creating message for output of detection inference
result = ObjectHypothesisWithPose()
result.id = str(class_obj[obj_meta.class_id])
result.score = obj_meta.confidence
left = obj_meta.rect_params.left
top = obj_meta.rect_params.top
width = obj_meta.rect_params.width
height = obj_meta.rect_params.height
bounding_box = BoundingBox2D()
bounding_box.center.x = float(left + (width / 2))
bounding_box.center.y = float(top - (height / 2))
bounding_box.size_x = width
bounding_box.size_y = height
detection = Detection2D()
detection.results.append(result)
detection.bbox = bounding_box
msg.detections.append(detection)
# Periodically check for objects with borderline confidence value that may be false positive detections.
# If such detections are found, annotate the frame with bboxes and confidence value.
# Save the annotated frame to file.
if ((saved_count["stream_" + str(frame_meta.pad_index)] % 30
== 0) and (obj_meta.confidence > 0.3
and obj_meta.confidence < 0.31)):
if is_first_obj:
is_first_obj = False
# Getting Image data using nvbufsurface
# the input should be address of buffer and batch_id
n_frame = pyds.get_nvds_buf_surface(
hash(gst_buffer), frame_meta.batch_id)
#convert python array into numy array format.
frame_image = np.array(n_frame, copy=True, order='C')
#covert the array into cv2 default color format
frame_image = cv2.cvtColor(frame_image,
cv2.COLOR_RGBA2BGRA)
save_image = True
frame_image = draw_bounding_boxes(frame_image, obj_meta,
obj_meta.confidence)
try:
l_obj = l_obj.next
except StopIteration:
break
# Get frame rate through this probe
fps_streams["stream{0}".format(frame_meta.pad_index)].get_fps()
# Publishing message with output of detection inference
self.publisher_detection.publish(msg)
if save_image:
cv2.imwrite(
folder_name + "/stream_" + str(frame_meta.pad_index) +
"/frame_" + str(frame_number) + ".jpg", frame_image)
saved_count["stream_" + str(frame_meta.pad_index)] += 1
try:
l_frame = l_frame.next
except StopIteration:
break
return Gst.PadProbeReturn.OK
def got_image(self, rgb_msg):
color_image = self._bridge.imgmsg_to_cv2(rgb_msg, 'bgr8')
final_bbox = None
if self._is_first_frame and self._inital_bbox is not None:
rospy.loginfo("Initializing tracker")
current_bbox = self._inital_bbox
bbox_center = self.calculate_bbox_center(current_bbox)
self._tracker.init(color_image, current_bbox)
self._is_first_frame = False
final_bbox = current_bbox
elif not self._is_first_frame:
ok, self.tracker_suggested_bbox = self._tracker.update(color_image)
if ok:
final_bbox = self.tracker_suggested_bbox
else:
self._current_status = 0
status_message = Int8()
status_message.data = self._current_status
print("Status", self._current_status)
self._pub_status.publish(status_message)
if final_bbox is not None:
self._last_bbox = final_bbox
width_ratio = float(final_bbox[2]) / float(color_image.shape[1])
height_ratio = float(final_bbox[3]) / float(color_image.shape[0])
if (width_ratio > self.max_bbox_ratio
or height_ratio > self.max_bbox_ratio
) and self._scale != self._fallback_scale:
rospy.loginfo("Scaling down...")
self._scale = self._fallback_scale
self._has_scale_changed = True
elif (width_ratio < self.max_bbox_ratio and height_ratio <
self.max_bbox_ratio) and self._scale == self._fallback_scale:
rospy.loginfo("Scaling back up...")
self._scale = 1.0
self._has_scale_changed = True
center = self.calculate_bbox_center(final_bbox)
if self.check_point_oob(center, color_image, self.oob_threshold):
self._current_status = 0
bbox_message = BoundingBox2D()
bbox_message.size_x = final_bbox[2]
bbox_message.size_y = final_bbox[3]
bbox_message.center.theta = 0
bbox_message.center.x = final_bbox[0] + final_bbox[2] / 2
bbox_message.center.y = final_bbox[1] + final_bbox[3] / 2
self._pub_bbox.publish(bbox_message)
status_message = Int8()
status_message.data = self._current_status
self._pub_status.publish(status_message)
if self.publish_result_img:
final_bbox = tuple([int(i) for i in final_bbox])
if self._current_status == 1:
cv2.rectangle(color_image, (final_bbox[0], final_bbox[1]),
(final_bbox[0] + final_bbox[2],
final_bbox[1] + final_bbox[3]), (0, 0, 255),
2)
else:
cv2.rectangle(color_image, (final_bbox[0], final_bbox[1]),
(final_bbox[0] + final_bbox[2],
final_bbox[1] + final_bbox[3]), (255, 0, 0),
2)
cv2.circle(color_image, center, 3, (255, 0, 0), 2)
imgmsg = self._bridge.cv2_to_imgmsg(color_image,
encoding="mono8")
self._pub_result_img.publish(imgmsg)