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