def estimate(self, faces, view_matrix, camera_matrix, dist_coeffs):
"""Estimate the head pose of the given face (z forward for rendering)"""
for f in faces:
if f.is_confirmed():
if "facial_landmarks" in f.features:
points_2d = f.features["facial_landmarks"].data
if f.pose is not None:
world_transform = f.pose.transform()
sensor_pose = Vector6D().from_transform(
np.dot(
np.dot(np.linalg.inv(world_transform),
world_transform),
np.linalg.inv(self.offset)))
r = sensor_pose.rotation().to_array()
t = sensor_pose.position().to_array()
self.__add_offset(r, -RX_OFFSET, -RY_OFFSET,
-RZ_OFFSET)
rvec = self.__euler2rodrigues(r)
success, rvec, tvec, _ = cv2.solvePnPRansac(
self.model_3d,
points_2d,
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE,
useExtrinsicGuess=True,
rvec=rvec,
tvec=t)
success = self.__check_consistency(tvec, rvec)
else:
success, rvec, tvec, _ = cv2.solvePnPRansac(
self.model_3d,
points_2d,
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
success = self.__check_consistency(tvec, rvec)
if success:
r = self.__rodrigues2euler(rvec)
#print r
r[2] = .0
self.__add_offset(r, RX_OFFSET, RY_OFFSET, RZ_OFFSET)
sensor_pose = Vector6D(x=tvec[0][0],
y=tvec[1][0],
z=tvec[2][0],
rx=r[0][0],
ry=abs(r[1][0]),
rz=r[2][0])
world_pose = Vector6D().from_transform(
np.dot(view_matrix, sensor_pose.transform()))
#print world_pose.rot
f.update_pose(world_pose.position(),
rotation=world_pose.rotation())
def observation_callback(self, ar_marker_msgs):
"""
"""
all_nodes = []
header = ar_marker_msgs.header
for marker in ar_marker_msgs.markers:
if not(marker.id in self.blacklist_id):
if not (marker.id in self.id_link):
self.new_node(marker)
# print self.id_link
# print self.id_link.keys()
id = self.id_link[marker.id]
pose = Vector6D().from_msg(marker.pose.pose)
header = marker.header
if self.ar_nodes[id].pose is None:
self.ar_nodes[id].pose = Vector6DStable(x=pose.pos.x, y=pose.pos.y, z=pose.pos.z,
rx=pose.rot.x, ry=pose.rot.y, rz=pose.rot.z, time=header.stamp)
else:
self.ar_nodes[id].pose.pos.update_no_kalmann(x=pose.pos.x, y=pose.pos.y, z=pose.pos.z, time=header.stamp)
self.ar_nodes[id].pose.rot.update_no_kalmann(x=pose.rot.x, y=pose.rot.y, z=pose.rot.z, time=header.stamp)
all_nodes.append(self.ar_nodes[id])
self.world_publisher.publish(self.ar_nodes.values(), [],header)
# print("pub")
if self.publish_tf is True:
self.tf_bridge.publish_tf_frames(self.ar_nodes.values(), [], header)
def observation_callback(self, ar_marker_msgs):
"""
"""
if self.robot_camera is not None:
header = ar_marker_msgs.header
header.stamp = rospy.Time()
header.frame_id = self.global_frame_id
success, view_pose = self.tf_bridge.get_pose_from_tf(self.global_frame_id, self.camera_frame_id)
if success is not True:
rospy.logwarn("[ar_perception] The camera sensor is not localized in world space (frame '{}'), please check if the sensor frame is published in /tf".format(self.global_frame_id))
else:
all_nodes = []
for marker in ar_marker_msgs.markers:
if marker.id in self.ar_nodes:
pose = Vector6D().from_msg(marker.pose.pose)
if self.ar_nodes[marker.id].pose is None:
self.ar_nodes[marker.id].pose = Vector6DStable(x=pose.pos.x, y=pose.pos.y, z=pose.pos.z,
rx=pose.rot.x, ry=pose.rot.y, rz=pose.rot.z, time=header.stamp)
else:
self.ar_nodes[marker.id].pose.pos.update(x=pose.pos.x, y=pose.pos.y, z=pose.pos.z, time=header.stamp)
self.ar_nodes[marker.id].pose.rot.update(x=pose.rot.x, y=pose.rot.y, z=pose.rot.z, time=header.stamp)
all_nodes.append(self.ar_nodes[marker.id])
self.world_publisher.publish(all_nodes, [], header)
if self.publish_viz is True:
self.marker_publisher.publish(all_nodes, header)
if self.publish_tf is True:
self.tf_bridge.publish_tf_frames(all_nodes, [], header)
def publish_tf_frames(self, tracks, events, time=None):
for track in tracks:
if track.is_located() is True and track.is_confirmed() is True:
self.tf_bridge.publish_pose_to_tf(track.pose, self.global_frame_id, track.id, time=time)
for event in events:
if event.is_located() is True:
frame = event.subject+event.description+event.object
pose = Vector6D(x=event.point.x, y=event.point.y, z=event.point.z)
self.tf_bridge.publish_pose_to_tf(pose, self.global_frame_id, frame, time=time)
def estimate(self, objects, view_matrix, camera_matrix, dist_coeffs):
""" """
for o in objects:
if o.bbox.depth is not None:
fx = camera_matrix[0][0]
fy = camera_matrix[1][1]
cx = camera_matrix[0][2]
cy = camera_matrix[1][2]
c = o.bbox.center()
z = o.bbox.depth
x = (c.x - cx) * z / fx
y = (c.y - cy) * z / fy
sensor_transform = Vector6D(x=x, y=y, z=z).transform()
world_pose = Vector6D().from_transform(
np.dot(view_matrix, sensor_transform))
position = world_pose.position()
rotation = Vector3D()
o.update_pose(position, rotation)
def observation_callback(self, ar_track_msg):
scene_changes = uwds3_msgs.msg.SceneChangesStamped()
scene_changes.world = "tracks"
scene_changes.header = ar_track_msg.header
scene_changes.header.stamp = rospy.Time().now()
scene_changes.header.frame_id = self.global_frame_id
if self.camera_info is not None:
if len(ar_track_msg.markers) > 0:
scene_changes.header.stamp = ar_track_msg.header.stamp
for object in ar_track_msg.markers:
success, view_pose = self.get_pose_from_tf2(
self.global_frame_id,
object.header.frame_id) #, ar_track_msg.header.stamp)
if success is True:
position = object.pose.pose.position
orientation = object.pose.pose.orientation
obj_sensor_pose = Vector6D(x=position.x,
y=position.y,
z=position.z).from_quaternion(
orientation.x,
orientation.y,
orientation.z,
orientation.w)
world_pose = view_pose + obj_sensor_pose
self.tracks[
object.id].pose_stamped.header = scene_changes.header
self.tracks[
object.
id].pose_stamped.pose.pose.position.x = world_pose.position(
).x
self.tracks[
object.
id].pose_stamped.pose.pose.position.y = world_pose.position(
).y
self.tracks[
object.
id].pose_stamped.pose.pose.position.z = world_pose.position(
).z
q = world_pose.quaternion()
self.tracks[
object.id].pose_stamped.pose.pose.orientation.x = q[0]
self.tracks[
object.id].pose_stamped.pose.pose.orientation.y = q[1]
self.tracks[
object.id].pose_stamped.pose.pose.orientation.z = q[2]
self.tracks[
object.id].pose_stamped.pose.pose.orientation.w = q[3]
scene_changes.changes.nodes.append(self.tracks[object.id])
self.tracks_publisher.publish(scene_changes)
def publish_tf_frames(self, tracks, events, header):
for track in tracks:
if track.is_located() is True and track.is_confirmed() is True:
self.publish_pose_to_tf(track.pose,
header.frame_id,
self.prefix + track.id,
header=header)
for event in events:
if event.is_located() is True:
frame = event.subject + event.description + event.object
pose = Vector6D(x=event.point.x,
y=event.point.y,
z=event.point.z)
self.publish_pose_to_tf(pose,
header.frame_id,
self.prefix + frame,
header=header)
def __init__(self):
face_pose_str = rospy.get_param("~face_global_pose", "0 0 0 0 0 0")
float_list = np.array([float(i) for i in face_pose_str.split()])
face_pose = Vector6D(x=float_list[0],
y=float_list[1],
z=float_list[2],
rx=float_list[3],
ry=float_list[4],
rz=float_list[5])
self.fake_face = SceneNode(label="face", pose=face_pose)
self.fake_face.shapes.append(Sphere(d=0.15))
self.fake_face.id = "face"
self.global_frame_id = rospy.get_param("~global_frame_id", "odom")
self.rgb_camera_info_topic = rospy.get_param("~rgb_camera_info_topic",
"")
self.tf_bridge = TfBridge()
self.world_publisher = WorldPublisher("human_tracks")
self.image_info_sub = rospy.Subscriber(
self.rgb_camera_info_topic,
CameraInfo,
self.callback,
queue_size=DEFAULT_SENSOR_QUEUE_SIZE)
def get_pose_from_tf2(self, source_frame, target_frame, time=None):
try:
if time is not None:
trans = self.tf_buffer.lookup_transform(
source_frame, target_frame, time)
else:
trans = self.tf_buffer.lookup_transform(
source_frame, target_frame, rospy.Time(0))
x = trans.transform.translation.x
y = trans.transform.translation.y
z = trans.transform.translation.z
rx = trans.transform.rotation.x
ry = trans.transform.rotation.y
rz = trans.transform.rotation.z
rw = trans.transform.rotation.w
pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw)
return True, pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logwarn("[perception] Exception occured: {}".format(e))
return False, None
def handle_perspective_taking(self, req):
camera = HumanCamera()
view_pose = Vector6D().from_msg(req.point_of_view.pose)
egocentric_relations = []
if req.use_target is True:
target_point = Vector3D().from_msg(req.target.point)
_, _, _, visible_nodes = self.internal_simulator.get_camera_view(view_pose, camera, target=target_point)
else:
_, _, _, visible_nodes = self.internal_simulator.get_camera_view(view_pose, camera)
for node1 in visible_nodes:
for node2 in visible_nodes:
if node1 != node2:
bbox1 = node1.bbox
bbox2 = node2.bbox
if is_right_of(bbox1, bbox2) is True:
description = node1.description+"("+node1.id[:6]+") is right of "+node2.description+"("+node2.id[:6]+")"
egocentric_relations.append(Fact(node1.id, description, predicate="right_of", object=node2.id))
if is_left_of(bbox1, bbox2) is True:
description = node1.description+"("+node1.id[:6]+") is left of "+node2.description+"("+node2.id[:6]+")"
egocentric_relations.append(Fact(node1.id, description, predicate="left_of", object=node2.id))
if is_behind(bbox1, bbox2) is True:
description = node1.description+"("+node1.id[:6]+") is behind "+node2.description+"("+node2.id[:6]+")"
egocentric_relations.append(Fact(node1.id, description, predicate="behind", object=node2.id))
return visible_nodes, egocentric_relations, True, ""
def __init__(self,
use_gui,
cad_models_additional_search_path,
env_urdf_file_path,
static_entities_config_filename,
robot_urdf_file_path,
global_frame_id,
base_frame_id,
position_tolerance=0.005):
self.tf_bridge = TfBridge()
self.entity_map = {}
self.entity_id_map = {}
self.reverse_entity_id_map = {}
self.joint_id_map = {}
self.reverse_joint_id_map = {}
self.constraint_id_map = {}
self.markers_id_map = {}
self.position_tolerance = position_tolerance
self.global_frame_id = global_frame_id
self.base_frame_id = base_frame_id
self.robot_urdf_file_path = robot_urdf_file_path
self.use_gui = use_gui
if self.use_gui is True:
self.client_simulator_id = p.connect(p.GUI)
else:
self.client_simulator_id = p.connect(p.DIRECT)
if cad_models_additional_search_path != "":
p.setAdditionalSearchPath(cad_models_additional_search_path)
self.static_nodes = []
if static_entities_config_filename != "":
with open(static_entities_config_filename, 'r') as stream:
static_entities = yaml.load(stream)
for entity in static_entities:
start_pose = Vector6D(x=entity["position"]["x"],
y=entity["position"]["y"],
z=entity["position"]["z"],
rx=entity["orientation"]["x"],
ry=entity["orientation"]["x"],
rz=entity["orientation"]["z"])
success, static_node = self.load_urdf(
entity["id"],
entity["file"],
start_pose,
label=entity["label"],
description=entity["description"],
static=True)
if success:
self.static_nodes.append(static_node)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
self.robot_loaded = False
self.joint_state_subscriber = rospy.Subscriber(
"/joint_states",
JointState,
self.joint_states_callback,
queue_size=1)
def draw(self,
image,
color,
thickness=1,
view_matrix=None,
camera_matrix=None,
dist_coeffs=None):
"""Draws the track"""
if self.is_confirmed():
track_color = (0, 200, 0, 0)
text_color = (50, 50, 50)
else:
if self.is_occluded():
track_color = (0, 0, 200, 0.3)
text_color = (250, 250, 250)
else:
track_color = (200, 0, 0, 0.3)
text_color = (250, 250, 250)
if self.is_confirmed():
if self.is_located() and self.is_confirmed():
if view_matrix is not None and \
camera_matrix is not None and \
dist_coeffs is not None:
sensor_pose = Vector6D().from_transform(
np.dot(np.linalg.inv(view_matrix),
self.pose.transform()))
rot = sensor_pose.rotation().to_array()
depth = sensor_pose.position().z
# for opencv convention
R = euler_matrix(rot[0][0], rot[1][0], rot[2][0], "rxyz")
rvec = cv2.Rodrigues(R[:3, :3])[0]
cv2.putText(image, "{0:.3}m".format(depth),
(self.bbox.xmin + 5, self.bbox.ymin - 5),
cv2.FONT_HERSHEY_SIMPLEX, .6, (255, 255, 255),
2)
cv2.drawFrameAxes(image, camera_matrix, dist_coeffs, rvec,
sensor_pose.position().to_array(), 0.1)
cv2.rectangle(image, (self.bbox.xmin, self.bbox.ymax - 26),
(self.bbox.xmax, self.bbox.ymax), (200, 200, 200),
-1)
# if self.mask is not None:
# mask = np.full((image.shape[0], image.shape[1], 1), 255)
# xmin = int(self.bbox.xmin)
# ymin = int(self.bbox.ymin)
# xmax = int(self.bbox.xmax)
# ymax = int(self.bbox.ymax)
# mask[ymin:ymax, xmin:xmax] = self.mask
# contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# cv2.drawContours(image, contours, 0, track_color, thickness)
self.bbox.draw(image, track_color, 2)
self.bbox.draw(image, text_color, 1)
cv2.putText(image, "{}".format(self.id[:6]),
(self.bbox.xmax - 60, self.bbox.ymax - 8),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1)
cv2.putText(image, self.label,
(self.bbox.xmin + 5, self.bbox.ymax - 8),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1)
if "facial_landmarks" in self.features:
self.features["facial_landmarks"].draw(image, track_color,
thickness)
else:
self.bbox.draw(image, track_color, 1)
def perception_pipeline(self,
view_matrix,
rgb_image,
depth_image=None,
time=None):
""" """
######################################################
# Detection
######################################################
detections = []
image_height, image_width, _ = rgb_image.shape
if depth_image is not None:
if self.table_depth is None:
x = int(image_width * (1 / 2.0))
y = int(image_height * (3 / 4.0))
if not math.isnan(depth_image[y, x]):
self.table_depth = depth_image[y, x] / 1000.0
table_depth = self.table_depth
else:
table_depth = None
table_detection = Detection(0, int(image_height / 2.0),
int(image_width), image_height, "table",
1.0, table_depth)
view_pose = Vector6D().from_transform(view_matrix)
if depth_image is not None:
if self.table_shape is None:
fx = self.camera_matrix[0][0]
fy = self.camera_matrix[1][1]
cx = self.camera_matrix[0][2]
cy = self.camera_matrix[1][2]
x = int(image_width * (1 / 2.0))
y = int(image_height * (3 / 4.0))
if not math.isnan(depth_image[y, x]):
z = depth_image[y, x] / 1000.0
x = (x - cx) * z / fx
y = (y - cy) * z / fy
table_border = Vector6D(x=x, y=y, z=z)
x = int(image_width * (1 / 2.0))
y = int(image_height * (1 / 2.0))
if not math.isnan(depth_image[y, x]):
z = depth_image[y, x] / 1000.0
x = (x - cx) * z / fx
y = (y - cy) * z / fy
table_center = Vector6D(x=x, y=y, z=z)
table_length = (image_width - cx) * z / fx
table_width = 2.0 * math.sqrt(
pow(table_border.pos.x - table_center.pos.x, 2) +
pow(table_border.pos.y - table_center.pos.y, 2) +
pow(table_border.pos.z - table_center.pos.z, 2))
center_in_world = view_pose + table_center
real_z = center_in_world.position().z
print center_in_world.position()
self.table_shape = Box(table_width, table_length,
real_z)
self.table_shape.pose.pos.z = -real_z / 2.0
self.table_shape.color = self.shape_estimator.compute_dominant_color(
rgb_image, table_detection.bbox)
if self.frame_count == 0:
person_detections = self.person_detector.detect(
rgb_image, depth_image=depth_image)
detections = person_detections
elif self.frame_count == 1:
object_detections = self.foreground_detector.detect(
rgb_image, depth_image=depth_image)
detections = object_detections
else:
detections = []
####################################################################
# Features estimation
####################################################################
self.color_features_estimator.estimate(rgb_image, detections)
######################################################
# Tracking
######################################################
if self.frame_count == 0:
object_tracks = self.object_tracker.update(rgb_image, [],
depth_image=depth_image)
person_tracks = self.person_tracker.update(rgb_image,
person_detections,
depth_image=depth_image)
elif self.frame_count == 1:
object_tracks = self.object_tracker.update(rgb_image,
object_detections,
depth_image=depth_image)
person_tracks = self.person_tracker.update(rgb_image, [],
depth_image=depth_image)
else:
object_tracks = self.object_tracker.update(rgb_image, [],
depth_image=depth_image)
person_tracks = self.person_tracker.update(rgb_image, [],
depth_image=depth_image)
if self.table_track is None:
self.table_track = Track(table_detection, 1, 4, 20)
else:
self.table_track.update(table_detection)
if self.table_shape is not None:
self.table_track.shapes = [self.table_shape]
support_tracks = [self.table_track]
tracks = object_tracks + person_tracks + support_tracks
########################################################
# Pose & Shape estimation
########################################################
self.object_pose_estimator.estimate(
object_tracks + person_tracks + support_tracks, view_matrix,
self.camera_matrix, self.dist_coeffs)
self.shape_estimator.estimate(rgb_image, tracks, self.camera_matrix,
self.dist_coeffs)
########################################################
# Monitoring
########################################################
events = self.action_monitor.monitor(support_tracks, object_tracks,
person_tracks, [])
return tracks, events
def __init__(self, face_3d_model_filename):
"""HeadPoseEstimator constructor"""
self.model_3d = np.load(face_3d_model_filename) / 100.0 / 4.6889 / 2.0
self.offset = Vector6D(rx=RX_OFFSET, ry=RY_OFFSET,
rz=RZ_OFFSET).transform()
def observation_callback(self, bgr_image_msg, depth_image_msg=None):
if self.camera_info is not None:
perception_timer = cv2.getTickCount()
bgr_image = self.bridge.imgmsg_to_cv2(bgr_image_msg, "bgr8")
rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB)
if depth_image_msg is not None:
depth_image = self.bridge.imgmsg_to_cv2(depth_image_msg)
else:
depth_image = None
if self.publish_visualization_image is True:
viz_frame = bgr_image
image_height, image_width, _ = bgr_image.shape
success, view_pose = self.get_pose_from_tf2(
self.global_frame_id, self.camera_frame_id)
if success is not True:
rospy.logwarn(
"[tabletop_perception] The camera sensor is not localized in world space (frame '{}'), please check if the sensor frame is published in /tf"
.format(self.global_frame_id))
else:
view_matrix = view_pose.transform()
self.frame_count %= self.n_frame
######################################################
# Detection
######################################################
detections = []
if depth_image is not None:
if self.table_depth is None:
x = int(image_width * (1 / 2.0))
y = int(image_height * (3 / 4.0))
if not math.isnan(depth_image[y, x]):
self.table_depth = depth_image[y, x] / 1000.0
table_depth = self.table_depth
else:
table_depth = None
table_detection = Detection(0, int(image_height / 2.0),
int(image_width), image_height,
"table", 1.0, table_depth)
if depth_image is not None:
if self.table_shape is None:
fx = self.camera_matrix[0][0]
fy = self.camera_matrix[1][1]
cx = self.camera_matrix[0][2]
cy = self.camera_matrix[1][2]
x = int(image_width * (1 / 2.0))
y = int(image_height * (3 / 4.0))
if not math.isnan(depth_image[y, x]):
z = depth_image[y, x] / 1000.0
x = (x - cx) * z / fx
y = (y - cy) * z / fy
table_border = Vector6D(x=x, y=y, z=z)
x = int(image_width * (1 / 2.0))
y = int(image_height * (1 / 2.0))
if not math.isnan(depth_image[y, x]):
z = depth_image[y, x] / 1000.0
x = (x - cx) * z / fx
y = (y - cy) * z / fy
table_center = Vector6D(x=x, y=y, z=z)
table_length = (image_width - cx) * z / fx
table_width = 2.0 * math.sqrt(
pow(
table_border.pos.x -
table_center.pos.x, 2) + pow(
table_border.pos.y -
table_center.pos.y, 2) + pow(
table_border.pos.z -
table_center.pos.z, 2))
center_in_world = view_pose + table_center
real_z = center_in_world.position().z
print view_pose.position()
print center_in_world.position()
self.table_shape = Box(table_width,
table_length, real_z)
self.table_shape.pose.pos.z = -real_z / 2.0
self.table_shape.color = self.shape_estimator.compute_dominant_color(
rgb_image, table_detection.bbox)
if self.frame_count == 0:
person_detections = self.detector.detect(
rgb_image, depth_image=depth_image)
detections = person_detections
elif self.frame_count == 1:
object_detections = self.foreground_detector.detect(
rgb_image, depth_image=depth_image)
detections = object_detections
else:
detections = []
####################################################################
# Features estimation
####################################################################
self.color_features_estimator.estimate(rgb_image, detections)
######################################################
# Tracking
######################################################
if self.frame_count == 0:
object_tracks = self.object_tracker.update(
rgb_image, [], depth_image=depth_image)
person_tracks = self.person_tracker.update(
rgb_image, person_detections, depth_image=depth_image)
elif self.frame_count == 1:
object_tracks = self.object_tracker.update(
rgb_image, object_detections, depth_image=depth_image)
person_tracks = self.person_tracker.update(
rgb_image, [], depth_image=depth_image)
else:
object_tracks = self.object_tracker.update(
rgb_image, [], depth_image=depth_image)
person_tracks = self.person_tracker.update(
rgb_image, [], depth_image=depth_image)
if self.table_track is None:
self.table_track = Track(table_detection, 1, 4, 20)
else:
self.table_track.update(table_detection)
if self.table_shape is not None:
self.table_track.shapes = [self.table_shape]
table_track = [self.table_track]
tracks = object_tracks + person_tracks + table_track
########################################################
# Pose & Shape estimation
########################################################
self.object_pose_estimator.estimate(
object_tracks + person_tracks + table_track, view_matrix,
self.camera_matrix, self.dist_coeffs)
self.shape_estimator.estimate(rgb_image, tracks,
self.camera_matrix,
self.dist_coeffs)
self.frame_count += 1
######################################################
# Visualization of debug image
######################################################
perception_fps = cv2.getTickFrequency() / (cv2.getTickCount() -
perception_timer)
cv2.rectangle(viz_frame, (0, 0), (250, 40), (200, 200, 200),
-1)
perception_fps_str = "Perception fps : {:0.1f}hz".format(
perception_fps)
cv2.putText(
viz_frame, "Nb detections/tracks : {}/{}".format(
len(detections), len(tracks)), (5, 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
cv2.putText(viz_frame, perception_fps_str, (5, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
scene_changes = SceneChangesStamped()
scene_changes.world = "tracks"
header = bgr_image_msg.header
header.frame_id = self.global_frame_id
scene_changes.header = header
for track in tracks:
if self.publish_visualization_image is True:
track.draw(viz_frame, (230, 0, 120, 125), 1,
view_matrix, self.camera_matrix,
self.dist_coeffs)
if track.is_confirmed():
scene_node = track.to_msg(header)
if self.publish_tf is True:
if track.is_located() is True:
self.publish_tf_pose(scene_node.pose_stamped,
self.global_frame_id,
scene_node.id)
scene_changes.changes.nodes.append(scene_node)
if self.publish_visualization_image is True:
viz_img_msg = self.bridge.cv2_to_imgmsg(viz_frame)
self.visualization_publisher.publish(viz_img_msg)
self.tracks_publisher.publish(scene_changes)