def GenerateFeaturePoints(self, frame, points, point_type):
feature_points = FeaturePoints()
feature_points.type.data = point_type
# Calculate center of points
[cpoint_x, cpoint_y] = np.int32(np.average(points, axis=0))
feature_points.center = Point2D(cpoint_x, cpoint_y)
# Creates a circle outline at the center of points
cv2.circle(frame, (cpoint_x, cpoint_y), radius=2,
color=(248, 123, 191), thickness=1)
feature_points.points = []
# Fill array of points with points
for i in range(points.shape[0]):
center_point = Point2D(points[i, 0], points[i, 1])
feature_points.points.append(center_point)
rospy.loginfo(feature_points.points)
poly_points = np.int32([points])
# Draw lines between the points
cv2.polylines(frame, poly_points, color=(
248, 123, 191), thickness=1, isClosed=True)
# Set zoom level
rospy.loginfo(np.float32(points))
size = self.GetShapeSize(points)
if size == 0.0:
# Features are out of frame, don't do anything
feature_points.zoom.data = 1.0
feature_points.yaw_angle.data = 0.0
return feature_points
feature_points.zoom.data = self.viewpoint_size / size
# rospy.loginfo("Zoom: " + str(feature_points.zoom))
# Set angle
tform = sktf.estimate_transform('similarity', np.float32(points), np.float32(self.viewpoint_points))
rospy.loginfo(tform.rotation * 180 / math.pi)
if (tform.rotation > math.pi / 2):
angle = -1 * (math.pi - tform.rotation)
else:
angle = tform.rotation
# angle = tform.rotation
# rospy.loginfo("Yaw Angle: " + str(angle * 180 / math.pi))
feature_points.yaw_angle.data = angle
# return points
return feature_points
def create_roi(x_pos, y_pos, radius=ROI().roi.radius, roi_id=0):
"""Create ROI with predefined positions"""
roi = ROI()
roi.roi.center = Point2D(x_pos, y_pos)
roi.roi.radius = radius
roi.id = roi_id
return roi
def sub_point_weighted(self, points, weights):
point = Point2D()
if len(points) != len(weights):
return point
for i in range(len(points)):
point.x += points[i].x * weights[i]
point.y += points[i].y * weights[i]
return point
def PublishPoints(self, frame, points, point_type):
feature_points = FeaturePoints()
feature_points.type.data = point_type
# Calculate center of points
[x, y] = np.int32(np.average(points, axis=0))
feature_points.center = Point2D(x, y)
feature_points.points = []
# Fill array of points with points
for i in range(points.shape[0]):
center_point = Point2D(points[i, 0], points[i, 1])
feature_points.points.append(center_point)
print(feature_points.points)
# Publish points
self.pub.publish(feature_points)
def pub_contour():
global final_list
merged_cont = Contour()
merged_cont.points = []
for i in range(len(final_list)):
point = Point2D()
point.x = final_list[i][0]
point.y = final_list[i][1]
merged_cont.points.append(point)
print('publishing')
pub.publish(merged_cont)
def waypoints():
pub = rospy.Publisher('/contour_surround', Contour, queue_size=5)
rospy.init_node('contour_surround', anonymous=True)
rate = rospy.Rate(1) # 10hz
contour = Contour()
contour.points = []
for i in range(4):
point = Point2D()
point.x = 1.9
point.y = 0.1 * i
contour.points.append(point)
print(contour.points)
while not rospy.is_shutdown():
pub.publish(contour)
rate.sleep()
def to_roi_msg(self):
roi = ROI(id=self.__id, type=self.__type)
roi.roi.radius = max(self.__rect.width / 2.0, self.__rect.height / 2.0)
roi.roi.center = Point2D(x=self.__rect.x, y=self.__rect.y)
return roi
def main(argv):
in_bag = rosbag.Bag(argv[0], 'r')
out_bag = rosbag.Bag(argv[1], 'w')
with in_bag, out_bag:
start_time = Time(rospy.Time(in_bag.get_start_time()))
out_bag.write('/bc/start_time', start_time)
for topic, msg, t in in_bag.read_messages(topics=face_topics +
audio_topics):
pid = re.findall(r'\d', topic)[0]
if topic in face_topics:
if not msg.landmarks3D:
continue
ff_msg = FaceFeatures()
# Success
ff_msg.success = True
# Landmarks
# 2D landmarks not provided
# Landmarks 3D
ff_msg.landmarks_3D = []
for landmark in msg.landmarks3D:
ff_msg.landmarks_3D.append(
Vector3(landmark.x, landmark.y, landmark.z))
# Landmarks visibilities (unused)
ff_msg.landmarks_visibilities = []
# Gaze angle
ff_msg.gaze_angle = Point2D(msg.gazeAngle.x, msg.gazeAngle.y)
# Gaze left
direction = msg.gazeDirection0
ff_msg.gaze_left = Vector3(direction.x, direction.y,
direction.z)
# Gaze right
direction = msg.gazeDirection1
ff_msg.gaze_right = Vector3(direction.x, direction.y,
direction.z)
# Head pose
pose = msg.head_pose
point = Point(pose.x, pose.y, pose.z)
pitch, yaw, roll = pose.rot_x, pose.rot_y, pose.rot_z
x, y, z, w = quaternion_from_euler(roll, pitch, yaw)
quaternion = Quaternion(x, y, z, w)
ff_msg.head_pose = Pose(point, quaternion)
# au name (unused)
ff_msg.au_name = ''
# au intensity (unused)
ff_msg.au_intensity = []
array = FaceFeaturesArray()
array.header = msg.header
array.features = [ff_msg]
out_bag.write('/pid' + pid + '/face_features', array, t)
elif topic in audio_topics:
ad_msg = AudioData()
ad_msg.header.stamp = msg.time
ad_msg.data = msg.chunk
ad_msg.sample_rate = 16000
ad_msg.num_channels = 1
ad_msg.sample_width = 2
ad_msg.is_bigendian = False
out_bag.write('/pid' + pid + '/audio/data', ad_msg, t)
def sub_point(self, p1, p2, t):
pt = Point2D()
pt.x = p1.x + (p2.x - p1.x) * t
pt.y = p1.y + (p2.y - p1.y) * t
return pt
def create_roi(x_pos, y_pos, radius):
"""Create ROI from coordinates"""
roi = ROI()
roi.roi.center = Point2D(x_pos, y_pos)
roi.roi.radius = radius
return roi