class OfflineTfBuffer(): def __init__(self, transforms): self.t_front = transforms[0].header.stamp self.t_back = transforms[-1].header.stamp self.buff = Buffer(cache_time=(self.t_back - self.t_front), debug=False) for transform in transforms: self.buff.set_transform(transform, '/auth') def LookupTransform(self, target_frame, source_frame, times): return [ self.buff.lookup_transform(target_frame, source_frame, t) for t in times ] def AvailableTimeRange(self, target_frame, source_frame, times): if times[0] > self.t_back or times[-1] < self.t_front: return [] idx_front = 0 idx_back = len(times) - 1 while times[idx_front] < self.t_front: idx_front += 1 while times[idx_back] > self.t_back: idx_back -= 1 return (idx_front, idx_back + 1) def CanTransform(self, target_frame, source_frame, times): return self.buff.can_transform( target_frame, source_frame, times[0]) and self.buff.can_transform( target_frame, source_frame, times[-1])
class LandmarkMethodROS(LandmarkMethodBase): def __init__(self, img_proc=None): super(LandmarkMethodROS, self).__init__(device_id_facedetection=rospy.get_param( "~device_id_facedetection", default="cuda:0")) self.subject_tracker = FaceEncodingTracker() if rospy.get_param( "~use_face_encoding_tracker", default=True) else SequentialTracker() self.bridge = CvBridge() self.__subject_bridge = SubjectListBridge() self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link") self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame") self.tf2_broadcaster = TransformBroadcaster() self.tf2_buffer = Buffer() self.tf2_listener = TransformListener(self.tf2_buffer) self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze") self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True) self.pose_stabilizers = {} # Introduce scalar stabilizers for pose. try: if img_proc is None: tqdm.write("Wait for camera message") cam_info = rospy.wait_for_message("/camera_info", CameraInfo, timeout=None) self.img_proc = PinholeCameraModel() # noinspection PyTypeChecker self.img_proc.fromCameraInfo(cam_info) else: self.img_proc = img_proc if np.array_equal( self.img_proc.intrinsicMatrix().A, np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])): raise Exception( 'Camera matrix is zero-matrix. Did you calibrate' 'the camera and linked to the yaml file in the launch file?' ) tqdm.write("Camera message received") except rospy.ROSException: raise Exception("Could not get camera info") # multiple person images publication self.subject_pub = rospy.Publisher("/subjects/images", MSG_SubjectImagesList, queue_size=3) # multiple person faces publication for visualisation self.subject_faces_pub = rospy.Publisher("/subjects/faces", Image, queue_size=3) Server(ModelSizeConfig, self._dyn_reconfig_callback) # have the subscriber the last thing that's run to avoid conflicts self.color_sub = rospy.Subscriber("/image", Image, self.process_image, buff_size=2**24, queue_size=3) def _dyn_reconfig_callback(self, config, _): self.model_points /= (self.model_size_rescale * self.interpupillary_distance) self.model_size_rescale = config["model_size"] self.interpupillary_distance = config["interpupillary_distance"] self.model_points *= (self.model_size_rescale * self.interpupillary_distance) self.head_pitch = config["head_pitch"] return config def process_image(self, color_msg): color_img = gaze_tools.convert_image(color_msg, "bgr8") timestamp = color_msg.header.stamp self.update_subject_tracker(color_img) if not self.subject_tracker.get_tracked_elements(): tqdm.write("\033[2K\033[1;31mNo face found\033[0m", end="\r") return self.subject_tracker.update_eye_images(self.eye_image_size) final_head_pose_images = [] for subject_id, subject in self.subject_tracker.get_tracked_elements( ).items(): if subject.left_eye_color is None or subject.right_eye_color is None: continue if subject_id not in self.pose_stabilizers: self.pose_stabilizers[subject_id] = [ Stabilizer(state_num=2, measure_num=1, cov_process=0.1, cov_measure=0.1) for _ in range(6) ] success, head_rpy, translation_vector = self.get_head_pose( subject.marks, subject_id) if success: # Publish all the data self.publish_pose(timestamp, translation_vector, head_rpy, subject_id) if self.visualise_headpose: # pitch roll yaw trans_msg = self.tf2_buffer.lookup_transform( self.ros_tf_frame, self.tf_prefix + "/head_pose_estimated" + str(subject_id), timestamp) rotation = trans_msg.transform.rotation euler_angles_head = list( transformations.euler_from_quaternion( [rotation.x, rotation.y, rotation.z, rotation.w])) euler_angles_head = gaze_tools.limit_yaw(euler_angles_head) face_image_resized = cv2.resize( subject.face_color, dsize=(224, 224), interpolation=cv2.INTER_CUBIC) final_head_pose_images.append( LandmarkMethodROS.visualize_headpose_result( face_image_resized, gaze_tools.get_phi_theta_from_euler( euler_angles_head))) if len(self.subject_tracker.get_tracked_elements().items()) > 0: self.publish_subject_list( timestamp, self.subject_tracker.get_tracked_elements()) if len(final_head_pose_images) > 0: headpose_image_ros = self.bridge.cv2_to_imgmsg( np.hstack(final_head_pose_images), "bgr8") headpose_image_ros.header.stamp = timestamp self.subject_faces_pub.publish(headpose_image_ros) def get_head_pose(self, landmarks, subject_id): """ We are given a set of 2D points in the form of landmarks. The basic idea is that we assume a generic 3D head model, and try to fit the 2D points to the 3D model based on the Levenberg-Marquardt optimization. We can use OpenCV's implementation of SolvePnP for this. This method is inspired by http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/ We are planning to replace this with our own head pose estimator. :param landmarks: Landmark positions to be used to determine head pose :param subject_id: ID of the subject that corresponds to the given landmarks :return: success - Whether the pose was successfully extracted rotation_vector - rotation vector that along with translation vector brings points from the model coordinate system to the camera coordinate system translation_vector - see rotation_vector """ camera_matrix = self.img_proc.intrinsicMatrix() dist_coeffs = self.img_proc.distortionCoeffs() try: success, rodrigues_rotation, translation_vector, _ = cv2.solvePnPRansac( self.model_points, landmarks.reshape(len(self.model_points), 1, 2), cameraMatrix=camera_matrix, distCoeffs=dist_coeffs, flags=cv2.SOLVEPNP_DLS) except cv2.error as e: tqdm.write( '\033[2K\033[1;31mCould not estimate head pose: {}\033[0m'. format(e), end="\r") return False, None, None if not success: tqdm.write( '\033[2K\033[1;31mUnsuccessful in solvingPnPRanscan\033[0m', end="\r") return False, None, None # this is generic point stabiliser, the underlying representation doesn't matter rotation_vector, translation_vector = self.apply_kalman_filter_head_pose( subject_id, rodrigues_rotation, translation_vector / 1000.0) rotation_vector[0] += self.head_pitch _rotation_matrix, _ = cv2.Rodrigues(rotation_vector) _rotation_matrix = np.matmul( _rotation_matrix, np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) _m = np.zeros((4, 4)) _m[:3, :3] = _rotation_matrix _m[3, 3] = 1 _rpy_rotation = np.array(transformations.euler_from_matrix(_m)) return success, _rpy_rotation, translation_vector def apply_kalman_filter_head_pose(self, subject_id, rotation_vector_unstable, translation_vector_unstable): stable_pose = [] pose_np = np.array( (rotation_vector_unstable, translation_vector_unstable)).flatten() for value, ps_stb in zip(pose_np, self.pose_stabilizers[subject_id]): ps_stb.update([value]) stable_pose.append(ps_stb.state[0]) stable_pose = np.reshape(stable_pose, (-1, 3)) rotation_vector = stable_pose[0] translation_vector = stable_pose[1] return rotation_vector, translation_vector def publish_subject_list(self, timestamp, subjects): assert (subjects is not None) subject_list_message = self.__subject_bridge.images_to_msg( subjects, timestamp) self.subject_pub.publish(subject_list_message) def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id): t = TransformStamped() t.header.frame_id = self.camera_frame t.header.stamp = timestamp t.child_frame_id = self.tf_prefix + "/head_pose_estimated" + str( subject_id) t.transform.translation.x = nose_center_3d_tf[0] t.transform.translation.y = nose_center_3d_tf[1] t.transform.translation.z = nose_center_3d_tf[2] rotation = transformations.quaternion_from_euler(*head_rpy) t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] try: self.tf2_broadcaster.sendTransform([t]) except rospy.ROSException as exc: if str(exc) == "publish() to a closed topic": pass else: raise exc self.tf2_buffer.set_transform(t, 'extract_landmarks') def update_subject_tracker(self, color_img): faceboxes = self.get_face_bb(color_img) if len(faceboxes) == 0: self.subject_tracker.clear_elements() return tracked_subjects = self.get_subjects_from_faceboxes( color_img, faceboxes) # track the new faceboxes according to the previous ones self.subject_tracker.track(tracked_subjects)
class ArmTCPTransformHandler: """ This class uses a TransformListener to handle transforms related to the TCP. """ def __init__(self): self.__tf_buffer = Buffer() self.__tf_listener = TransformListener(self.__tf_buffer) self.__static_broadcaster = StaticTransformBroadcaster() def ee_link_to_tcp_pose_target(self, pose, ee_link): try: transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform( ee_link, "TCP", rospy.Time(0)) transform_tcp_to_ee_link.header.frame_id = "ee_link_target" transform_tcp_to_ee_link.child_frame_id = "tcp_target" stamp = transform_tcp_to_ee_link.header.stamp transform_world_to_tcp_target = self.transform_from_pose( pose, "base_link", "ee_link_target", stamp) self.__tf_buffer.set_transform(transform_world_to_tcp_target, "default_authority") self.__tf_buffer.set_transform(transform_tcp_to_ee_link, "default_authority") ee_link_target_transform = self.__tf_buffer.lookup_transform( "base_link", "tcp_target", stamp) return self.pose_from_transform(ee_link_target_transform.transform) except ArmTCPTransformHandlerException: self.set_empty_tcp_to_ee_link_transform(ee_link) return pose def tcp_to_ee_link_pose_target(self, pose, ee_link): try: transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform( "TCP", ee_link, rospy.Time(0)) transform_tcp_to_ee_link.header.frame_id = "tcp_target" transform_tcp_to_ee_link.child_frame_id = "ee_link_target" stamp = transform_tcp_to_ee_link.header.stamp transform_world_to_tcp_target = self.transform_from_pose( pose, "base_link", "tcp_target", stamp) self.__tf_buffer.set_transform(transform_world_to_tcp_target, "default_authority") self.__tf_buffer.set_transform(transform_tcp_to_ee_link, "default_authority") ee_link_target_transform = self.__tf_buffer.lookup_transform( "base_link", "ee_link_target", stamp) return self.pose_from_transform(ee_link_target_transform.transform) except ArmTCPTransformHandlerException: self.set_empty_tcp_to_ee_link_transform(ee_link) return pose def tcp_to_ee_link_position_target(self, position, ee_link): pose = Pose(position, Quaternion(0, 0, 0, 1)) return self.tcp_to_ee_link_pose_target(pose, ee_link).position def tcp_to_ee_link_quaternion_target(self, quaternion, ee_link): pose = Pose(Point(0, 0, 0), quaternion) return self.tcp_to_ee_link_pose_target(pose, ee_link).orientation def tcp_to_ee_link_rpy_target(self, roll, pitch, yaw, ee_link): qx, qy, qz, qw = quaternion_from_euler(roll, pitch, yaw) pose = Pose(Point(0, 0, 0), Quaternion(qx, qy, qz, qw)) pose = self.tcp_to_ee_link_pose_target(pose, ee_link) new_roll, new_pitch, new_yaw = euler_from_quaternion(*pose.orientation) return new_roll, new_pitch, new_yaw def set_empty_tcp_to_ee_link_transform(self, ee_link): ee_link_to_tcp_transform = self.transform_from_pose( Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)), ee_link, "TCP") self.__static_broadcaster.sendTransform(ee_link_to_tcp_transform) return ee_link_to_tcp_transform def lookup_transform(self, target_frame, source_frame, stamp=None): if stamp: return self.__tf_buffer.lookup_transform(target_frame, source_frame, stamp) return self.__tf_buffer.lookup_transform(target_frame, source_frame, rospy.Time(0)) def display_target_pose(self, pose, name="target_pose"): t = self.transform_from_pose(pose, "base_link", name) self.__static_broadcaster.sendTransform(t) @staticmethod def transform_from_pose(pose, parent_frame, child_frame, stamp=None): t = TransformStamped() t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation = pose.orientation t.header.frame_id = parent_frame t.child_frame_id = child_frame if stamp: t.header.stamp = stamp return t @staticmethod def pose_from_transform(transform): pose = Pose() pose.position.x = transform.translation.x pose.position.y = transform.translation.y pose.position.z = transform.translation.z pose.orientation = transform.rotation return pose
class ToolTransformHandler: """ This class uses a tfBuffer to handle transforms related to the tools. """ def __init__(self): self.__tf_buffer = Buffer() self.__tf_listener = TransformListener(self.__tf_buffer) self.__static_broadcaster = StaticTransformBroadcaster() self.__tool_transform = self.empty_transform() self.__tcp_transform = self.empty_transform() self.__enable_tcp = False # Publisher self.__tcp_publisher = rospy.Publisher('~tcp', TCP, queue_size=10, latch=True) rospy.Timer(rospy.Duration.from_sec(0.5), self.__send_tcp_transform) # Services rospy.Service('~set_tcp', SetTCP, self.__callback_set_tcp) rospy.Service('~reset_tcp', Trigger, self.__callback_reset_tcp) rospy.Service('~enable_tcp', SetBool, self.__callback_enable_tcp) def __callback_set_tcp(self, req): self.__enable_tcp = True t = self.empty_transform() if req.orientation == Quaternion(): t.transform.rotation = Quaternion(*quaternion_from_euler( req.rpy.roll, req.rpy.pitch, req.rpy.yaw)) else: t.transform.rotation = req.orientation t.transform.translation.x = req.position.x t.transform.translation.y = req.position.y t.transform.translation.z = req.position.z self.set_tcp(t) return CommandStatus.SUCCESS, "Success" def __callback_reset_tcp(self, _): self.set_tcp(copy.deepcopy(self.__tool_transform)) return CommandStatus.SUCCESS, "Success" def __callback_enable_tcp(self, req): self.__enable_tcp = req.value self.__send_tcp_transform(None) self.__publish_tcp_transform() return CommandStatus.SUCCESS, "Success" def set_tool(self, tool_transform): """ Updates the transform object_base -> tool_link_target in local tfBuffer :param grip: """ self.__tool_transform = tool_transform return self.set_tcp(copy.deepcopy(self.__tool_transform)) def set_tcp(self, tcp_transform): """ Updates the transform object_base -> tool_link_target in local tfBuffer :param grip: """ self.__tcp_transform = tcp_transform self.__tf_buffer.set_transform(self.__tcp_transform, "default_authority") self.__send_tcp_transform(None) self.__publish_tcp_transform() return True def __publish_tcp_transform(self): msg = TCP() msg.enabled = self.__enable_tcp if self.__enable_tcp: msg.position.x = self.__tcp_transform.transform.translation.x msg.position.y = self.__tcp_transform.transform.translation.y msg.position.z = self.__tcp_transform.transform.translation.z msg.orientation = self.__tcp_transform.transform.rotation msg.rpy.roll, msg.rpy.pitch, msg.rpy.yaw = euler_from_quaternion([ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ]) else: msg.orientation = Quaternion(0, 0, 0, 1) self.__tcp_publisher.publish(msg) def __send_tcp_transform(self, _): self.__static_broadcaster.sendTransform( self.__tcp_transform if self.__enable_tcp else self. empty_transform()) @staticmethod def transform_from_dict(dict_): t = TransformStamped() t.transform.translation.x = dict_["translation"][0] t.transform.translation.y = dict_["translation"][1] t.transform.translation.z = dict_["translation"][2] t.transform.rotation.x = dict_["quaternion"][0] t.transform.rotation.y = dict_["quaternion"][1] t.transform.rotation.z = dict_["quaternion"][2] t.transform.rotation.w = dict_["quaternion"][3] t.header.frame_id = "tool_link" t.child_frame_id = "TCP" return t @staticmethod def empty_transform(): t = TransformStamped() t.transform.rotation = Quaternion(0, 0, 0, 1) t.header.frame_id = "tool_link" t.child_frame_id = "TCP" return t