def publish_roi(self):
msg = RegionOfInterest()
msg.x_offset = (self.center[0] - self.majorAxis) * self.overlayImgSize
msg.y_offset = (self.center[1] - self.minorAxis) * self.overlayImgSize
msg.width = int(self.majorAxis * 2 * self.overlayImgSize)
msg.height = int(self.minorAxis * 2 * self.overlayImgSize)
self.roiPub.publish(msg)
def getResult(self, predictions):
boxes = predictions.pred_boxes if predictions.has(
"pred_boxes") else None
if predictions.has("pred_masks"):
masks = np.asarray(predictions.pred_masks)
else:
return
result_msg = Result()
result_msg.header = self._header
result_msg.class_ids = predictions.pred_classes if predictions.has(
"pred_classes") else None
result_msg.class_names = np.array(
self._class_names)[result_msg.class_ids.numpy()]
result_msg.scores = predictions.scores if predictions.has(
"scores") else None
for i, (x1, y1, x2, y2) in enumerate(boxes):
mask = np.zeros(masks[i].shape, dtype="uint8")
mask[masks[i, :, :]] = 255
mask = self._bridge.cv2_to_imgmsg(mask)
mask.encoding = "mono8" # 参考mask_rcnn_node line164
result_msg.masks.append(mask)
box = RegionOfInterest()
box.x_offset = np.uint32(x1)
box.y_offset = np.uint32(y1)
box.height = np.uint32(y2 - y1)
box.width = np.uint32(x2 - x1)
result_msg.boxes.append(box)
return result_msg
def camera_info(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def convert_to_ros_bounding_box(bounding_box):
ros_bb = RegionOfInterest()
ros_bb.x_offset = bounding_box['x']
ros_bb.y_offset = bounding_box['y']
ros_bb.width = bounding_box['width']
ros_bb.height = bounding_box['height']
return ros_bb
def _build_result_msg(self, msg, result):
result_msg = Result()
result_msg.header = msg.header
for i, (y1, x1, y2, x2) in enumerate(result['rois']):
box = RegionOfInterest()
box.x_offset = x1#np.asscalar(x1)
box.y_offset = y1#np.asscalar(y1)
box.height = y2-y1#np.asscalar(y2 - y1)
box.width = x2-x1#np.asscalar(x2 - x1)
result_msg.boxes.append(box)
class_id = result['class_ids'][i]
result_msg.class_ids.append(class_id)
class_name = self._class_names[class_id]
result_msg.class_names.append(class_name)
score = result['scores'][i]
result_msg.scores.append(score)
mask = Image()
mask.header = msg.header
mask.height = result['masks'].shape[0]
mask.width = result['masks'].shape[1]
mask.encoding = "mono8"
mask.is_bigendian = False
mask.step = mask.width
mask.data = (result['masks'][:, :, i] * 255).tobytes()
result_msg.masks.append(mask)
return result_msg
def to_roi(top_left, bottom_right):
msg = RegionOfInterest()
msg.x_offset = round(top_left[0])
msg.y_offset = round(top_left[1])
msg.width = round(abs(bottom_right[0] - top_left[0]))
msg.height = round(abs(bottom_right[1] - top_left[1]))
return msg
def getResult(predictions, classes):
boxes = predictions.pred_boxes if predictions.has("pred_boxes") else None
if predictions.has("pred_masks"):
masks = np.asarray(predictions.pred_masks)
#print(type(masks))
else:
return
result_msg = Result()
# result_msg.header = self._header
result_msg.class_ids = predictions.pred_classes if predictions.has(
"pred_classes") else None
result_msg.class_names = np.array(classes)[result_msg.class_ids.numpy()]
result_msg.scores = predictions.scores if predictions.has(
"scores") else None
for i, (x1, y1, x2, y2) in enumerate(boxes):
mask = np.zeros(masks[i].shape, dtype="uint8")
mask[masks[i, :, :]] = 255
mask = br.cv2_to_imgmsg(mask)
result_msg.masks.append(mask)
box = RegionOfInterest()
box.x_offset = np.uint32(x1)
box.y_offset = np.uint32(y1)
box.height = np.uint32(y2 - y1)
box.width = np.uint32(x2 - x1)
result_msg.boxes.append(box)
return result_msg
def publish(self,
boxes,
scores,
classes,
num,
category_index,
masks=None,
fps=0):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
if classes[i] in category_index.keys():
class_name = category_index[classes[i]]['name']
else:
class_name = 'N/A'
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax - xmin) / 2.0
box.y_offset = ymin + (ymax - ymin) / 2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(100 * scores[i])
if masks is not None:
obj.mask = self._bridge.cv2_to_imgmsg(
masks[i], encoding="passthrough")
msg.objects.append(obj)
msg.fps = fps
# publish detection message
self.DetPub.publish(msg)
def callback(image_msg):
global pub_Image
global pub_BoxesImage
global pub_Boxes
global face_cascade
global bridge
global boxes_msg
global faces
global local_position_pub
global goal_pose
# Convert ros image into a cv2 image
cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Find the faces and draw a rectangle around them
faces = face_cascade.detectMultiScale(cv_gray, 1.3, 5)
for (x, y, w, h) in faces:
cv_img = cv2.rectangle(cv_img, (x, y), (x + w, y + h), (255, 0, 0), 2)
# For each image, we also want to publish the croped image
crop_img = cv_img[y:y + h, x:x + w]
crop_msg = bridge.cv2_to_imgmsg(crop_img, encoding="bgr8")
# And the region of interest information
pub_BoxesImage.publish(crop_msg)
boxes_msg = RegionOfInterest()
boxes_msg.x_offset = x
boxes_msg.y_offset = y
boxes_msg.height = h
boxes_msg.width = w
boxes_msg.do_rectify = False
pub_Boxes.publish(boxes_msg)
# Publish the original image with the rectangles
mod_img = bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
pub_Image.publish(mod_img)
def imgCallback(self, image):
num = 0
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
# 如果存在人
roi = RegionOfInterest()
try:
position = self.detectWave(cv_image, self.check_gender)
cv2.rectangle(cv_image, (position[1], position[3]),
(position[0], position[2]), (0, 0, 255))
roi.x_offset = position[0]
roi.y_offset = position[2]
roi.width = position[1] - position[0]
roi.height = position[3] - position[2]
self.roi = roi
self.roi_pub.publish(roi)
if self.ispub == False:
stringq = "I can tell one wave. Now I will recognize people. "
self.word_pub.publish(stringq)
stringq = "ok "
self.face_pub.publish(stringq)
self.ispub = True
rospy.loginfo("One Wave!")
num = 1
except:
self.roi = roi
if num == 0:
self.roi_pub.publish(roi)
cv_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
self.img_pub.publish(cv_image)
def mouse_callback(self, event, x, y, flags, param):
print 'Mouse callback status', self.status
if self.status == 'initializing':
if event == cv2.EVENT_MOUSEMOVE:
self.color_image_drawing = np.copy(self.color_image)
p1_x = max(0, x - self.x_width / 2)
p1_y = max(0, y - self.y_width / 2)
p2_x = min(self.img_size_x - 1, x + self.x_width / 2)
p2_y = min(self.img_size_y - 1, y + self.y_width / 2)
cv2.rectangle(self.color_image_drawing, (p1_x, p1_y),
(p2_x, p2_y),
color=(0, 255, 0),
thickness=1)
if event == cv2.EVENT_LBUTTONDOWN:
self.status = 'tracking'
init_track_msg = RegionOfInterest()
init_track_msg.x_offset = max(0, x - self.x_width / 2)
init_track_msg.y_offset = max(0, y - self.y_width / 2)
init_track_msg.height = self.y_width
init_track_msg.width = self.x_width
self.init_tracking_pub.publish(init_track_msg)
def publish(self, boxes, scores, classes, num, category_index, masks=None, fps=0):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
if classes[i] in category_index.keys():
class_name = category_index[classes[i]]['name']
else:
class_name = 'N/A'
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax-xmin)/2.0
box.y_offset = ymin + (ymax-ymin)/2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(100*scores[i])
if masks is not None:
obj.mask = self._bridge.cv2_to_imgmsg(masks[i], encoding="passthrough")
msg.objects.append(obj)
msg.fps = fps
# publish detection message
self.DetPub.publish(msg)
def face_detect_cb(self, event):
if self.image is not None and self.lock.acquire(False):
cnn_results = self.recogniser.detect_faces(self.image,
scale=self.cnn_scale)
features = Features()
features.features = []
for k, d in enumerate(cnn_results):
padding = int(self.image.shape[0] * self.cnn_padding)
feature = Feature()
roi = RegionOfInterest()
roi.x_offset = np.maximum(d.left() - padding, 0)
roi.y_offset = np.maximum(d.top() - padding, 0)
roi.height = np.minimum(d.bottom() - roi.y_offset + padding,
self.image.shape[0])
roi.width = np.minimum(d.right() - roi.x_offset + padding,
self.image.shape[1])
feature.roi = roi
feature.crop = self.bridge.cv2_to_imgmsg(
np.array(
self.image[roi.y_offset:roi.y_offset + roi.height,
roi.x_offset:roi.x_offset + roi.width, :]))
features.features.append(feature)
self.lock.release()
self.faces_pub.publish(features)
def publish(self, boxes, scores, classes, num, image_shape, category_index, masks=None):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = np.asscalar(xmin)
box.y_offset = np.asscalar(ymin)
box.height = np.asscalar(ymax - ymin)
box.width = np.asscalar(xmax - xmin)
display_str = '##\nnumber {} {}: {}% at image coordinates (({}, {}) to ({}, {}))\n##'.format(i, class_name, int(100*scores[i]), left, top, right, bottom)
print(display_str)
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(scores[i])
obj.mask = masks[i]
msg.objects.append(obj)
# publish detection message
self.DetPub.publish(msg)
def publish_roi(self):
if not self.drag_start:
if self.track_box is not None:
roi_box = self.track_box
elif self.detect_box is not None:
roi_box = self.detect_box
else:
return
try:
roi_box = self.cvBox2D_to_cvRect(roi_box)
except:
return
# Watch out for negative offsets
roi_box[0] = max(0, roi_box[0])
roi_box[1] = max(0, roi_box[1])
try:
ROI = RegionOfInterest()
ROI.x_offset = int(roi_box[0])
ROI.y_offset = int(roi_box[1])
ROI.width = int(roi_box[2])
ROI.height = int(roi_box[3])
self.roi_pub.publish(ROI)
except:
rospy.loginfo("Publishing ROI failed")
def main():
rospy.wait_for_service('detect_keypoints')
detect_keypoint = rospy.ServiceProxy('detect_keypoints',
MankeyKeypointDetection)
# Get the test data path
project_path = os.path.join(os.path.dirname(__file__), os.path.pardir)
project_path = os.path.abspath(project_path)
test_data_path = os.path.join(project_path, 'test_data')
cv_rbg_path = os.path.join(test_data_path, '000000_rgb.png')
cv_depth_path = os.path.join(test_data_path, '000000_depth.png')
# Read the image
cv_rgb = cv2.imread(cv_rbg_path, cv2.IMREAD_COLOR)
cv_depth = cv2.imread(cv_depth_path, cv2.IMREAD_ANYDEPTH)
# The bounding box
roi = RegionOfInterest()
roi.x_offset = 261
roi.y_offset = 194
roi.width = 327 - 261
roi.height = 260 - 194
# Build the request
request = MankeyKeypointDetectionRequest()
bridge = CvBridge()
request.rgb_image = bridge.cv2_to_imgmsg(cv_rgb, 'bgr8')
request.depth_image = bridge.cv2_to_imgmsg(cv_depth)
request.bounding_box = roi
response = detect_keypoint(request)
print response
def publish_roi(self):
if not self.drag_start:
if self.track_box is not None:
roi_box = self.track_box
elif self.detect_box is not None:
roi_box = self.detect_box
else:
return
try:
roi_box = self.cvBox2D_to_cvRect(roi_box)
except:
return
# Watch out for negative offsets
roi_box[0] = max(0, roi_box[0])
roi_box[1] = max(0, roi_box[1])
try:
ROI = RegionOfInterest()
ROI.x_offset = int(roi_box[0])
ROI.y_offset = int(roi_box[1])
ROI.width = int(roi_box[2])
ROI.height = int(roi_box[3])
self.roi_pub.publish(ROI)
except:
rospy.loginfo("Publishing ROI failed")
def rosmsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
"""
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = self._height
msg.width = self._width
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [
self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0
]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0,
0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def face_detect_cb(self, event):
if self.image is not None and self.lock.acquire(False):
cnn_results = self.recogniser.detect_faces(self.image,
scale=self.cnn_scale)
features = Features()
features.features = []
closest_face = None
closest_face_img = None
max_distance = float("inf")
for k, d in enumerate(cnn_results):
padding = int(self.image.shape[0] * self.cnn_padding)
feature = Feature()
roi = RegionOfInterest()
roi.x_offset = np.maximum(d.left() - padding, 0)
roi.y_offset = np.maximum(d.top() - padding, 0)
roi.height = np.minimum(d.bottom() - roi.y_offset + padding,
self.image.shape[0])
roi.width = np.minimum(d.right() - roi.x_offset + padding,
self.image.shape[1])
feature.roi = roi
feature.crop = self.bridge.cv2_to_imgmsg(
np.array(
self.image[roi.y_offset:roi.y_offset + roi.height,
roi.x_offset:roi.x_offset + roi.width, :]))
if self.depth_image is not None:
xw = np.array(self.depth_image[
roi.y_offset:roi.y_offset + roi.height,
roi.x_offset:roi.x_offset +
roi.width]).mean() * self.rs_depth_scale
else:
xw = 2.0
xc = roi.x_offset + roi.width / 2.0
yc = roi.y_offset + roi.height / 2.5
x, y, z = self.get_world_coordinates(xw, self.image.shape[1],
self.image.shape[0], xc,
yc)
distance = math.sqrt(x * x + y * y + z * z)
if distance < max_distance:
closest_face = x, y, z
max_distance = distance
features.features.append(feature)
self.lock.release()
self.faces_pub.publish(features)
if closest_face is not None:
x, y, z = closest_face
self.last_face_pos = closest_face
# self.face_debug.publish(closest_face_img)
# print(x, y, z)
self.send_face_transform(x, y, z)
def prediction_generate(self, match, last_match): #print "prediction_generate" prediction_roi = RegionOfInterest() prediction_roi.x_offset = 2 * match.x_offset - last_match.x_offset prediction_roi.y_offset = 2 * match.y_offset - last_match.y_offset print match.x_offset, last_match.x_offset, prediction_roi.x_offset print match.y_offset, last_match.y_offset, prediction_roi.y_offset return prediction_roi
def prediction_generate(self, match, last_match):
#print "prediction_generate"
prediction_roi = RegionOfInterest()
prediction_roi.x_offset = 2 * match.x_offset - last_match.x_offset
prediction_roi.y_offset = 2 * match.y_offset - last_match.y_offset
print match.x_offset, last_match.x_offset, prediction_roi.x_offset
print match.y_offset, last_match.y_offset, prediction_roi.y_offset
return prediction_roi
def do_camshift(self, cv_image):
""" Get the image size """
image_size = cv.GetSize(cv_image)
image_width = image_size[0]
image_height = image_size[1]
""" Convert to HSV and keep the hue """
hsv = cv.CreateImage(image_size, 8, 3)
cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV)
self.hue = cv.CreateImage(image_size, 8, 1)
cv.Split(hsv, self.hue, None, None, None)
""" Compute back projection """
backproject = cv.CreateImage(image_size, 8, 1)
""" Run the cam-shift algorithm """
cv.CalcArrBackProject( [self.hue], backproject, self.hist )
cv.ShowImage("Backproject", backproject)
if self.track_window and is_rect_nonzero(self.track_window):
crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
""" If mouse is pressed, highlight the current selected rectangle
and recompute the histogram """
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(cv_image, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(cv_image, cv_image, 0.5)
cv.Copy(save, sub)
x,y,w,h = self.selection
cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255))
sel = cv.GetSubRect(self.hue, self.selection )
cv.CalcArrHist( [sel], self.hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist)
if max_val != 0:
cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
""" The width and height on the returned track_box seem to be exchanged--a bug in CamShift? """
((x, y), (w, h), angle) = track_box
track_box = ((x, y), (h, w), angle)
cv.EllipseBox(cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0)
roi = RegionOfInterest()
roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2)))
roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2)))
roi.width = int(track_box[1][0])
roi.height = int(track_box[1][1])
self.ROI.publish(roi)
cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist))
if not self.backproject_mode:
return cv_image
else:
return backproject
def extended_face_roi(self, face_roi, img_shape):
extroi = RegionOfInterest()
wext = int(face_roi.width * FACE_ROI_WIDTH_EXTENTION_K)
hext = int(face_roi.height * FACE_ROI_HEIGHT_EXTENTION_K)
extroi.x_offset = max(face_roi.x_offset - wext, 0)
extroi.y_offset = max(face_roi.y_offset - hext, 0)
extroi.width = min(face_roi.width + 2*wext, img_shape[1] - extroi.x_offset - 1)
extroi.height = min(face_roi.height + 2*hext, img_shape[0] - extroi.y_offset - 1)
return extroi
def pcl_segment_objects(self,
cloud,
non_planar_ratio,
cluster_tol,
min_size,
max_size,
visualize=False):
# segments objects in pcl by clustering
# npr is about how much of the cloud to filter out
# cluster_tol distance between each point to be considered in 1 cluster. 0.02 is 2cm
# last two are min and max number of points in a cluster
try:
rospy.loginfo('waiting for {}'.format('/segment_objects'))
rospy.wait_for_service('/segment_objects')
rospy.loginfo('connected to {}'.format('/segment_objects'))
segmentation_req = SegmentObjectsRequest(cloud, non_planar_ratio,
cluster_tol, min_size,
max_size)
segmentation_res = self.segment_objects_srv(segmentation_req)
width = cloud.width
rospy.loginfo('No of clusters found from segment objects : ')
rospy.loginfo(len(segmentation_res.clusters))
bounding_boxes = []
for cluster in segmentation_res.clusters:
left = cluster.indices[0] % width
right = cluster.indices[0] % width
top = int(cluster.indices[0] / width)
bottom = int(cluster.indices[0] / width)
for index in cluster.indices:
row = int(index / width)
column = index % width
left = min(left, column)
right = max(right, column)
top = min(top, row)
bottom = max(bottom, row)
bounding_box = RegionOfInterest()
bounding_box.x_offset = left
bounding_box.y_offset = top
bounding_box.width = right - left
bounding_box.height = bottom - top
bounding_boxes.append(bounding_box)
if visualize:
self.visualize_bounding_boxes(bounding_boxes,
'PCL segmented objects')
return bounding_boxes, segmentation_res.objects, segmentation_res.whole
except ServiceException as ex:
rospy.logwarn(ex)
def converts_to_ObjectInBoxe(image_size,
ymin,
xmin,
ymax,
xmax,
probability=(),
object_name_list=(),
use_normalized_coordinates=True,
do_rectify=False):
"""Adds a bounding box to an image.
Bounding box coordinates can be specified in either absolute (pixel) or
normalized coordinates by setting the use_normalized_coordinates argument.
Each string in display_str_list is displayed on a separate line above the
bounding box in black text on a rectangle filled with the input 'color'.
If the top of the bounding box extends to the edge of the image, the strings
are displayed below the bounding box.
Args:
image_size: image_size.
ymin: ymin of bounding box.
xmin: xmin of bounding box.
ymax: ymax of bounding box.
xmax: xmax of bounding box.
object_name_list: list of strings to display in box
(each to be shown on its own line).
use_normalized_coordinates: If True (default), treat coordinates
ymin, xmin, ymax, xmax as relative to the image. Otherwise treat
coordinates as absolute.
"""
im_height, im_width = image_size
if use_normalized_coordinates:
(left, right, top, bottom) = (xmin * im_width, xmax * im_width,
ymin * im_height, ymax * im_height)
else:
(left, right, top, bottom) = (xmin, xmax, ymin, ymax)
object = Object()
object.object_name = object_name_list[0]
object.probability = probability[0]
roi = RegionOfInterest()
roi.x_offset = int(left)
roi.y_offset = int(top)
roi.height = int(bottom - top)
roi.width = int(right - left)
roi.do_rectify = do_rectify
object_in_box = ObjectInBox()
object_in_box.object = object
object_in_box.roi = roi
return object_in_box
def click(s, x, y, click, u):
if click:
roi = RegionOfInterest()
#r = 10
roi.x_offset = x #max(x-r,0)
roi.y_offset = y #max(y-r,0)
#roi.width = 2*r
#roi.height= 2*r
roi_pub.publish(roi)
print roi.x_offset, roi.y_offset
def click(s, x, y, click, u): if click: roi = RegionOfInterest() #r = 10 roi.x_offset = x#max(x-r,0) roi.y_offset = y#max(y-r,0) #roi.width = 2*r #roi.height= 2*r roi_pub.publish(roi) print roi.x_offset, roi.y_offset
def yoloCallback(self, msg):
roi = RegionOfInterest()
for data in msg.bounding_boxes:
if data.Class == self.target:
roi.x_offset = data.xmin
roi.y_offset = data.ymin
roi.width = data.xmax -data.xmin
roi.height = data.ymax - data.ymin
self.roi_pub.publish(roi)
return 0
def publish_roi(self):
try:
ROI = RegionOfInterest()
ROI.x_offset = int(self.x)
ROI.y_offset = int(self.y)
ROI.width = int(self.w)
ROI.height = int(self.h)
self.roi_pub.publish(ROI)
except:
rospy.loginfo("Publishing ROI failed")
def publish_roi(self):
try:
roi = RegionOfInterest()
roi.x_offset = 0
roi.y_offset = 0
roi.width = 0
roi.height = 0
self.roi_pub.publish(roi)
except:
rospy.loginfo("Publishing ROI failed")
def publish_track_roi(self):
try:
roi = RegionOfInterest()
x,y,w,h = self.track_box
roi.x_offset = x + 0.5 * w
roi.y_offset = y + 0.5 * h
roi.width = w
roi.height = h
self.roi_pub.publish(roi)
except:
rospy.loginfo("Publishing ROI failed")
def extended_face_roi(self, face_roi, img_shape):
extroi = RegionOfInterest()
wext = int(face_roi.width * FACE_ROI_WIDTH_EXTENTION_K)
hext = int(face_roi.height * FACE_ROI_HEIGHT_EXTENTION_K)
extroi.x_offset = max(face_roi.x_offset - wext, 0)
extroi.y_offset = max(face_roi.y_offset - hext, 0)
extroi.width = min(face_roi.width + 2 * wext,
img_shape[1] - extroi.x_offset - 1)
extroi.height = min(face_roi.height + 2 * hext,
img_shape[0] - extroi.y_offset - 1)
return extroi
def do_camshift(self, cv_image):
""" Get the image size """
image_size = cv.GetSize(cv_image)
image_width = image_size[0]
image_height = image_size[1]
""" Convert to HSV and keep the hue """
hsv = cv.CreateImage(image_size, 8, 3)
cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV)
self.hue = cv.CreateImage(image_size, 8, 1)
cv.Split(hsv, self.hue, None, None, None)
""" Compute back projection """
backproject = cv.CreateImage(image_size, 8, 1)
""" Run the cam-shift algorithm """
cv.CalcArrBackProject( [self.hue], backproject, self.hist )
if self.track_window and is_rect_nonzero(self.track_window):
crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
""" If mouse is pressed, highlight the current selected rectangle
and recompute the histogram """
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(cv_image, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(cv_image, cv_image, 0.5)
cv.Copy(save, sub)
x,y,w,h = self.selection
cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255))
sel = cv.GetSubRect(self.hue, self.selection )
cv.CalcArrHist( [sel], self.hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist)
if max_val != 0:
cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox( cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 )
roi = RegionOfInterest()
roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2)))
roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2)))
roi.width = int(track_box[1][0])
roi.height = int(track_box[1][1])
self.ROI.publish(roi)
cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist))
if not self.backproject_mode:
return cv_image
else:
return backproject
def tracked_object_message(track):
msg = TrackedObject()
msg.id = track.track_id
roi_msg = RegionOfInterest()
roi_msg.x_offset = int(track.x1)
roi_msg.y_offset = int(track.y1)
roi_msg.height = int(track.get_height())
roi_msg.width = int(track.get_width())
msg.roi = roi_msg
return msg
def main(visualize):
rospy.wait_for_service('detect_keypoints')
detect_keypoint = rospy.ServiceProxy('detect_keypoints',
MankeyKeypointDetection)
# Get the test data path
project_path = os.path.join(os.path.dirname(__file__), os.path.pardir)
project_path = os.path.abspath(project_path)
test_data_path = os.path.join(project_path, 'test_data')
cv_rbg_path = os.path.join(test_data_path, '000000_rgb.png')
cv_depth_path = os.path.join(test_data_path, '000000_depth.png')
# Read the image
cv_rgb = cv2.imread(cv_rbg_path, cv2.IMREAD_COLOR)
cv_depth = cv2.imread(cv_depth_path, cv2.IMREAD_ANYDEPTH)
# The bounding box
roi = RegionOfInterest()
roi.x_offset = 261
roi.y_offset = 194
roi.width = 327 - 261
roi.height = 260 - 194
# Build the request
request = MankeyKeypointDetectionRequest()
bridge = CvBridge()
request.rgb_image = bridge.cv2_to_imgmsg(cv_rgb, 'bgr8')
request.depth_image = bridge.cv2_to_imgmsg(cv_depth)
request.bounding_box = roi
response = detect_keypoint(request)
print response
if visualize:
import open3d as o3d
vis_list = []
color = o3d.geometry.Image(cv_rgb)
depth = o3d.geometry.Image(cv_depth)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(color, depth)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd,
o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
vis_list.append(pcd)
for keypoint in response.keypoints_camera_frame:
keypoints_coords \
= o3d.geometry.TriangleMesh.create_coordinate_frame(
size=0.1, origin=[keypoint.x, keypoint.y, keypoint.z])
vis_list.append(keypoints_coords)
o3d.visualization.draw_geometries(vis_list)
def BuildMsg(self):
bbox = RegionOfInterest()
bbox.x_offset = self.bbox[0]
bbox.y_offset = self.bbox[1]
bbox.height = self.bbox[3] - self.bbox[1]
bbox.width = self.bbox[2] - self.bbox[0]
personLocation = PersonLocationMsg()
personLocation.targetId = self.id
personLocation.annotationType = self.annotationType
personLocation.bbox = bbox
return personLocation
def compute_roi_from_indices(indices, width, height, padding = 30):
xvals = [ind - width*(math.floor(ind / width)) for ind in indices]
yvals = [math.floor(ind / width) for ind in indices]
roi = RegionOfInterest()
roi.x_offset = max(0, min(xvals) - padding)
roi.y_offset = max(0, min(yvals) - padding)
roi.height = max(yvals) - roi.y_offset + padding
if roi.height + roi.y_offset > height:
roi.height = height - roi.y_offset
roi.width = max(xvals) - roi.x_offset + padding
if roi.width + roi.x_offset > width:
roi.width = width - roi.x_offset
roi.do_rectify = 0
return roi
def publish(self, boxes, classes, labels, seg_map, fps=0):
# init detection message
msg = Segmentation()
boxes = []
for i in range(boxes.shape[0]):
class_name = label[i]
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax-xmin)/2.0
box.y_offset = ymin + (ymax-ymin)/2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill segmentation message
msg.boxes.append(box)
msg.class_names.append(class_name)
msg.seg_map = self._bridge.cv2_to_imgmsg(seg_map, encoding="passthrough")
msg.fps = fps
# publish detection message
self.SegPub.publish(msg)
def publish_roi(self):
roi_box = self.track_window
# roi_box = self.track_box
try:
roi_box = self.cvBox2D_to_cvRect(roi_box)
except:
return
# Watch out for negative offsets
roi_box[0] = max(0, roi_box[0])
roi_box[1] = max(0, roi_box[1])
try:
roi = RegionOfInterest()
roi.x_offset = int(roi_box[0])
roi.y_offset = int(roi_box[1])
roi.width = int(roi_box[2])
roi.height = int(roi_box[3])
self.roi_pub.publish(roi)
except:
rospy.loginfo("Publishing ROI failed")
def match_template(self, cv_image):
frame = np.array(cv_image, dtype=np.uint8)
W,H = frame.shape[1], frame.shape[0]
w,h = self.template.shape[1], self.template.shape[0]
width = W - w + 1
height = H - h + 1
# Make sure that the template image is smaller than the source
if W < w or H < h:
rospy.loginfo( "Template image must be smaller than video frame." )
return False
if frame.dtype != self.template.dtype:
rospy.loginfo("Template and video frame must have same depth and number of channels.")
return False
# Create copies of the images to modify
frame_copy = frame.copy()
template_copy = self.template.copy()
# Down pyramid the images
for k in range(self.numDownPyrs):
# Start with the source image
W = (W + 1) / 2
H = (H + 1) / 2
frame_small = np.array([H, W], dtype=frame.dtype)
frame_small = cv2.pyrDown(frame_copy)
# frame_window = "PyrDown " + str(k)
# cv.NamedWindow(frame_window, cv.CV_NORMAL)
# cv.ShowImage(frame_window, cv.fromarray(frame_small))
# cv.ResizeWindow(frame_window, 640, 480)
# Prepare for next loop, if any
frame_copy = frame_small.copy()
#Next, do the target
w = (w + 1) / 2
h = (h + 1) / 2
template_small = np.array([h, w], dtype=self.template.dtype)
template_small = cv2.pyrDown(template_copy)
# template_window = "Template PyrDown " + str(k)
# cv.NamedWindow(template_window, cv.CV_NORMAL)
# cv.ShowImage(template_window, cv.fromarray(template_small))
# cv.ResizeWindow(template_window, 640, 480)
# Prepare for next loop, if any
template_copy = template_small.copy()
# Perform the match on the shrunken images
small_frame_width = frame_copy.shape[1]
small_frame_height = frame_copy.shape[0]
small_template_width = template_copy.shape[1]
small_template_height = template_copy.shape[0]
result_width = small_frame_width - small_template_width + 1
result_height = small_frame_height - small_template_height + 1
result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1)
result = np.array(result_mat, dtype = np.float32)
cv2.matchTemplate(frame_copy, template_copy, cv.CV_TM_CCOEFF_NORMED, result)
cv2.imshow("Result", result)
return (0, 0, 100, 100)
# # Find the best match location
# (minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result)
#
# # Transform point back to original image
# target_location = Point()
# target_location.x, target_location.y = maxLoc
#
# return (target_location.x, target_location.y, w, h)
# Find the top match locations
locations = self.MultipleMaxLoc(result, self.numMaxima)
foundPointsList = list()
confidencesList = list()
W,H = frame.shape[1], frame.shape[0]
w,h = self.template.shape[1], self.template.shape[0]
# Search the large images at the returned locations
for currMax in range(self.numMaxima):
# Transform the point to its corresponding point in the larger image
#locations[currMax].x *= int(pow(2.0, self.numDownPyrs))
#locations[currMax].y *= int(pow(2.0, self.numDownPyrs))
locations[currMax].x += w / 2
locations[currMax].y += h / 2
searchPoint = locations[currMax]
print "Search Point", searchPoint
# If we are searching for multiple targets and we have found a target or
# multiple targets, we don't want to search in the same location(s) again
# if self.findMultipleTargets and len(foundPointsList) != 0:
# thisTargetFound = False
# numPoints = len(foundPointsList)
#
# for currPoint in range(numPoints):
# foundPoint = foundPointsList[currPoint]
# if (abs(searchPoint.x - foundPoint.x) <= self.searchExpansion * 2) and (abs(searchPoint.y - foundPoint.y) <= self.searchExpansion * 2):
# thisTargetFound = True
# break
#
# # If the current target has been found, continue onto the next point
# if thisTargetFound:
# continue
# Set the source image's ROI to slightly larger than the target image,
# centred at the current point
searchRoi = RegionOfInterest()
searchRoi.x_offset = searchPoint.x - w / 2 - self.searchExpansion
searchRoi.y_offset = searchPoint.y - h / 2 - self.searchExpansion
searchRoi.width = w + self.searchExpansion * 2
searchRoi.height = h + self.searchExpansion * 2
#print (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height)
# Make sure ROI doesn't extend outside of image
if searchRoi.x_offset < 0:
searchRoi.x_offset = 0
if searchRoi.y_offset < 0:
searchRoi.y_offset = 0
if (searchRoi.x_offset + searchRoi.width) > (W - 1):
numPixelsOver = (searchRoi.x_offset + searchRoi.width) - (W - 1)
print "NUM PIXELS OVER", numPixelsOver
searchRoi.width -= numPixelsOver
if (searchRoi.y_offset + searchRoi.height) > (H - 1):
numPixelsOver = (searchRoi.y_offset + searchRoi.height) - (H - 1)
searchRoi.height -= numPixelsOver
mask = (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height)
frame_mat = cv.fromarray(frame)
searchImage = cv.CreateMat(searchRoi.height, searchRoi.width, cv.CV_8UC3)
searchImage = cv.GetSubRect(frame_mat, mask)
searchArray = np.array(searchImage, dtype=np.uint8)
# Perform the search on the large images
result_width = searchRoi.width - w + 1
result_height = searchRoi.height - h + 1
result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1)
result = np.array(result_mat, dtype = np.float32)
cv2.matchTemplate(searchArray, self.template, cv.CV_TM_CCOEFF_NORMED, result)
# Find the best match location
(minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result)
maxValue *= 100
# Transform point back to original image
target_location = Point()
target_location.x, target_location.y = maxLoc
target_location.x += searchRoi.x_offset - w / 2 + self.searchExpansion
target_location.y += searchRoi.y_offset - h / 2 + self.searchExpansion
if maxValue >= self.matchPercentage:
# Add the point to the list
foundPointsList.append(maxLoc)
confidencesList.append(maxValue)
# If we are only looking for a single target, we have found it, so we
# can return
if not self.findMultipleTargets:
break
if len(foundPointsList) == 0:
rospy.loginfo("Target was not found to required confidence")
return (target_location.x, target_location.y, w, h)
