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)