def object_predict(self, object_data, header, image_np, image, offset_x,
offset_y):
image_height, image_width, channels = image.shape
obj = Detection2D()
obj_hypothesis = ObjectHypothesisWithPose()
object_id = object_data[0]
object_score = object_data[1]
dimensions = object_data[2]
# TODO - Need to make it so that the size and offset of the bounding box is corrected to the original
# 320 x 240 image. Center x and center y need to be tested, while size x and size y need to be fixed
obj.header = header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2] - dimensions[0]) * image_height)
obj.bbox.size_x = int((dimensions[3] - dimensions[1]) * image_width)
obj.bbox.center.x = int(
(dimensions[1] + dimensions[3]) * image_height /
2) + offset_x #do visualize this later
obj.bbox.center.y = int((dimensions[0] + dimensions[2]) * image_width /
2) + offset_y #do visualize this later
return obj
def callbackPoseRCNN(self, data):
global obj, VecNeural #,obj_hypothesis
# recive data
objArray = Detection2DArray()
obj = Detection2D()
#obj_hypothesis= ObjectHypothesisWithPose()
VecNeural = Vector3()
# rcnn_pose
objArray = data
obj = objArray.detections
# rospy.loginfo(" lenth obj: %f", len(obj))
if len(obj) != 0:
# obj_hypothesis.id = obj[0].results[0].id
# obj_hypothesis.score = obj[0].results[0].score
# obj_hypothesis.pose.pose.position.x = obj[0].results[0].pose.pose.position.x
# obj_hypothesis.pose.pose.position.y = obj[0].results[0].pose.pose.position.y
# obj_hypothesis.pose.pose.position.z = obj[0].results[0].pose.pose.position.z
VecNeural.x = self.kalman.x[0] = obj[0].results[
0].pose.pose.position.x
VecNeural.y = self.kalman.x[1] = obj[0].results[
0].pose.pose.position.y
VecNeural.z = self.kalman.x[2] = obj[0].results[
0].pose.pose.position.z
def _construct_detection_msg_and_update_detection_image(
self, detection_res, channel_id, stamp):
if channel_id == Side.PORT:
multiplier = -1
else:
multiplier = 1
detection_array_msg = Detection2DArray()
detection_array_msg.header.frame_id = self.pub_frame_id
detection_array_msg.header.stamp = stamp
for object_id, detection in detection_res.items():
detection_msg = Detection2D()
detection_msg.header = detection_array_msg.header
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = object_id.value
object_hypothesis.score = detection['confidence']
object_hypothesis.pose.pose = self._detection_to_pose(
detection['pos'], channel_id)
# Filter out object detection outliers
if abs(object_hypothesis.pose.pose.position.y) > self.water_depth:
continue
else:
pos = self.channel_size + multiplier * detection['pos']
self.detection_image[
0,
max(pos -
10, 0):min(pos + 10, self.channel_size *
2), :] = self.detection_colors[object_id]
detection_msg.results.append(object_hypothesis)
detection_array_msg.detections.append(detection_msg)
return detection_array_msg
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 object_predict(self, object_data, header, image_np, image):
image_height, image_width, channels = image.shape
obj = Detection2D()
obj_hypothesis = ObjectHypothesisWithPose()
object_id = object_data[0]
object_score = object_data[1]
dimensions = object_data[2]
obj.header = header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2] - dimensions[0]) * image_height)
obj.bbox.size_x = int((dimensions[3] - dimensions[1]) * image_width)
obj.bbox.center.x = int(
(dimensions[1] + dimensions[3]) * image_height / 2)
obj.bbox.center.y = int(
(dimensions[0] + dimensions[2]) * image_width / 2)
# rospy.loginfo("publish bbox.size x: %d", obj.bbox.size_x)
# rospy.loginfo("publish bbox.size y: %d", obj.bbox.size_y)
# rospy.loginfo("publish bbox.center x: %d", obj.bbox.center.x)
# rospy.loginfo("publish bbox.center y: %d", obj.bbox.center.y)
return obj
def callback(self, ros_data):
'''Callback function of subscribed topic. '''
if ros_data.header.stamp < self.latest_finish_time:
return
#### direct conversion to CV2 ####
now = rospy.Time.now()
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# dim = (int(len(image_np / 20)), int(len(image_np[0]) / 20))
# cv2.resize(image_np, dim)
# image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
results = detect(self.net, self.meta, image_np)
detections = Detection2DArray()
for result in results:
detection = Detection2D()
detection.header = ros_data.header
res = ObjectHypothesisWithPose()
res.id = self.get_id(result[0])
res.score = result[1]
detection.results.append(res)
detection.bbox.size_x = result[2][2]
detection.bbox.size_y = result[2][3]
detection.bbox.center.x = result[2][0]
detection.bbox.center.y = result[2][1]
detections.detections.append(detection)
self.res_pub.publish(detections)
rospy.loginfo_throttle(
1, 'Took yolo %s to process image' %
((rospy.Time.now() - now).to_sec()))
self.latest_finish_time = rospy.Time.now()
def object_predict(self, object_data, header, image_np, image):
image_height, image_width, channels = image.shape
obj = Detection2D()
obj_hypothesis = ObjectHypothesisWithPose()
object_id = object_data[0]
object_score = object_data[1]
dimensions = object_data[2]
obj.header = header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2] - dimensions[0]) * image_height)
obj.bbox.size_x = int((dimensions[3] - dimensions[1]) * image_width)
obj.bbox.center.x = int(
(dimensions[1] + dimensions[3]) * image_height / 2)
obj.bbox.center.y = int(
(dimensions[0] + dimensions[2]) * image_width / 2)
#print(str(obj.bbox))
# print('height:'+str(image_height))
# print('width: '+str(image_width))
return obj
def callback(self):
image = rospy.wait_for_message(
"/zed2/zed_node/left/image_rect_color/compressed", CompressedImage)
time1 = rospy.Time.now().to_sec()
self.timer.header = image.header
self.timer.header.frame_id = "zed2_left_camera_frame"
self.timer.time_ref = rospy.Time.now()
self.timer_pub.publish(self.timer)
cv_image = self.bridge.compressed_imgmsg_to_cv2(image, "bgr8")
_, bboxes = detect_image(self.yolo,
cv_image,
"",
input_size=YOLO_INPUT_SIZE,
show=False,
CLASSES=TRAIN_CLASSES,
score_threshold=0.3,
iou_threshold=0.1,
rectangle_colors=(255, 0, 0))
detect = Detection2DArray()
detect.header = image.header
for Object in bboxes:
detection = Detection2D()
hypo = ObjectHypothesisWithPose()
#Start x
x1 = Object[0]
#End x
x2 = Object[2]
#Start y
y1 = Object[1]
#end y
y2 = Object[3]
#Size x
Sx = x2 - x1
#Size y
Sy = y2 - y1
#Center x
Cx = x1 + Sx / 2
#Center y
Cy = y1 + Sy / 2
detection.bbox.center.x = Cx
detection.bbox.center.y = Cy
detection.bbox.size_x = Sx
detection.bbox.size_y = Sy
hypo.id = int(Object[5])
hypo.score = Object[4]
detection.results = [
hypo,
]
detection.is_tracking = False
detect.detections.append(detection)
self.boxes_pub.publish(detect)
self.callback()
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 track(self, image):
detections = Detection2DArray()
success, boxes = self.tracker.update(image)
if not success:
return detections
for box in boxes:
x, y, w, h = box
detection = Detection2D()
detection.bbox.center.x = x
detection.bbox.center.y = y
detection.bbox.size_x = w
detection.bbox.size_y = h
detections.detections.append(detection)
return detections
def _publish_marker_detection(self, marker, cos_sim):
"""Publish detected marker"""
distance = self._get_distance_to_marker(marker)
object_hypothesis = ObjectHypothesisWithPose()
object_hypothesis.id = 1
# the higher the cos_sim (further away from 90 degree angle between current_pose
# and the marker), the lower the score
object_hypothesis.score = (-cos_sim + (self.buoy_radius * 2)) / (
self.buoy_radius * 2)
marker_transformed = self._transform_pose(
marker, from_frame=marker.header.frame_id)
object_hypothesis.pose.pose = marker_transformed.pose
# Add random noise to pose of object
object_hypothesis.pose.pose.position.x += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.position.y += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.position.z += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.x += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.y += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.z += np.random.randn(
) * self.noise_sigma
object_hypothesis.pose.pose.orientation.w += np.random.randn(
) * self.noise_sigma
# Wrap ObjectHypothesisWithPose msg into Detection2D msg
detection_msg = Detection2D()
detection_msg.header.frame_id = self.published_frame_id
detection_msg.header.stamp = rospy.Time.now()
detection_msg.results.append(object_hypothesis)
# Wrap Detection2D msg into Detection2DArray msg
detection_array_msg = Detection2DArray()
detection_array_msg.header = detection_msg.header
detection_array_msg.detections.append(detection_msg)
self.pub.publish(detection_array_msg)
# Publish detection as a Marker for visualization in rviz
detected_marker = copy.deepcopy(marker)
detected_marker.header.stamp = rospy.Time.now()
detected_marker.ns = 'detected_{}'.format(detected_marker.ns)
detected_marker.color = ColorRGBA(0, 1, 0, 1)
detected_marker.lifetime.secs = 1
self.pub_detected_markers.publish(detected_marker)
def callback_yolo(self, image):
image = self.image
cv_image = self.bridge.compressed_imgmsg_to_cv2(image, "bgr8")
_, bboxes = detect_image(self.yolo,
cv_image,
"",
input_size=YOLO_INPUT_SIZE,
show=False,
rectangle_colors=(255, 0, 0))
detect = Detection2DArray()
detect.header = image.header
for Object in bboxes:
detection = Detection2D()
hypo = ObjectHypothesisWithPose()
#Start x
x1 = Object[0]
#End x
x2 = Object[2]
#Start y
y1 = Object[1]
#end y
y2 = Object[3]
#Size x
Sx = x2 - x1
#Size y
Sy = y2 - y1
#Center x
Cx = x1 + Sx / 2
#Center y
Cy = y1 + Sy / 2
detection.bbox.center.x = Cx
detection.bbox.center.y = Cy
detection.bbox.size_x = Sx
detection.bbox.size_y = Sy
hypo.id = int(Object[5])
hypo.score = Object[4]
detection.results = [
hypo,
]
detect.detections.append(detection)
self.bbox_pub.publish(detect)
def __init__(self, type):
self.liveview = EpCamera(type)
self.csi_img = np.zeros((1280, 720, 3), np.uint8)
self.ep_image = np.zeros((1280, 720, 3), np.uint8)
self.image_pub = rospy.Publisher("image_topic", Image,
queue_size=10) #ep视频流信息
self.csi_image_pub = rospy.Publisher("csi_topic", Image,
queue_size=10) #nano上的csi视频流信息
self.face_detect_pub = rospy.Publisher("face_detect",
Detection2D,
queue_size=10) #人脸roi信息
self.bridge = CvBridge()
self.detMsg = Detection2D()
self.face_roi_thread = threading.Thread(target=self.get_face_roi_task)
#self.camera = CSICamera(width=640, height=360, capture_width=1280, capture_height=720, capture_fps=30)
self.open()
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 create_detections_msg(self, image_np, output_dict):
img_height = image_np.shape[0]
img_width = image_np.shape[1]
boxes = output_dict['detection_boxes']
classes = output_dict['detection_classes']
scores = output_dict['detection_scores']
detections = Detection2DArray()
detections.header.stamp = self.get_clock().now().to_msg()
detections.detections = []
for i in range(len(boxes)):
det = Detection2D()
det.header = detections.header
det.results = []
detected_object = ObjectHypothesisWithPose()
detected_object.id = classes[i].item()
detected_object.score = scores[i].item()
det.results.append(detected_object)
# box is ymin, xmin, ymax, xmax in normalized coordinates
box = boxes[i]
det.bbox.size_y = (box[2] - box[0]) * img_height
det.bbox.size_x = (box[3] - box[1]) * img_width
det.bbox.center.x = (box[1] + box[3]) * img_height / 2
det.bbox.center.y = (box[0] + box[2]) * img_width / 2
if (self.republish_image):
box_img = image_np[int(box[0] * img_height):int(box[2] *
img_height),
int(box[1] * img_width):int(box[3] *
img_width)]
det.source_img = img_utils.image_np_to_image_msg(box_img)
detections.detections.append(det)
return detections
def detect_objects(self, data):
"""
This function detects and classifies the objects in the image provided through a Service Request by running on her the provided
detection model. Returns a vision_msgs/Detection2DArray, for which each detection is populated only with the bbox and results fields. Moreover,
for what it concerns the results field, each result is populated only with the id and score fields.
All the other fields are not significant for this application, so they have been ignored.
Args:
data (sensor_msgs/Image): image to perform object detection on.
Returns:
pepper_object_detectionResponse: response encapsulating data regarding detected objects, structured as in service definition.
"""
# Convert image from sensor_msgs/Image to numpy array
img_np = ros_numpy.numpify(data.img)
rospy.loginfo('Object detection server computing predictions...')
detections = self._detection_model(img_np)
rospy.loginfo('Predictions computed!')
message = Detection2DArray()
# Insert all the predictions into the message and return them
for class_id, score, box in zip(detections['detection_classes'],
detections['detection_scores'],
detections['detection_boxes']):
detection = Detection2D()
detection.bbox.size_x = box[3] - box[1]
detection.bbox.size_y = box[2] - box[0]
detection.bbox.center.x = box[1] + detection.bbox.size_x / 2
detection.bbox.center.y = box[0] + detection.bbox.size_y / 2
detected_object = ObjectHypothesisWithPose()
detected_object.score = score
detected_object.id = class_id
detection.results.append(detected_object)
message.detections.append(detection)
# Create a response object
response = pepper_object_detectionResponse()
response.detections = message
return response
def publish_markers(self, fid_data_array):
fidarray = FiducialTransformArray()
fidarray.header.stamp = rospy.Time.now()
vis = Detection2DArray()
vis.header.stamp = rospy.Time.now()
for fid in fid_data_array:
if VIS_MSGS:
obj = Detection2D()
oh = ObjectHypothesisWithPose()
oh.id = fid.id
oh.pose.pose.position.x = fid.translation.x
oh.pose.pose.position.y = fid.translation.y
oh.pose.pose.position.z = fid.translation.z
oh.pose.pose.orientation.w = fid.rotation.w
oh.pose.pose.orientation.x = fid.rotation.x
oh.pose.pose.orientation.y = fid.rotation.y
oh.pose.pose.orientation.z = fid.rotation.z
oh.score = math.exp(-2 * OBJECT_ERROR)
obj.results.append(oh)
vis.detections.append(obj)
else:
data = FiducialTransform()
data.fiducial_id = fid.id
data.transform.translation = fid.translation
data.transform.rotation = fid.rotation
data.image_error = IMAGE_ERROR
data.object_error = OBJECT_ERROR
data.fiducial_area = FIDUCIAL_AREA
fidarray.transforms.append(data)
if VIS_MSGS:
self.fid_pub.publish(vis)
else:
self.fid_pub.publish(fidarray)
def encode_detection_msg(self,detections):
detections_msg = Detection2DArray()
if (len(detections)>0):
i=0
detstring='Detected:'
for det in detections:
detection = Detection2D()
detection.header.seq = self.detection_seq
detection.header.stamp = rospy.Time.now()
detection.header.frame_id = self.camera_frame
result = [ObjectHypothesisWithPose()]
result[0].id = det[0]
result[0].score = det[1]
detection.results = result
detection.bbox.center.x = (det[2]+det[4])/2
detection.bbox.center.y = (det[3]+det[5])/2
detection.bbox.size_x = det[4]-det[2]
detection.bbox.size_y = det[5]-det[3]
detections_msg.detections.append(detection)
detstring=detstring+' '+self.classes[int(det[0])]+', p=%.2f.'%(det[1])
i+=1
rospy.logwarn(detstring)
self.detection_seq += 1
return detections_msg
def object_predict(self, object_data, header, image_np, image, ID):
image_height, image_width, channels = image.shape
obj = Detection2D()
obj_hypothesis = ObjectHypothesisWithPose()
object_id = ID
object_score = object_data[1]
dimensions = object_data[2]
obj.header = header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2] - dimensions[0]) * image_height)
obj.bbox.size_x = int((dimensions[3] - dimensions[1]) * image_width)
#the center point of x coordinate is determined here...
obj.bbox.center.x = int(
(dimensions[1] + dimensions[3]) * image_width / 2)
#the center point of y coordinate is determined here...
obj.bbox.center.y = int(
(dimensions[0] + dimensions[2]) * image_height / 2)
#detection results are returned as type Detection2D to objArray...
return obj
def callback_synchronize(self, msg_img, msg_bbox, cam_id):
cv_img = self.cv_bridge.imgmsg_to_cv2(msg_img)
new_bbox = Detection2DArray()
print("aaaaa")
for detection in msg_bbox.detections:
box = Detection2D()
box.bbox = detection.bbox
pose = ObjectHypothesisWithPose()
print(detection.results[0].id)
if detection.results[0].id == 7: # id:7 Traffic light
x_min = int(detection.bbox.center.x -
(detection.bbox.size_x / 2))
y_min = int(detection.bbox.center.y -
(detection.bbox.size_y / 2))
x_max = int(detection.bbox.center.x +
(detection.bbox.size_x / 2))
y_max = int(detection.bbox.center.y +
(detection.bbox.size_y / 2))
img_cropped = cv_img[y_min:y_max, x_min:x_max]
preprocessed_img = self.preprocess_img(img=img_cropped)
predict_f = self.model_traffic_light(preprocessed_img)
predict = predict_f.data.max(1, keepdim=True)[1]
pose.id = int(1000 + int(predict))
cnn_prob = float(torch.exp(predict_f)[0][int(predict)])
pose.score = cnn_prob
box.results.append(pose)
# pose.id = int(1000 + int(predict))
# box.results.append(pose)
if self.validate_light_with_brightness_region(
img_cropped, int(predict)):
new_bbox.detections.append(box)
elif detection.results[0].id == 8: # id:8 Traffic sign
x_min = int(detection.bbox.center.x -
(detection.bbox.size_x / 2))
y_min = int(detection.bbox.center.y -
(detection.bbox.size_y / 2))
x_max = int(detection.bbox.center.x +
(detection.bbox.size_x / 2))
y_max = int(detection.bbox.center.y +
(detection.bbox.size_y / 2))
img_cropped = cv_img[y_min:y_max, x_min:x_max]
preprocessed_img = self.preprocess_img(img=img_cropped)
predict_f = self.model_traffic_sign(preprocessed_img)
predict = predict_f.data.max(1, keepdim=True)[1]
cnn_prob = float(torch.exp(predict_f)[0][int(predict)])
pose.score = cnn_prob
pose.id = int(2000 + int(predict))
box.results.append(pose)
new_bbox.detections.append(box)
else:
box = detection
new_bbox.detections.append(box)
new_bbox.header = msg_bbox.header
if cam_id == "cam_fm_01":
self.pub_fm01_new_bbox.publish(new_bbox)
elif cam_id == "cam_fr_01":
self.pub_fr01_new_bbox.publish(new_bbox)
elif cam_id == "cam_fl_01":
self.pub_fl01_new_bbox.publish(new_bbox)
def EfficientDetNode():
rospy.init_node('efficient_det_node', anonymous=True)
rospy.Subscriber('input', String, image_callback, queue_size=1)
pub = rospy.Publisher('/image_detections', Detection2DArray, queue_size=10)
rate = rospy.Rate(1) # 10hz
path_list = os.listdir(path)
path_list.sort(key=lambda x: int(x.split('.')[0]))
stamp_file = open(stamp_path)
stamp_lines = stamp_file.readlines()
stamp_i = 0
for filename in path_list:
img_path = filename
cur_frame = img_path[:-4]
img_path = path + "/" + img_path
cur_stamp = ((float)(stamp_lines[stamp_i][-13:].strip('\n')))
# cur_stamp = rospy.Time.from_sec(
# ((float)(stamp_lines[stamp_i][-13:].strip('\n'))))
stamp_i += 1
detection_results = Detection2DArray()
# tf bilinear interpolation is different from any other's, just make do
input_sizes = [512, 640, 768, 896, 1024, 1280, 1280, 1536, 1536]
input_size = input_sizes[
compound_coef] if force_input_size is None else force_input_size
ori_imgs, framed_imgs, framed_metas = preprocess(img_path,
max_size=input_size)
if use_cuda:
x = torch.stack(
[torch.from_numpy(fi).cuda() for fi in framed_imgs], 0)
else:
x = torch.stack([torch.from_numpy(fi) for fi in framed_imgs], 0)
x = x.to(torch.float32 if not use_float16 else torch.float16).permute(
0, 3, 1, 2)
model = EfficientDetBackbone(compound_coef=compound_coef,
num_classes=len(obj_list),
ratios=anchor_ratios,
scales=anchor_scales)
model.load_state_dict(
torch.load(f'weights/efficientdet-d{compound_coef}.pth',
map_location='cpu'))
model.requires_grad_(False)
model.eval()
if use_cuda:
model = model.cuda()
if use_float16:
model = model.half()
with torch.no_grad():
features, regression, classification, anchors = model(x)
regressBoxes = BBoxTransform()
clipBoxes = ClipBoxes()
out = postprocess(x, anchors, regression, classification,
regressBoxes, clipBoxes, threshold,
iou_threshold)
out = invert_affine(framed_metas, out)
display(cur_frame, out, ori_imgs, imshow=False, imwrite=True)
for i in range(len(out)):
for j in range(len(out[i]['rois'])):
x1, y1, x2, y2 = out[i]['rois'][j].astype(np.int)
obj = obj_list[out[i]['class_ids'][j]]
score = float(out[i]['scores'][j])
result = ObjectHypothesisWithPose()
result.score = score
if (obj == 'car'):
result.id = 0
if (obj == 'person'):
result.id = 1
if (obj == 'cyclist'):
result.id = 2
detection_msg = Detection2D()
detection_msg.bbox.center.x = (x1 + x2) / 2
detection_msg.bbox.center.y = (y1 + y2) / 2
detection_msg.bbox.size_x = x2 - x1
detection_msg.bbox.size_y = y2 - y1
detection_msg.results.append(result)
detection_results.detections.append(detection_msg)
rospy.loginfo("%d: %lf", detection_msg.results[0].id,
detection_msg.results[0].score)
detection_results.header.seq = cur_frame
#detection_results.header.stamp = cur_stamp
rospy.loginfo(detection_results.header.stamp)
pub.publish(detection_results)
if not os.path.exists(txt_path):
os.makedirs(txt_path)
#with open(f'txt/{cur_frame}.txt', 'w') as f:
with open(f'{txt_path}/{cur_frame}.txt', 'w') as f:
#f.write(str((float)(stamp_lines[stamp_i][-13:].strip('\n'))) + "\n")
f.write(str(cur_stamp) + "\n")
for detection in detection_results.detections:
f.write(str(detection.bbox.center.x) + " ")
f.write(str(detection.bbox.center.y) + " ")
f.write(str(detection.bbox.size_x) + " ")
f.write(str(detection.bbox.size_y) + " ")
f.write(str(detection.results[0].id) + " ")
f.write(str(detection.results[0].score) + "\n")
f.close()
rate.sleep()
print('running speed test...')
with torch.no_grad():
print('test1: model inferring and postprocessing')
print('inferring image for 10 times...')
t1 = time.time()
for _ in range(10):
_, regression, classification, anchors = model(x)
out = postprocess(x, anchors, regression, classification,
regressBoxes, clipBoxes, threshold,
iou_threshold)
out = invert_affine(framed_metas, out)
t2 = time.time()
tact_time = (t2 - t1) / 10
print(f'{tact_time} seconds, {1 / tact_time} FPS, @batch_size 1')
def step(self):
stamp = super().step()
if not stamp:
return
# Publish camera data
if self._image_publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
image = self._wb_device.getImage()
if image is None:
return
# Image data
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = self._wb_device.getHeight()
msg.width = self._wb_device.getWidth()
msg.is_bigendian = False
msg.step = self._wb_device.getWidth() * 4
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = image
msg.encoding = 'bgra8'
self._image_publisher.publish(msg)
self.__message_info.header.stamp = Time(
seconds=self._node.robot.getTime()).to_msg()
self._camera_info_publisher.publish(self.__message_info)
if self._wb_device.hasRecognition(
) and (self._recognition_publisher.get_subscription_count() > 0 or
self._recognition_webots_publisher.get_subscription_count()
> 0):
self._wb_device.recognitionEnable(self._timestep)
objects = self._wb_device.getRecognitionObjects()
if objects is None:
return
# Recognition data
reco_msg = Detection2DArray()
reco_msg_webots = WbCameraRecognitionObjects()
reco_msg.header.stamp = stamp
reco_msg_webots.header.stamp = stamp
reco_msg.header.frame_id = self._frame_id
reco_msg_webots.header.frame_id = self._frame_id
for obj in objects:
# Getting Object Info
position = Point()
orientation = Quaternion()
position.x = obj.get_position()[0]
position.y = obj.get_position()[1]
position.z = obj.get_position()[2]
axangle = obj.get_orientation()
quat = axangle2quat(axangle[:-1], axangle[-1])
orientation.w = quat[0]
orientation.x = quat[1]
orientation.y = quat[2]
orientation.z = quat[3]
obj_model = obj.get_model().decode('UTF-8')
obj_center = [
float(i) for i in obj.get_position_on_image()
]
obj_size = [float(i) for i in obj.get_size_on_image()]
obj_id = obj.get_id()
obj_colors = obj.get_colors()
# Object Info -> Detection2D
reco_obj = Detection2D()
hyp = ObjectHypothesisWithPose()
hyp.id = obj_model
hyp.pose.pose.position = position
hyp.pose.pose.orientation = orientation
reco_obj.results.append(hyp)
reco_obj.bbox.center.x = obj_center[0]
reco_obj.bbox.center.y = obj_center[1]
reco_obj.bbox.size_x = obj_size[0]
reco_obj.bbox.size_y = obj_size[1]
reco_msg.detections.append(reco_obj)
# Object Info -> WbCameraRecognitionObject
reco_webots_obj = WbCameraRecognitionObject()
reco_webots_obj.id = obj_id
reco_webots_obj.model = obj_model
reco_webots_obj.pose.pose.position = position
reco_webots_obj.pose.pose.orientation = orientation
reco_webots_obj.bbox.center.x = obj_center[0]
reco_webots_obj.bbox.center.y = obj_center[1]
reco_webots_obj.bbox.size_x = obj_size[0]
reco_webots_obj.bbox.size_y = obj_size[1]
for i in range(0, obj.get_number_of_colors()):
color = ColorRGBA()
color.r = obj_colors[3 * i]
color.g = obj_colors[3 * i + 1]
color.b = obj_colors[3 * i + 2]
reco_webots_obj.colors.append(color)
reco_msg_webots.objects.append(reco_webots_obj)
self._recognition_webots_publisher.publish(reco_msg_webots)
self._recognition_publisher.publish(reco_msg)
else:
self._wb_device.recognitionDisable()
else:
self._wb_device.disable()
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 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, depth_msg):
color_image = self._bridge.imgmsg_to_cv2(rgb_msg, '8UC3')
depth_image = self._bridge.imgmsg_to_cv2(
depth_msg, "8UC1"
)
## Enable to reporject depth map to 3D
"""
points_3D = cv2.reprojectImageTo3D(depth_image, self.Q2)
mask_map = depth_image > 0
output_points = points_3D[mask_map]
output_colors = color_image[mask_map]
cv2.imwrite("color.jpg", color_image)
cv2.imwrite("depth.jpg", depth_image)
if self.savepointcloud:
# output_file = "reconstructed.ply"
print ("\n Creating the output file... \n")
self.write_pointcloud(output_points, output_colors, output_file)
self.savepointcloud = False
"""
final_bbox = None
if self._lock_ROI:
self.init_variables_hard(False)
center = ( rgb_msg.width//2, rgb_msg.height//2)
width = rgb_msg.width//10
height = rgb_msg.height//10
self._inital_bbox = (
int(center[0] - width / 2),
int(center[1] - height / 2),
width,
height,
)
# rospy.loginfo("Initializing tracker")
current_bbox = self._inital_bbox
bbox_center = self.calculate_bbox_center(current_bbox)
self._is_first_frame = False
final_bbox = current_bbox
self._current_status = 1
T = TimeReference()
T.header.stamp = depth_msg.header.stamp
T.source = str(self._current_status)
self._pub_status.publish(T)
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 = Detection2D()
# Initialize header info with that of depthmap's
bbox_message.header.stamp = depth_msg.header.stamp
bbox_message.header.seq = "bbox_INFO"
# bbox info
bbox_message.bbox.size_x = final_bbox[2]
bbox_message.bbox.size_y = final_bbox[3]
bbox_message.bbox.center.theta = 0
bbox_message.bbox.center.x = final_bbox[0] + final_bbox[2] / 2
bbox_message.bbox.center.y = final_bbox[1] + final_bbox[3] / 2
self._pub_bbox.publish(bbox_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)
# cv2.imshow('TRACKING',color_image)
# cv2.waitKey(1)
imgmsg = self._bridge.cv2_to_imgmsg(
color_image, 'rgb8'
)
self._pub_result_img.publish(imgmsg)
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)
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_boundbox(color_img):
if not has_color_mask:
return 'Failed to unpack color mask.'
print "Triggered callback_boundbox."
img = bridge.imgmsg_to_cv2(color_img, "bgr8") # BGR OpenCV image
rgb = img[:, :, ::-1] # flip to RGB for display
hsv = cv2.cvtColor(img,
cv2.COLOR_BGR2HSV) # convert to HSV for color masking
bg_mask = cv2.inRange(hsv, np.array(floor), np.array(ceiling))
fg_mask = cv2.bitwise_not(bg_mask)
fg_mask = cv2.morphologyEx(fg_mask, cv2.MORPH_OPEN,
np.ones((3, 3), np.uint8))
# Apply final mask to show sherds and black out background
objects = cv2.bitwise_and(rgb, rgb, mask=fg_mask)
# Display original image and segmented objects
plt.subplot(121), plt.imshow(rgb)
plt.title('Original Image'), plt.xticks([]), plt.yticks([])
plt.subplot(122), plt.imshow(objects)
plt.title('Sherds'), plt.xticks([]), plt.yticks([])
plt.show()
# Find contours in gray 'objects' image. Finding contours in 'color_mask' image returns false positives.
gray_objects = cv2.cvtColor(np.array(objects), cv2.COLOR_BGR2GRAY)
_, contours, _ = cv2.findContours(gray_objects, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
# Set minimum rectangle area to be recognized as sherd
min_height_px = 30
min_width_px = 30
print("Found %d objects in this frame - may or may not all be sherds." %
(len(contours)))
print(
"Recognizing only rectangles larger than %d by %d pixels as sherds." %
(min_height_px, min_width_px))
# Construct Detection2DArray ROS message to contain all valid bounding boxes
BoundingBoxes_Array = Detection2DArray()
BoundingBoxes_Array.header = color_img.header # meta-data
# For each detected contour, find bounding box
for cnt in contours:
rect = cv2.minAreaRect(cnt)
# box dimensions in pixels
height_px = rect[1][1]
width_px = rect[1][0]
# If rectangle >= specified area...
if height_px >= min_height_px and width_px >= min_width_px:
print("This sherd's bounding box: " + str(rect))
# Extract (x,y) coordinates of box corners in order to draw rectangles, starting at "lowest" corner (largest y-coordinate) and moving CW
# Note: height is distance between 0th and 1st corner. Width is distance between 1st and 2nd corner.
box = cv2.boxPoints(rect)
box = np.int0(box)
print("Box corner coordinates: " + str(box))
# Debugging: draw bounding boxes around sherds
# Convert original RGB image to np.array to draw contours as boxes
rgb_contours = cv2.drawContours(np.array(rgb), [box], 0,
(255, 0, 0), 3)
# x,y of bounding box centers
x_center_pos = rect[0][0]
y_center_pos = rect[0][1]
# transform rotation angle to read between long side and y-axis
angle = rect[2]
if width_px < height_px:
angle += 180
else:
angle += 90
# transform rotation angle so that gripper never rotates more than 90deg CW or CCW
if angle > 90:
grip_angle = angle - 180
else:
grip_angle = angle
# Debugging: print
print("x center is " + str(x_center_pos))
print("y center is " + str(y_center_pos))
print("Box rotation angle is %f." % angle)
print("Gripper rotates %f degrees." % grip_angle)
plt.figure("Figure 2")
plt.imshow(rgb_contours)
# plt.imshow(img[:, :, ::-1])
plt.title("Bounding Box around Sherd")
plt.show()
# Construct a Detection2D() msg to add this bounding box to BoundingBoxes_Array
BoundingBoxes_Element = Detection2D()
BoundingBoxes_Element.header = BoundingBoxes_Array.header
BoundingBoxes_Element.bbox.center.x = x_center_pos
BoundingBoxes_Element.bbox.center.y = y_center_pos
BoundingBoxes_Element.bbox.center.theta = np.radians(
grip_angle) # angle converted to radians
BoundingBoxes_Element.bbox.size_x = width_px
BoundingBoxes_Element.bbox.size_y = height_px
BoundingBoxes_Array.detections.append(BoundingBoxes_Element)
else:
pass
# Publish the BoundingBoxes_Array message to the 'Bounding_Boxes' topic
boundbox_pub.publish(BoundingBoxes_Array)
print "BoundingBoxes_Array msg published to Bounding_Boxes."
def object_predict(self, object_data, header, image_np, image):
image_height, image_width, channels = image.shape
obj = Detection2D()
obj_hypothesis = ObjectHypothesisWithPose()
object_id = object_data[0]
object_score = object_data[1]
dimensions = object_data[2]
obj.header = header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
#obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2] - dimensions[0]) * image_height)
obj.bbox.size_x = int((dimensions[3] - dimensions[1]) * image_width)
obj.bbox.center.x = int(
(dimensions[1] + dimensions[3]) * image_width / 2)
obj.bbox.center.y = int(
(dimensions[0] + dimensions[2]) * image_height / 2)
###################################
pixelDiametro = obj.bbox.size_x
# choose the bigest size
if (obj.bbox.size_x > obj.bbox.size_y):
pixelDiametro = obj.bbox.size_x
else:
pixelDiametro = obj.bbox.size_y
#DIAMETER_LANDMARCK_M = 0.24 OR 0.5
metersDiametroLandmarck = self.DIAMETER_LANDMARCK_M
#DISTANCE_FOCAL = 750
distFocus_real = self.DISTANCE_FOCAL
altura = float(
(metersDiametroLandmarck * distFocus_real) / pixelDiametro)
# rospy.loginfo("--------------------------------")
# rospy.loginfo("Diametro Marcador Real: %f", metersDiametroLandmarck)
# # rospy.loginfo("Distancia Focal Real: %f", distFocus_real)
# rospy.loginfo("Diametro (pixel): %f", pixelDiametro)
# rospy.loginfo("Altura Drone (m): %f", altura)
###################################
pixel_x = int((obj.bbox.center.x - (image_width / 2)) * (-1))
pixel_y = int((obj.bbox.center.y - (image_height / 2)) * (1))
k = float(metersDiametroLandmarck / pixelDiametro)
obj_hypothesis.pose.pose.position.x = pixel_x * k
obj_hypothesis.pose.pose.position.y = pixel_y * k
obj_hypothesis.pose.pose.position.z = altura
obj.results.append(obj_hypothesis)
#rospy.loginfo("publish obj_hypothesis.score: %d", object_score)
# rospy.loginfo("publish bbox.size x: %d", obj.bbox.size_x)
# rospy.loginfo("publish bbox.size y: %d", obj.bbox.size_y)
# rospy.loginfo("publish bbox.center x: %d", obj.bbox.center.x)
# rospy.loginfo("publish bbox.center y: %d", obj.bbox.center.y)
return obj