def StreamProcessedImages(self, request, context): try: detector = Detector(searchpath=['/usr/local/lib'], families='tag36h11', nthreads=3, quad_decimate=1.0, quad_sigma=0.8, refine_edges=1, decode_sharpening=0.25, debug=0) with picamera.PiCamera(resolution=(width, height), framerate=1) as camera: # camera.resolution = (width, height) print("camera", camera) # Start a preview and let the camera warm up for 2 seconds camera.start_preview() time.sleep(2) with picamera.array.PiRGBArray(camera) as stream: for foo in camera.capture_continuous(stream, 'rgb', use_video_port=True): stream.flush() # gray = np.mean(stream.array, axis=2, dtype=np.uint8) gray = rgb2gray(stream.array) K = (329.8729619143081, 332.94611303946357, 528.0, 396.0) detections = detector.detect(gray, estimate_tag_pose=True, camera_params=K, tag_size=0.08) print("gray.shape", gray.shape) print("stream.array.shape", stream.array.shape) gray3 = np.zeros((height, width, 3)) gray3[:, :, 1] = gray print("detected {} images".format(len(detections))) image_data = stream.array # image_data = image_data.reshape((320, 2, # 240, 2, 3)).max(3).max(1) proto_image = things_pb2.Image( width=width, height=height, image_data=image_data.tobytes()) proto_detections = map(detection_to_proto, detections) message = things_pb2.ProcessedImage( image=proto_image, apriltag_detections=proto_detections) stream.seek(0) yield message except KeyboardInterrupt: print('interrupted!') except Exception as e: print(e) raise e finally: print('ended')
class AtLocalization: """ Handles image rectification and april tag detection. """ def __init__(self, camera_info, tag_size): # init image rectification self.pcm = PinholeCameraModel() self.pcm.fromCameraInfo(camera_info) self.K_rect, self.mapx, self.mapy = self._init_rectification() # init apriltag detector self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.tag_size = tag_size self.camera_params = (self.K_rect[0, 0], self.K_rect[1, 1], self.K_rect[0, 2], self.K_rect[1, 2]) def _init_rectification(self): """ Establish rectification mapping. """ w = self.pcm.width h = self.pcm.height K_rect, roi = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D, (w, h), 1.0) mapx = np.ndarray(shape=(h, w, 1), dtype='float32') mapy = np.ndarray(shape=(h, w, 1), dtype='float32') mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D, self.pcm.R, self.pcm.P, (w, h), cv2.CV_32FC1, mapx, mapy) return K_rect, mapx, mapy def rectify(self, img_raw, interpolation=cv2.INTER_NEAREST): """ Rectify image. """ return cv2.remap(img_raw, self.mapx, self.mapy, interpolation) def detect(self, img): img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(img_gray, estimate_tag_pose=True, camera_params=self.camera_params, tag_size=self.tag_size) return tags
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') self.bridge = CvBridge() self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # subscribe to camera stream self.sub_camera_img = rospy.Subscriber("camera_node/image/compressed", CompressedImage, self.callback, queue_size=1) # publish modified image self.pub_modified_img = rospy.Publisher(f"~image/compressed", CompressedImage, queue_size=1) calibration_data = self.read_yaml_file(f"/data/config/calibrations/camera_intrinsic/{self.veh}.yaml") self.K = np.array(calibration_data["camera_matrix"]["data"]).reshape(3, 3) self.log("Letsgoooooo") def callback(self, msg): """ On camera callback, project the little duckies into the image on top of the april tags. """ img = self.read_image(msg) # convert to cv2 img # detect april tag and extract its reference frame grayscale_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(grayscale_img, estimate_tag_pose=False, camera_params=None, tag_size=None) # self.visualize_at_detection(img, tags) # determine H (using apriltag library) homography_list = [tag.homography for tag in tags] # derive P p_list = [self.projection_matrix(self.K, H) for H in homography_list] # project the model and draw it (using Renderer) for P in p_list: img = self.renderer.render(img, P) # publish modified image img_out = self.bridge.cv2_to_compressed_imgmsg(img) img_out.header = msg.header img_out.format = msg.format self.pub_modified_img.publish(img_out) @staticmethod def projection_matrix(K, H): """ Find the projection matrix by expanding H. """ R_2d = np.linalg.inv(K) @ H # R_2d = [r1 r2 t] R_2d = R_2d / np.linalg.norm(R_2d[:, 0]) r1 = R_2d[:, 0] r2 = R_2d[:, 1] t = R_2d[:, 2] r3 = np.cross(r1, r2) R = np.column_stack((r1, r2, r3)) # R = [r1 r2 r3] # make sure R is orthogonal W, U, Vt = cv2.SVDecomp(R) R = U @ Vt Rt = np.column_stack((R, t)) # Rt = [r1 r2 r3 t] P = K @ Rt return P def read_image(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def read_yaml_file(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" %(fname, exc), type='fatal') rospy.signal_shutdown() return @staticmethod def visualize_at_detection(img, tags): """ Visualize detected april tags for debugging. """ for tag in tags: for idx in range(len(tag.corners)): cv2.line(img, tuple(tag.corners[idx - 1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0)) cv2.putText(img, str(tag.tag_id), org=(tag.corners[0, 0].astype(int) + 10, tag.corners[0, 1].astype(int) + 10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, color=(0, 0, 255)) def on_shutdown(self): super(ARNode, self).on_shutdown()
class FusedLocalizationNode(DTROS): def __init__(self, node_name): super(FusedLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh_name = rospy.get_namespace().strip("/") self.radius = rospy.get_param( f'/{self.veh_name}/kinematics_node/radius', 100) self.baseline = 0.0968 x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100) self.rectify_flag = rospy.get_param( f'/{self.veh_name}/{node_name}/rectify', 100) self.encoder_conf_flag = False self.bridge = CvBridge() self.image = None self.image_timestamp = rospy.Time.now() self.cam_info = None self.camera_info_received = False self.newCameraMatrix = None self.at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) trans_base_cam = np.array([0.0582, 0.0, 0.1072]) rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]), np.radians(15)) rot_cam_base = rot_base_cam.T trans_cam_base = -rot_cam_base @ trans_base_cam self.pose_cam_base = SE3_from_rotation_translation( rot_cam_base, trans_cam_base) self.tfs_msg_cam_base = TransformStamped() self.tfs_msg_cam_base.header.frame_id = 'camera' self.tfs_msg_cam_base.header.stamp = rospy.Time.now() self.tfs_msg_cam_base.child_frame_id = 'at_baselink' self.tfs_msg_cam_base.transform = self.pose2transform( self.pose_cam_base) self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster() translation_map_at = np.array([0.0, 0.0, 0.08]) rot_map_at = np.eye(3) self.pose_map_at = SE3_from_rotation_translation( rot_map_at, translation_map_at) self.tfs_msg_map_april = TransformStamped() self.tfs_msg_map_april.header.frame_id = 'map' self.tfs_msg_map_april.header.stamp = rospy.Time.now() self.tfs_msg_map_april.child_frame_id = 'apriltag' self.tfs_msg_map_april.transform = self.pose2transform( self.pose_map_at) self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster() self.tfs_msg_april_cam = TransformStamped() self.tfs_msg_april_cam.header.frame_id = 'apriltag' self.tfs_msg_april_cam.header.stamp = rospy.Time.now() self.tfs_msg_april_cam.child_frame_id = 'camera' self.br_april_cam = tf.TransformBroadcaster() self.tfs_msg_map_base = TransformStamped() self.tfs_msg_map_base.header.frame_id = 'map' self.tfs_msg_map_base.header.stamp = rospy.Time.now() self.tfs_msg_map_base.child_frame_id = 'fused_baselink' self.br_map_base = tf.TransformBroadcaster() self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi]) R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2) R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2) R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2) R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2) R_dtc_c = R_x_c @ R_z_c R_a_dta = R_y_a @ R_z_a self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c, np.array([0, 0, 0])) self.T_a_dta = SE3_from_rotation_translation(R_a_dta, np.array([0, 0, 0])) self.sub_image_comp = rospy.Subscriber( f'/{self.veh_name}/camera_node/image/compressed', CompressedImage, self.image_cb, buff_size=10000000, queue_size=1) self.sub_cam_info = rospy.Subscriber( f'/{self.veh_name}/camera_node/camera_info', CameraInfo, self.cb_camera_info, queue_size=1) self.pub_tf_fused_loc = rospy.Publisher( f'/{self.veh_name}/{node_name}/transform_stamped', TransformStamped, queue_size=10) def image_cb(self, image_msg): try: image = self.readImage(image_msg) except ValueError as e: self.logerr('Could not decode image: %s' % e) return self.image = image self.image_timestamp = image_msg.header.stamp def cb_camera_info(self, msg): if not self.camera_info_received: self.cam_info = msg self.camera_info_received = True def detect_april_tag(self): if self.rectify_flag: K = self.newCameraMatrix else: K = np.array(self.cam_info.K).reshape((3, 3)) camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2]) img_grey = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(img_grey, True, camera_params, 0.065) list_pose_tag = [] for tag in tags: pose_SE3 = SE3_from_rotation_translation(tag.pose_R, np.squeeze(tag.pose_t)) list_pose_tag.append(pose_SE3) return list_pose_tag def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def pose2transform(self, pose: SE3) -> Transform: rot, trans = rotation_translation_from_SE3(pose) quat_tf = quaternion_from_matrix(pose) quaternion = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3]) translation = Vector3(trans[0], trans[1], trans[2]) return Transform(translation, quaternion) def pose_inverse(self, pose: SE3) -> Transform: rot, trans = rotation_translation_from_SE3(pose) rot_inv = rot.T trans_inv = -rot_inv @ trans return SE3_from_rotation_translation(rot_inv, trans_inv) def rectify_image(self): cameraMatrix = np.array(self.cam_info.K).reshape((3, 3)) distCoeffs = np.array(self.cam_info.D) newCameraMatrix, roi = cv2.getOptimalNewCameraMatrix( cameraMatrix, distCoeffs, (640, 480), 1.0) self.newCameraMatrix = newCameraMatrix mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix, distCoeffs, np.eye(3), newCameraMatrix, (640, 480), cv2.CV_32FC1) self.image = cv2.remap(self.image, mapx, mapy, cv2.INTER_LINEAR) def run(self): while not rospy.is_shutdown(): if self.image is None: continue if not self.camera_info_received: continue if self.rectify_flag: self.rectify_image() list_pose_tag = self.detect_april_tag() if not list_pose_tag: if self.encoder_conf_flag: trans_map_base_2D, theta_map_base_2D = translation_angle_from_SE2( self.pose_map_base_SE2) self.pose_map_base_SE2 = encoder_localization_client( trans_map_base_2D[0], trans_map_base_2D[1], theta_map_base_2D) self.tfs_msg_map_base.transform = self.pose2transform( SE3_from_SE2(self.pose_map_base_SE2)) self.tfs_msg_map_base.header.stamp = rospy.Time.now() self.pub_tf_fused_loc.publish(self.tfs_msg_map_base) self.br_map_base.sendTransformMessage(self.tfs_msg_map_base) else: pose_dta_dtc = self.pose_inverse(list_pose_tag[0]) pose_april_cam = self.T_a_dta @ pose_dta_dtc @ self.T_dtc_c pose_map_base = self.pose_map_at @ pose_april_cam @ self.pose_cam_base quat_map_base = quaternion_from_matrix(pose_map_base) euler = euler_from_quaternion(quat_map_base) theta = euler[2] rot_map_base, translation_map_base = rotation_translation_from_SE3( pose_map_base) pose_map_base_SE2_enc = encoder_localization_client( translation_map_base[0], translation_map_base[1], theta) self.encoder_conf_flag = True self.pose_map_base_SE2 = SE2_from_xytheta( [translation_map_base[0], translation_map_base[1], theta]) self.tfs_msg_map_base.transform = self.pose2transform( SE3_from_SE2(self.pose_map_base_SE2)) self.tfs_msg_map_base.header.stamp = rospy.Time.now() self.pub_tf_fused_loc.publish(self.tfs_msg_map_base) self.br_map_base.sendTransformMessage(self.tfs_msg_map_base) self.tfs_msg_map_april.header.stamp = self.image_timestamp self.static_tf_br_map_april.sendTransform( self.tfs_msg_map_april) self.tfs_msg_cam_base.header.stamp = self.image_timestamp self.static_tf_br_cam_base.sendTransform(self.tfs_msg_cam_base) self.tfs_msg_april_cam.transform = self.pose2transform( pose_april_cam) self.tfs_msg_april_cam.header.stamp = self.image_timestamp self.br_april_cam.sendTransformMessage(self.tfs_msg_april_cam)
# grab the current frame frame = vs.read() # handle the frame from VideoCapture or VideoStream frame = frame[1] if args.get("video", False) else frame # if we are viewing a video and we did not grab a frame, # then we have reached the end of the video if frame is None: break # (assuming you have a variable "frame" with the current camera frame in it) frameGray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #converts to grayscale tags = at_detector.detect(frameGray, estimate_tag_pose=True, camera_params=picam_params, tag_size=tag_size) # detector requires a grayscale frame; do not estimate pose for now for tag in tags: # print the detected tag id and floating point coordinates of its center # print("%s: %6.1f,%6.1f" % (tag.tag_id, tag.center[0], tag.center[1])) # draw a point at the center of the apriltag center = tuple(tag.center.astype(int).ravel()) # cv2.circle(frame, center, 5, (0, 255, 0), -1) # draw box around apriltag using corners for i in range(4): pt0 = tuple(tag.corners[i - 1].astype(int).ravel()) pt1 = tuple(tag.corners[i].astype(int).ravel())
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") # Load calibration files self.calib_data = self.readYamlFile( '/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml') self.log('Loaded intrinsics calibration file') self.extrinsics = self.readYamlFile( '/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml') self.log('Loaded extrinsics calibration file') # Retrieve intrinsic info self.cam_info = self.setCamInfo(self.calib_data) rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer( rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') self.log('Renderer object created') # Create AprilTag detector object self.at_detector = Detector() # Create cv_bridge self.bridge = CvBridge() # Define subscriber to recieve images self.image_sub = rospy.Subscriber( '/' + self.veh + '/camera_node/image/compressed', CompressedImage, self.callback) # Publish the rendered image to a new topic self.augmented_pub = rospy.Publisher('~image/compressed', CompressedImage, queue_size=1) self.log(node_name + ' initialized and running') def callback(self, ros_image): ''' MAIN TASK Converse the image from the camera_node to a cv2 image. Then, detects the apriltags position and retrieves the homography. With the homography and the intrinsic camera matrix, it computes the projection matrix. Finally, the provided Renderer class returns an image with the model rendered on it, which is then published to a new topic ''' # Convert to cv2 image image = self.readImage(ros_image) gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # Extract camera parameters K = np.array(self.cam_info.K).reshape((3, 3)) cam_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2]) # Detect apriltags detections = self.at_detector.detect(gray_image, estimate_tag_pose=True, camera_params=cam_params, tag_size=0.065) # Render a model in each of the detected Apriltags rendered_image = image for tag in detections: # Extract homography H = np.array(tag.homography).reshape((3, 3)) # Obtain Projection matrix projection_matrix = self.projection_matrix(K, H) # Render the model rendered_image = self.renderer.render(rendered_image, projection_matrix) # Publish image with models rendered augmented_image = self.bridge.cv2_to_compressed_imgmsg(rendered_image) self.augmented_pub.publish(augmented_image) def setCamInfo(self, calib_data): ''' Introduces the camera information from the dictionary obtained reading the yaml file to a CameraInfo object ''' cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info def projection_matrix(self, intrinsic, homography): """ Method that computes a Projection matrix from 3D world to image plane. Args: intrinsic: (np.array) Camera matrix homography: (np.array) 2D Homography obtained by detecting the apriltag position Returns: P: Projeciton matrix """ # Extract parameters K = intrinsic K_inv = np.linalg.inv(K) H = homography # Compute 2D Rt matrix Rt = K_inv @ H # Normalize so that ||R1|| = 1 Rt = Rt / np.linalg.norm(Rt[:, 0]) t = np.array(Rt[:, 2]).reshape((3, 1)) # Extract rotation vectors R1 = np.array(Rt[:, 0]).reshape((3, 1)) R2 = np.array(Rt[:, 1]).reshape((3, 1)) # Compute third rotation vector to be orthogonal to the other two R3 = np.array(np.cross(Rt[:, 0], Rt[:, 1])).reshape((3, 1)) # Construct 3D rotation matrix R = np.concatenate((R1, R2, R3), axis=1) # Perform polar decomposition to enforce orthogonality U, S, Vt = np.linalg.svd(R) R = U @ Vt # Compute projection matrix P = K @ (np.concatenate((R, t), axis=1)) return P def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(ARNode, self).onShutdown()
class AprilTag_Localization(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(AprilTag_Localization, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh_name = rospy.get_namespace().strip("/") # read calibration intrinsic_fname = '/data/config/calibrations/camera_intrinsic/default.yaml' if 'INTRINSICS_FILE' in os.environ: intrinsic_fname = os.environ['CALIBRATION_FILE'] intrinsics = self.readYamlFile(intrinsic_fname) self.D = np.array(intrinsics['distortion_coefficients']['data']) self.K = np.reshape(np.array(intrinsics['camera_matrix']['data']), [3, 3]) # init poses self.x = 0 self.y = 0 self.theta = 0 # transforms self.A_to_map = tf.transformations.identity_matrix() # april-tag self.A_to_map[2, 3] = -0.095 self.Ad_to_A = np.array([[0, 1, 0, 0], [0, 0, -1, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.C_to_Cd = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) self.baselink_to_camera = tf.transformations.rotation_matrix( np.pi * (20 / 180), (0, 1, 0)) self.baselink_to_camera[0:3, 3] = np.array([0.0582, 0, 0.1072]) # april tag detector self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # subscriber for images self.listener = tf.TransformListener() self.sub = rospy.Subscriber( f'/{self.veh_name}/camera_node/image/compressed', CompressedImage, self.image_callback) # publisher self.pub_pose = rospy.Publisher(f'/{self.veh_name}/pose/apriltag', TransformStamped, queue_size=30) self.br = tf.TransformBroadcaster() # other important things self.cvbr = CvBridge() self.log("Initialized.") def image_callback(self, msg): """Detects april-tags and renders duckie on them.""" img = self.readImage(msg) stamp = rospy.Time(msg.header.stamp.secs, msg.header.stamp.nsecs) if len(img) > 0: K = self.K tags = self.at_detector.detect( img[:, :, 0], estimate_tag_pose=True, camera_params=[K[0, 0], K[1, 1], K[0, 2], K[1, 2]], tag_size=0.0675) ### TODO: Code here to calculate pose of camera (?) self.broadcast_pose(self.baselink_to_camera, 'at_baselink', 'camera', stamp) if len(tags) > 0: at_camera = tf.transformations.identity_matrix() at_camera[0:3, 0:3] = np.array( tags[0].pose_R) ## only detects first tag! at_camera[0:3, 3] = np.array(tags[0].pose_t).flatten() at = self.C_to_Cd @ at_camera @ self.Ad_to_A self.broadcast_pose(at, 'camera', 'apriltag', rospy.Time.now()) self.broadcast_pose(self.A_to_map, 'apriltag', 'map', rospy.Time.now()) try: (trans, q) = self.listener.lookupTransform( 'map', 'at_baselink', rospy.Time(0)) msg = TransformStamped() msg.header.frame_id = 'map' msg.child_frame_id = 'at_baselink' msg.transform.translation.x = trans[0] msg.transform.translation.y = trans[1] msg.transform.translation.z = trans[2] msg.transform.rotation.x = q[0] msg.transform.rotation.y = q[1] msg.transform.rotation.z = q[2] msg.transform.rotation.w = q[3] msg.header.stamp = rospy.Time.now() self.pub_pose.publish(msg) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: pass def broadcast_pose(self, pose, frame_id, child_frame_id, stamp): q = tf.transformations.quaternion_from_matrix(pose) self.br.sendTransform((pose[0, 3], pose[1, 3], pose[2, 3]), q, stamp, child_frame_id, frame_id) def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.cvbr.compressed_imgmsg_to_cv2(msg_image) image_undistorted = cv2.undistort( cv_image, self.K, self.D) # TODO: move this to GPU. return image_undistorted except CvBridgeError as e: self.log(e) return [] except AttributeError as e: return [] def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(AprilTag_Localization, self).onShutdown()
class ATLocalizationNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ATLocalizationNode, self).__init__( node_name=node_name, node_type=NodeType.PERCEPTION) self.veh = rospy.get_namespace().strip("/") # bridge between opencv and ros self.bridge = CvBridge() # construct subscriber for images self.camera_sub = rospy.Subscriber( 'camera_node/image/compressed', CompressedImage, self.callback, queue_size=1, buff_size=(20*(1024**2))) # self.debug_pub = rospy.Publisher( # '~debug_image/compressed', CompressedImage) # tf broadcasters self.static_tf_br = tf2_ros.StaticTransformBroadcaster() self.tf_br = tf2_ros.TransformBroadcaster() # april-tag detector self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # keep track of the ID of the landmark tag self.at_id = None # get camera calibration parameters (homography, camera matrix, distortion parameters) intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \ rospy.get_namespace().strip("/") + ".yaml" extrinsics_file = '/data/config/calibrations/camera_extrinsic/' + \ rospy.get_namespace().strip("/") + ".yaml" rospy.loginfo('Reading camera intrinsics from {}'.format( intrinsics_file)) rospy.loginfo('Reading camera extrinsics from {}'.format( extrinsics_file)) intrinsics = readYamlFile(intrinsics_file) extrinsics = readYamlFile(extrinsics_file) self.h = intrinsics['image_height'] self.w = intrinsics['image_width'] cam_mat = np.array( intrinsics['camera_matrix']['data']).reshape(3, 3) distortion_coeff = np.array( intrinsics['distortion_coefficients']['data']) H_ground2img = np.linalg.inv( np.array(extrinsics['homography']).reshape(3, 3)) # precompute some quantities new_cam_mat, _ = cv2.getOptimalNewCameraMatrix( cam_mat, distortion_coeff, (640, 480), 1.0) self.map1, self.map2, = cv2.initUndistortRectifyMap( cam_mat, distortion_coeff, np.eye(3), new_cam_mat, (640, 480), cv2.CV_32FC1) self.camera_params = ( new_cam_mat[0, 0], new_cam_mat[1, 1], new_cam_mat[0, 2], new_cam_mat[1, 2]) # define and broadcast static tfs self.camloc_camcv = np.array([[0.0, 0.0, 1.0, 0.0], [-1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) self.atcv_atloc = np.array([[0.0, 1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [-1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) camcv_base_estimated = estimateTfFromHomography( H_ground2img, new_cam_mat) theta = 15.0 / 180.0 * np.pi base_camloc_nominal = np.array([[np.cos(theta), 0.0, np.sin(theta), 0.0582], [0.0, 1.0, 0.0, 0.0], [-np.sin(theta), 0.0, np.cos(theta), 0.1072], [0.0, 0.0, 0.0, 1.0]]) self.camloc_base = self.camloc_camcv @ camcv_base_estimated # self.camloc_base = np.linalg.inv(base_camloc_nominal) self.map_atloc = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.085], [0.0, 0.0, 0.0, 1.0]]) broadcastTF(self.map_atloc, 'map', 'april_tag', self.static_tf_br) def callback(self, data): img_gray = cv2.cvtColor(self.readImage(data), cv2.COLOR_BGR2GRAY) try: undistorted_img = cv2.remap( img_gray, self.map1, self.map2, cv2.INTER_LINEAR) # msg = self.bridge.cv2_to_compressed_imgmsg(undistorted_img, dst_format='jpeg') # self.debug_pub.publish(msg) except: rospy.logwarn( 'Was not able to undistort image for april tag detection') return tags = self.at_detector.detect( undistorted_img, estimate_tag_pose=True, camera_params=self.camera_params, tag_size=TAG_SIZE) for tag in tags: if self.at_id is None: self.at_id = tag.tag_id if tag.tag_id == self.at_id: # Assumes all tags have a unique id camcv_atcv = np.zeros((4, 4)) camcv_atcv[:3, :3] = tag.pose_R camcv_atcv[:3, -1] = np.squeeze(tag.pose_t) camcv_atcv[-1, -1] = 1.0 camloc_atloc = self.camloc_camcv @ camcv_atcv @ self.atcv_atloc atloc_camloc = np.linalg.inv(camloc_atloc) map_base = self.map_atloc @ atloc_camloc @ self.camloc_base broadcastTF(atloc_camloc, 'april_tag', 'camera', self.tf_br, data.header.stamp) broadcastTF(map_base, 'map', 'at_baselink', self.tf_br, data.header.stamp) break def readImage(self, msg_image): try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return []
# grab an image from the camera file = camera.getFileName() imageBW = camera.getImage() # we're out of images if imageBW is None: break # AprilDetect, after accounting for distortion (if fisheye) if camParams['fisheye']: dim1 = imageBW.shape[:2][::-1] #dim1 is the dimension of input image to un-distort map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, numpy.eye(3), K, dim1, cv2.CV_16SC2) undistorted_img = cv2.remap(imageBW, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) tags = at_detector.detect(undistorted_img, True, camParams['cam_params'], args.tagSize/1000) else: tags = at_detector.detect(imageBW, True, camParams['cam_params'], args.tagSize/1000) if file: print("File: {0}".format(file)) # get time to capture and convert print("Time to capture and detect = {0:.3f} sec, found {1} tags".format(time.time() - myStart, len(tags))) for tag in tags: tagpos = getPos(getTransform(tag)) tagrot = getRotation(getTransform(tag)) print("Tag {0} pos = {1} m, Rot = {2} deg".format(tag.tag_id, tagpos.round(3), tagrot.round(1)))
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') # # Write your code here # self.bridge = CvBridge() self.image = None self.image_height = None self.image_width = None self.cam_info = None self.camera_info_received = False self.at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.sub_image_comp = rospy.Subscriber( f'/{self.veh}/camera_node/image/compressed', CompressedImage, self.image_cb, buff_size=10000000, queue_size=1 ) self.sub_cam_info = rospy.Subscriber(f'/{self.veh}/camera_node/camera_info', CameraInfo, self.cb_camera_info, queue_size=1) self.pub_aug_image = rospy.Publisher(f'/{self.veh}/{node_name}/image/compressed', CompressedImage, queue_size=10) def image_cb(self, image_msg): try: image = self.readImage(image_msg) except ValueError as e: self.logerr('Could not decode image: %s' % e) return self.image = image self.image_height = np.shape(image)[0] self.image_width = np.shape(image)[1] def cb_camera_info(self, msg): if not self.camera_info_received: self.cam_info = msg self.camera_info_received = True def detect_april_tag(self): K = np.array(self.cam_info.K).reshape((3, 3)) camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2]) img_grey = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(img_grey, True, camera_params, 0.065) list_hom_tag = [] for tag in tags: list_hom_tag.append(tag.homography) return list_hom_tag def projection_matrix(self, intrinsic, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ K = intrinsic P_ext = np.linalg.inv(K) @ homography r_1 = P_ext[:, 0] r_1_norm = np.linalg.norm(r_1) r_1 = r_1 / r_1_norm r_2 = P_ext[:, 1] r_2_norm = np.linalg.norm(r_2) r_2 = r_2 / r_2_norm r_3 = np.cross(r_1, r_2) t_norm = (r_1_norm + r_2_norm)/2 t = P_ext[:, 2] / t_norm P_ext_new = np.concatenate((r_1, r_2, r_3, t)).reshape((-1, 4), order='F') P = K @ P_ext_new return P def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self,fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" %(fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(ARNode, self).onShutdown() def run(self): while not rospy.is_shutdown(): if self.image is None: continue if not self.camera_info_received: continue list_hom_tag = self.detect_april_tag() if not list_hom_tag: continue img_rend = self.image for h in list_hom_tag: proj_mat = self.projection_matrix(np.array(self.cam_info.K).reshape((3, 3)), h) img_rend = self.renderer.render(img_rend, proj_mat) comp_image = self.bridge.cv2_to_compressed_imgmsg(img_rend) self.pub_aug_image.publish(comp_image)
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer( rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') # bridge between opencv and ros self.bridge = CvBridge() # construct subscriber for images self.camera_sub = rospy.Subscriber('camera_node/image/compressed', CompressedImage, self.callback) # construct publisher for AR images self.pub = rospy.Publisher('~augemented_image/compressed', CompressedImage, queue_size=10) # april-tag detector self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=2.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # get camera calibration parameters (homography, camera matrix, distortion parameters) self.intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \ rospy.get_namespace().strip("/") + ".yaml" rospy.loginfo('Reading camera intrinsics from {}'.format( self.intrinsics_file)) intrinsics = self.readYamlFile(self.intrinsics_file) self.h = intrinsics['image_height'] self.w = intrinsics['image_width'] self.camera_mat = np.array( intrinsics['camera_matrix']['data']).reshape(3, 3) # Precompute some matricies self.camera_params = (self.camera_mat[0, 0], self.camera_mat[1, 1], self.camera_mat[0, 2], self.camera_mat[1, 2]) self.inv_camera_mat = np.linalg.inv(self.camera_mat) self.mat_3Dto2D = np.concatenate((np.identity(3), np.zeros((3, 1))), axis=1) self.T = np.zeros((4, 4)) self.T[-1, -1] = 1.0 def callback(self, data): img = self.readImage(data) tags = self.at_detector.detect(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), estimate_tag_pose=True, camera_params=self.camera_params, tag_size=TAG_SIZE) for tag in tags: H_tag2img = tag.homography projection_mat = self.projection_matrix(H_tag2img) self.renderer.render(img, projection_mat) msg = self.bridge.cv2_to_compressed_imgmsg(img, dst_format='jpeg') self.pub.publish(msg) def projection_matrix(self, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ T_plane = self.inv_camera_mat @ homography T_plane = T_plane / np.linalg.norm( T_plane[:, 0] ) # estimate scale using rotation matrix basis constraint r1 = T_plane[:, 0] r2 = T_plane[:, 1] t = T_plane[:, 2] # Make sure r1 and r2 form an orthogonal basis then generate r3 r2 = (r2 - np.dot(r2, r1) * r1) r2 = r2 / np.linalg.norm(r2) r3 = np.cross(r1, r2) self.T[:3, :] = np.column_stack((r1, r2, r3, t)) return self.camera_mat @ self.mat_3Dto2D @ self.T def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(ARNode, self).onShutdown()
class AprilTagDetector(DTROS): def __init__(self): super(AprilTagDetector, self).__init__(node_name='apriltag_detector_node', node_type=NodeType.PERCEPTION) # get static parameters self.family = rospy.get_param('~family', 'tag36h11') self.nthreads = rospy.get_param('~nthreads', 1) self.quad_decimate = rospy.get_param('~quad_decimate', 1.0) self.quad_sigma = rospy.get_param('~quad_sigma', 0.0) self.refine_edges = rospy.get_param('~refine_edges', 1) self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25) self.tag_size = rospy.get_param('~tag_size', 0.065) # dynamic parameter self.detection_freq = DTParam('~detection_freq', default=-1, param_type=ParamType.INT, min_value=-1, max_value=30) self._detection_reminder = DTReminder( frequency=self.detection_freq.value) # camera info self._camera_parameters = None self._camera_frame = None # create detector object self._at_detector = Detector(families=self.family, nthreads=self.nthreads, quad_decimate=self.quad_decimate, quad_sigma=self.quad_sigma, refine_edges=self.refine_edges, decode_sharpening=self.decode_sharpening) # create a CV bridge object self._bridge = CvBridge() # create subscribers self._img_sub = rospy.Subscriber('image_rect', Image, self._img_cb) self._cinfo_sub = rospy.Subscriber('camera_info', CameraInfo, self._cinfo_cb) # create publishers self._img_pub = rospy.Publisher('tag_detections_image/compressed', CompressedImage, queue_size=1, dt_topic_type=TopicType.VISUALIZATION) self._tag_pub = rospy.Publisher('tag_detections', AprilTagDetectionArray, queue_size=1, dt_topic_type=TopicType.PERCEPTION) self._img_pub_busy = False # create TF broadcaster self._tf_bcaster = tf.TransformBroadcaster() # spin forever rospy.spin() def _img_cb(self, data): if self._camera_parameters is None: return if not self._detection_reminder.is_time( frequency=self.detection_freq.value): return # --- # turn image message into grayscale image img = self._bridge.imgmsg_to_cv2(data, desired_encoding='mono8') # detect tags tags = self._at_detector.detect(img, True, self._camera_parameters, self.tag_size) # draw the detections on an image (if needed) if self._img_pub.anybody_listening() and not self._img_pub_busy: self._img_pub_busy = True Thread(target=self._publish_detections_image, args=(img, tags)).start() # pack detections into a message msg = AprilTagDetectionArray() detection_time = rospy.Time.now() msg.header.stamp = detection_time msg.header.frame_id = self._camera_frame for tag in tags: # turn rotation matrix into quaternion q = _matrix_to_quaternion(tag.pose_R) p = tag.pose_t.T[0] # create single tag detection object detection = AprilTagDetection( transform=Transform(translation=Vector3(x=p[0], y=p[1], z=p[2]), rotation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])), tag_id=tag.tag_id, tag_family=str(tag.tag_family), hamming=tag.hamming, decision_margin=tag.decision_margin, homography=tag.homography.flatten().astype( np.float32).tolist(), center=tag.center.tolist(), corners=tag.corners.flatten().tolist(), pose_error=tag.pose_err) # add detection to array msg.detections.append(detection) # publish tf self._tf_bcaster.sendTransform(p.tolist(), q.tolist(), detection_time, '/tag{:s}'.format(str(tag.tag_id)), self._camera_frame) # publish detections self._tag_pub.publish(msg) # update healthy frequency metadata self._tag_pub.set_healthy_freq(self._img_sub.get_frequency()) self._img_pub.set_healthy_freq(self._img_sub.get_frequency()) def _publish_detections_image(self, img, tags): # get a color buffer from the BW image color_img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) # draw each tag for tag in tags: for idx in range(len(tag.corners)): cv2.line(color_img, tuple(tag.corners[idx - 1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0)) # draw the tag ID cv2.putText(color_img, str(tag.tag_id), org=(tag.corners[0, 0].astype(int) + 10, tag.corners[0, 1].astype(int) + 10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, color=(0, 0, 255)) # pack image into a message img_msg = self._bridge.cv2_to_compressed_imgmsg(color_img) # --- self._img_pub.publish(img_msg) self._img_pub_busy = False def _cinfo_cb(self, data): K = np.array(data.K).reshape((3, 3)) self._camera_parameters = (K[0, 0], K[1, 1], K[0, 2], K[1, 2]) self._camera_frame = data.header.frame_id # once we got the camera info, we can stop the subscriber self._cinfo_sub.shutdown()
# by frame ret, frame = vid.read() grayFrame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) curr_detections = at_detector.detect(grayFrame, estimate_tag_pose=True, camera_params=[1402, 1402, 642, 470], tag_size=0.091) for i in range(len(curr_detections)): detections[curr_detections[i].tag_id] = curr_detections[i] print(i) blur = cv2.GaussianBlur(grayFrame, (5, 5), 0) ret, bw_im = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) # zbar qr = decode(bw_im, symbols=[ZBarSymbol.QRCODE]) # print(qr) for j in range(len(qr)):
class ApriltagDetector: """ ApriltagDetector类, 用来检测特定的Apriltag标签 Attributes: cali_file: str 广角摄像头矫正文件路径,如果想做姿态估计需要用到 """ def __init__(self): cur_dir = os.path.dirname(os.path.abspath(__file__)) # self.cali_file = rospkg.RosPack().get_path('pi_cam') + "/camera_info/calibrations/default.yaml" # self.cali_file = "default.yaml" self.camera_info_msg = load_camera_info_3() # self.detector = Detector( # print(cur_dir + '/../../../../devel_isolated/apriltag/lib') # self.detector = Detector(searchpath=[cur_dir + '/../../../../devel_isolated/apriltag/lib'], self.detector = Detector( # self.detector = Detector(families='tag36h11', nthreads=1, quad_decimate=3.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.cameraMatrix = np.array( self.camera_info_msg.K).reshape((3, 3)) self.camera_params = ( self.cameraMatrix[0, 0], self.cameraMatrix[1, 1], self.cameraMatrix[0, 2], self.cameraMatrix[1, 2]) self.image = None self.detections = [] self.tags = [] self.tag = None if hasattr(R, 'from_dcm'): self.from_dcm_or_matrix = R.from_dcm else: self.from_dcm_or_matrix = R.from_matrix def detect(self, cv_image, label_tags=True, tag_size=0.065): """ 检测函数 Keyword arguments: cv_image: image 原图 tag_size:float Apriltag标签的尺寸,以米为单位,估算距离需要 Returns: tags: [Detection object] 检测到的Apriltag Detection object: tag_family = tag36h11 tag_id = 5 hamming = 0 decision_margin = 75.3475112915 homography = [[-4.04986020e+00 -8.25442426e+00 7.03246452e+02] [ 1.60941194e+01 2.64459780e+00 2.71387964e+02] [-3.94154088e-03 1.31415634e-02 1.00000000e+00]] center = [703.24645207 271.38796427] corners = [[687.30065918 253.60606384] [684.64343262 287.48184204] [719.746521 289.78796387] [722.19494629 254.99520874]] pose_R = [[-0.05079941 -0.99821651 0.03135634] [ 0.99851052 -0.05139001 -0.01832521] [ 0.01990393 0.03037872 0.99934027]] pose_t = [[0.72577546] [0.04402775] [0.58391835]] pose_err = 5.94095039944e-07 """ if len(cv_image.shape) == 3: image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) else: image_gray = cv_image self.tags = self.detector.detect( image_gray, True, self.camera_params, tag_size) # tag size in meter if label_tags: self.label_tags(cv_image, self.tags) self.image = cv_image self.detections = self.toApriltagDetections() if len(self.detections) > 0: self.tag = self.detections[0] else: self.tag = None return cv_image def label_tags(self, rect_image, tags=None): if tags is None: tags = self.tags for tag in tags: for idx in range(len(tag.corners)): cv2.line(rect_image, tuple( tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0)) cv2.putText(rect_image, str(tag.tag_id), org=(tag.corners[0, 0].astype(int)+10, tag.corners[0, 1].astype(int)+10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, color=(0, 0, 255)) def toApriltagDetections(self, tags=None): if tags is None: tags = self.tags detections = [] for tag in tags: r = self.from_dcm_or_matrix(np.array(tag.pose_R)) offset = np.array(tag.pose_t)*100 euler = r.as_euler('xyz', degrees=True) detection = (tag.tag_id, euler, offset) detections.append(detection) return detections def isDetected(self, id=None): if id is None and len(self.detections) > 0: return True else: for tag in self.detections: if id == tag[0]: self.tag = tag return True self.tag = None return False
if camParams['fisheye'] and dim1 is None: #Only need to get mapping at first frame dim1 = imageBW.shape[: 2][:: -1] #dim1 is the dimension of input image to un-distort map1, map2 = cv2.fisheye.initUndistortRectifyMap( K, D, numpy.eye(3), K, dim1, cv2.CV_16SC2) if camParams['fisheye']: imageBW = cv2.remap(imageBW, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) #tags = at_detector.detect(undistorted_img, True, camParams['cam_params'], args.tagSize/1000) #else: tags = at_detector.detect(imageBW, True, camParams['cam_params'], args.tagSize / 1000) # add any new tags to database, or existing one to duplicates tagsused = 0 for tag in tags: if tag.pose_err < args.maxerror * 1e-8: tagsused += 1 tagPlacement.addTag(tag) tagPlacement.getBestTransform() if file: print("File: {0}".format(file)) #get current location and rotation state of vehicle in ArduPilot NED format (rel camera) (posD, rotD) = tagPlacement.getArduPilotNED()
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh_name = rospy.get_namespace().strip("/") self.node_name = rospy.get_name().strip("/") # initialize renderer rospack = rospkg.RosPack() self.renderer = Renderer( rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') # initialize apriltag detector self.at_detector = Detector( families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0, #searchpath=[] ) self.at_camera_params = None # self.at_tag_size = 0.065 # fixed param self.at_tag_size = 2 # to scale pose matrix to homography # initialize member variables self.bridge = CvBridge() self.camera_info_received = False self.camera_info = None self.camera_P = None self.camera_K = None self.cv2_img = None # initialize subs and pubs self.sub_compressed_img = Subscriber( "/{}/camera_node/image/compressed".format(self.veh_name), CompressedImage, self.cb_image, queue_size=1) self.loginfo("Subcribing to topic {}".format( "/{}/camera_node/image/compressed".format(self.veh_name))) self.sub_camera_info = Subscriber("/{}/camera_node/camera_info".format( self.veh_name), CameraInfo, self.cb_camera_info, queue_size=1) self.loginfo("Subcribing to topic {}".format( "/{}/camera_node/camera_info".format(self.veh_name))) self.pub_aug_img = Publisher( "/{}/{}/augmented_image/compressed".format(self.veh_name, self.node_name), CompressedImage, queue_size=1) self.loginfo("Publishing to topic {}".format( "/{}/{}/augmented_image/compressed".format(self.veh_name, self.node_name))) def cb_image(self, msg): # cv2_img = self.bridge.compressed_imgmsg_to_cv2(msg) self.img_msg = msg return def aug_image_and_pub(self, msg): cv2_img = self.bridge.compressed_imgmsg_to_cv2(msg) # TODO: 1. Detect the AprilTag and extract its reference frame greyscale_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(greyscale_img, True, self.at_camera_params, self.at_tag_size) # TODO: 2. Estimate the homography matrix # Read function estimate_pose_for_tag_homography in https://github.com/AprilRobotics/apriltag/blob/master/apriltag_pose.c # The pose_R and pose_t have been already computed pass # TODO 3. Derive the transformation from the reference frame of the AprilTag to the target image reference frame # project from X_T in frame T to pixel: P_camera @ [R|t] @ X_T # return P_T = P_camera @ [R|t] def compose_P_from_tag(P_camera, tag): T = np.concatenate((np.concatenate( (tag.pose_R, tag.pose_t), axis=1), np.array([[0, 0, 0, 1.0]])), axis=0) self.logdebug("P_camera is \n {}".format(P_camera)) P = P_camera @ T self.logdebug("P is \n {}".format(P)) # norm_P = P/P[2,2] # self.logdebug("norm_P is \n {}".format(norm_P)) return P # TODO 4. Project our 3D model in the image (pixel space) and draw it aug_img = cv2_img for tag in tags: aug_img = self.renderer.render( aug_img, compose_P_from_tag(self.camera_P, tag)) aug_image_msg = self.bridge.cv2_to_compressed_imgmsg(aug_img) aug_image_msg.header = msg.header self.pub_aug_img.publish(aug_image_msg) def cb_camera_info(self, msg): # self.logdebug("camera info received! ") if not self.camera_info_received: self.camera_info = msg # TODO: decide whether to use K or P for projection self.camera_P = np.array(msg.P).reshape((3, 4)) self.camera_K = np.array(msg.K).reshape((3, 3)) self.at_camera_params = (self.camera_P[0, 0], self.camera_P[1, 1], self.camera_P[0, 2], self.camera_P[1, 2]) self.camera_info_received = True return # def projection_matrix(self, intrinsic, homography): # """ # Write here the compuatation for the projection matrix, namely the matrix # that maps the camera reference frame to the AprilTag reference frame. # """ # # # TODO: # # Write your code here # # # pass def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def run(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): if self.camera_info_received: img_msg = deepcopy(self.img_msg) self.aug_image_and_pub(img_msg) # self.logdebug("published one augmented image...") else: self.logwarn("Image received, by initialization not finished") rate.sleep() def onShutdown(self): super(ARNode, self).onShutdown()
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. rospy.loginfo("[ARNode]: Initializing Renderer ...") self.renderer = Renderer( rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') # April Tag Detector rospy.loginfo("[ARNode]: Initializing Detector ...") self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # CV Bridge rospy.loginfo("[ARNode]: Initializing CV Bridge ...") self.bridge = CvBridge() # Intrinsics rospy.loginfo("[ARNode]: Loading Camera Intrinsics ...") if (not os.path.isfile( f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml') ): rospy.logwarn( f'[AugmentedRealityBasics]: Could not find {self.veh}.yaml. Loading default.yaml' ) camera_intrinsic = self.readYamlFile( f'/data/config/calibrations/camera_intrinsic/default.yaml') else: camera_intrinsic = self.readYamlFile( f'/data/config/calibrations/camera_intrinsic/{self.veh}.yaml') self.K = np.array(camera_intrinsic['camera_matrix']['data']).reshape( 3, 3) # Subscribers rospy.loginfo("[ARNode]: Initializing Subscribers ...") self.image_subscriber = rospy.Subscriber( 'camera_node/image/compressed', CompressedImage, self.callback) # Publishers rospy.loginfo("[ARNode]: Initializing Publishers ...") self.mod_img_pub = rospy.Publisher('ar_node/image/compressed', CompressedImage, queue_size=1) @staticmethod def projection_matrix(intrinsic, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ P_list = [] for H in homography: R2D_t = np.linalg.inv(intrinsic).dot(H) # [R2D_t] = [r1, r2, t] R2D_t = R2D_t / np.linalg.norm(R2D_t[:, 0]) #normalize r1 = R2D_t[:, 0] r2 = R2D_t[:, 1] t = R2D_t[:, 2] # r3 must be orthogonal to r1 and r2 r3 = np.cross(r1, r2) R3D = np.column_stack((r1, r2, r3)) # since R3D is not a proper rotation matrix yet, we have to orthonormalize it with a polar decomposition: A = RP = W, U, Vt = cv2.SVDecomp(R3D) R3D = U.dot(Vt) R3D_t = np.column_stack((R3D, t)) # R3D_t = [r1, r2, r3, t] P = intrinsic.dot(R3D_t) P_list.append(P) return P_list def callback(self, imgmsg): # Convert img message to cv2 img = self.readImage(imgmsg) # Convert to greyscale img_grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Detect april tags tags = self.at_detector.detect(img_grey, estimate_tag_pose=False, camera_params=None, tag_size=None) # self.visualize_at_detection(img, tags) #debug visualization, marks april tags # Get homographies of april tags homography_list = [tag.homography for tag in tags] # Compute P from homographies P_list = self.projection_matrix(self.K, homography_list) # Render duckies on april tags for P in P_list: img = self.renderer.render(img, P) # Publish modified image modified_imgmsg = self.bridge.cv2_to_compressed_imgmsg(img) modified_imgmsg.header.stamp = rospy.Time.now() self.mod_img_pub.publish(modified_imgmsg) return def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ if (not os.path.isfile(fname)): rospy.logwarn("[ARNode]: Could not find file in %s" % fname) return with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return @staticmethod def visualize_at_detection(img, tags): """ Visualizes detected april tags for debugging. """ for tag in tags: for index in range(len(tag.corners)): cv2.line(img, tuple(tag.corners[index - 1, :].astype(int)), tuple(tag.corners[index, :].astype(int)), (0, 255, 0)) cv2.putText(img, str(tag.tag_id), org=(tag.corners[0, 0].astype(int) + 10, tag.corners[0, 1].astype(int) + 10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, color=(0, 0, 255)) def on_shutdown(self): super(ARNode, self).on_shutdown()
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') self.at_detector = Detector(families='tag36h11', nthreads=5, quad_decimate=2.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.bridge=CvBridge() self.pub_tagimg=rospy.Publisher('at_detect/image/compressed',CompressedImage, queue_size=1) self.sub_img = rospy.Subscriber('camera_node/image/compressed', CompressedImage, self.cb_at_detect) def cb_at_detect(self,msg): img=self.readImage(msg) img_gray=cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) camerainfo=self.load_intrinsics() cameraMatrix = np.array(camerainfo.K).reshape((3,3)) camera_params = (cameraMatrix[0,0],cameraMatrix[1,1],cameraMatrix[0,2],cameraMatrix[1,2]) tags = self.at_detector.detect(img_gray, False, camera_params, 0.065) for tag in tags: # R=tag.pose_R # t=(tag.pose_t).reshape((3,)) # extrinsic=np.eye(4) # extrinsic[0:3,0:3]=R # extrinsic[0:3,3]=t # aux=np.eye(3) # aux=np.c_[aux,[0,0,0]] # projection=np.linalg.multi_dot([cameraMatrix,aux,extrinsic]) # rospy.loginfo(extrinsic) projection1=self.projection_matrix(cameraMatrix,tag.homography) #rospy.loginfo(projection1) img=self.renderer.render(img,projection1) # for idx in range(len(tag.corners)): # cv2.line(img, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0)) tag_msg=self.bridge.cv2_to_compressed_imgmsg(img) self.pub_tagimg.publish(tag_msg) def projection_matrix(self, camera_matrix, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ # Compute rotation along the x and y axis as well as the translation #homography = homography * (-1) rot_and_transl = np.dot(np.linalg.inv(camera_matrix), homography) col_1 = rot_and_transl[:, 0] col_2 = rot_and_transl[:, 1] col_3 = rot_and_transl[:, 2] # normalise vectors l=np.linalg.norm(col_1, 2) rot_1 = col_1 / l rot_2 = col_2 / l translation = col_3 / l # compute the orthonormal basis rot_3 = np.cross(rot_1, rot_2) # finally, compute the 3D projection matrix from the model to the current frame projection = np.stack((rot_1, rot_2, rot_3, translation)).T #rospy.loginfo(projection) return np.dot(camera_matrix, projection) def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self,fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" %(fname, exc), type='fatal') rospy.signal_shutdown() return def load_intrinsics(self): # load intrinsic calibration cali_file_folder = '/data/config/calibrations/camera_intrinsic/' cali_file = cali_file_folder + rospy.get_namespace().strip("/") + ".yaml" calib_data=self.readYamlFile(cali_file) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info def onShutdown(self): super(ARNode, self).onShutdown()
class LocalizationNode(DTROS): def __init__(self, node_name): """Wheel Encoder Localization Node Calculates the pose of the Duckiebot using only AprilTags """ # Initialize the DTROS parent class super(LocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION) self.veh_name = rospy.get_namespace().strip("/") # define initial stuff self.camera_x = 0.065 self.camera_z = 0.11 self.camera_slant = 19/180*math.pi self.height_streetsigns = 0.07 # Load calibration files self._res_w = 640 self._res_h = 480 self.calibration_call() # define tf stuff self.br = tf.TransformBroadcaster() self.static_tf_br = tf2_ros.StaticTransformBroadcaster() # Initialize classes self.at_detec = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.helper = Helpers(self.current_camera_info) self.bridge = CvBridge() # Subscribers self.cam_image = rospy.Subscriber(f'/{self.veh_name}/camera_node/image/compressed', CompressedImage, self.callback, queue_size=10) # Publishers self.pub_baselink_pose = rospy.Publisher('~at_baselink_pose', TransformStamped, queue_size=1) # static broadcasters self.broadcast_static_transform_baselink() self.log("Initialized") def callback(self, image): """ Callback method that ties everything together """ # convert the image to cv2 format image = self.bridge.compressed_imgmsg_to_cv2(image) # undistort the image image = self.helper.process_image(image) # detect AprilTags gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) tags = self.at_detec.detect(gray, estimate_tag_pose=True, camera_params=self.camera_params, tag_size=0.065) for tag in tags: self.broadcast_tag_camera_transform(tag) def broadcast_tag_camera_transform(self, tag): """ compute the transform between AprilTag and camera """ camera_to_tag_R = tag.pose_R camera_to_tag_t = tag.pose_t # build T matrix camera_to_tag_T = np.append(camera_to_tag_R, camera_to_tag_t, axis=1) camera_to_tag_T = np.append(camera_to_tag_T, [[0, 0, 0, 1]], axis=0) # translate T into the rviz frame convention T_rviz = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) self.rviz_to_tag_T = np.matmul(T_rviz, camera_to_tag_T) # translation and rotation arrays br_translation_array = np.array(self.rviz_to_tag_T[0:3, 3]) br_rotation_array = tf.transformations.quaternion_from_matrix(self.rviz_to_tag_T) # publish & broadcast br_transform_msg = self.broadcast_packer("camera", "apriltag", br_translation_array, br_rotation_array) self.br.sendTransform(br_translation_array, br_rotation_array, br_transform_msg.header.stamp, br_transform_msg.child_frame_id, br_transform_msg.header.frame_id) # self.pub_baselink_pose.publish(br_transform_msg) self.broadcast_static_frame_rotation_transform() self.broadcast_static_transform_apriltag() self.broadcast_map_baselink_transform() def broadcast_map_baselink_transform(self): # baselink --> map transform tf_source_end map_baselink_T = np.linalg.inv(self.camera_baselink_T @ self.rviz_to_tag_T @ self.frame_rotation_T @ self.map_tag_T) # map_baselink_T = self.map_tag_T @ self.rviz_to_tag_T @ self.camera_baselink_T br_translation_array = np.array(map_baselink_T[0:3, 3]) br_rotation_array = tf.transformations.quaternion_from_matrix(map_baselink_T) br_transform_msg = self.broadcast_packer("at_baselink", "map", br_translation_array, br_rotation_array) self.pub_baselink_pose.publish(br_transform_msg) def broadcast_static_transform_baselink(self): # camera --> baselink transform static_translation_array = (self.camera_x, 0, self.camera_z) rotation_matrix = np.array([[math.cos(self.camera_slant), 0, math.sin(self.camera_slant), 0], [0, 1, 0, 0], [-math.sin(self.camera_slant), 0, math.cos(self.camera_slant), 0], [0, 0, 0, 1]]) static_rotation_array = tf.transformations.quaternion_from_matrix(rotation_matrix) self.camera_baselink_T = np.copy(rotation_matrix) self.camera_baselink_T[0:3, 3] = static_translation_array static_transform_msg = self.broadcast_packer("at_baselink", "camera", static_translation_array, static_rotation_array) # broadcast packed message self.static_tf_br.sendTransform(static_transform_msg) def broadcast_static_frame_rotation_transform(self): # change in coordinate frame from ATD convention to rviz convention static_translation_array = (0, 0, 0) static_rotation = np.linalg.inv(np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]])) self.frame_rotation_T = np.copy(static_rotation) self.frame_rotation_T[0:3, 3] = static_translation_array static_rotation_array = tf.transformations.quaternion_from_matrix(static_rotation) static_transform_msg = self.broadcast_packer("apriltag", "apriltag_rot", static_translation_array, static_rotation_array) # broadcast packed message self.static_tf_br.sendTransform(static_transform_msg) def broadcast_static_transform_apriltag(self): # map --> AprilTag transform static_translation_array = (0, 0, -self.height_streetsigns) static_rotation_array = tf.transformations.quaternion_from_matrix(np.eye(4)) self.map_tag_T = np.copy(np.eye(4)) self.map_tag_T[0:3, 3] = static_translation_array static_transform_msg = self.broadcast_packer("apriltag_rot", "map", static_translation_array, static_rotation_array) # broadcast packed message self.static_tf_br.sendTransform(static_transform_msg) def calibration_call(self): # copied from the camera driver (dt-duckiebot-interface) # For intrinsic calibration self.cali_file_folder = '/data/config/calibrations/camera_intrinsic/' self.frame_id = rospy.get_namespace().strip('/') + '/camera_optical_frame' self.cali_file = self.cali_file_folder + rospy.get_namespace().strip("/") + ".yaml" # Locate calibration yaml file or use the default otherwise if not os.path.isfile(self.cali_file): self.logwarn("Calibration not found: %s.\n Using default instead." % self.cali_file) self.cali_file = (self.cali_file_folder + "default.yaml") # Shutdown if no calibration file not found if not os.path.isfile(self.cali_file): rospy.signal_shutdown("Found no calibration file ... aborting") # Load the calibration file self.original_camera_info = self.load_camera_info(self.cali_file) self.original_camera_info.header.frame_id = self.frame_id self.current_camera_info = copy.deepcopy(self.original_camera_info) self.update_camera_params() self.log("Using calibration file: %s" % self.cali_file) # extrinsic calibration self.ex_cali_file_folder = '/data/config/calibrations/camera_extrinsic/' self.ex_cali_file = self.ex_cali_file_folder + rospy.get_namespace().strip("/") + ".yaml" self.ex_cali = self.readYamlFile(self.ex_cali_file) # define camera parameters for AT detector self.camera_params = [self.current_camera_info.K[0], self.current_camera_info.K[4], self.current_camera_info.K[2], self.current_camera_info.K[5]] def update_camera_params(self): # copied from the camera driver (dt-duckiebot-interface) """ Update the camera parameters based on the current resolution. The camera matrix, rectification matrix, and projection matrix depend on the resolution of the image. As the calibration has been done at a specific resolution, these matrices need to be adjusted if a different resolution is being used. """ scale_width = float(self._res_w) / self.original_camera_info.width scale_height = float(self._res_h) / self.original_camera_info.height scale_matrix = np.ones(9) scale_matrix[0] *= scale_width scale_matrix[2] *= scale_width scale_matrix[4] *= scale_height scale_matrix[5] *= scale_height # Adjust the camera matrix resolution self.current_camera_info.height = self._res_h self.current_camera_info.width = self._res_w # Adjust the K matrix self.current_camera_info.K = np.array(self.original_camera_info.K) * scale_matrix # Adjust the P matrix (done by Rohit) scale_matrix = np.ones(12) scale_matrix[0] *= scale_width scale_matrix[2] *= scale_width scale_matrix[5] *= scale_height scale_matrix[6] *= scale_height self.current_camera_info.P = np.array(self.original_camera_info.P) * scale_matrix def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" %(fname, exc), type='fatal') rospy.signal_shutdown() return @staticmethod def load_camera_info(filename): # copied from the camera driver (dt-duckiebot-interface) """Loads the camera calibration files. Loads the intrinsic camera calibration. Args: filename (:obj:`str`): filename of calibration files. Returns: :obj:`CameraInfo`: a CameraInfo message object """ with open(filename, 'r') as stream: calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info @staticmethod def broadcast_packer(header_frame, child_frame, translation_array, rotation_array): transform_msg = TransformStamped() transform_msg.header.stamp = rospy.Time.now() transform_msg.header.frame_id = header_frame transform_msg.child_frame_id = child_frame transform_msg.transform.translation = Vector3(*translation_array) transform_msg.transform.rotation = Quaternion(*rotation_array) return transform_msg def onShutdown(self): super(LocalizationNode, self).onShutdown()
class SpecificWorker(GenericWorker): def __init__(self, proxy_map, startup_check=False): super(SpecificWorker, self).__init__(proxy_map) # Drone self.state = 'idle' self.x = 0 self.y = 0 self.pose = {} self.appleCatched = False self.treeCatched = False self.depthX = 180 self.depthY = 180 #AprilTags self.center = 0 self.tags = {} self.n_tags = 0 self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # Image self.image = [] self.depth = [] self.camera_name = "frontCamera" self.data = {} self.depth_array = [] self.color = 0 #Timer self.times = 0 self.Period = 100 if startup_check: self.startup_check() else: self.timer.timeout.connect(self.compute) self.timer.start(self.Period) def __del__(self): print('SpecificWorker destructor') def setParams(self, params): #try: # self.innermodel = InnerModel(params["InnerModelPath"]) #except: # traceback.print_exc() # print("Error reading config params") return True def draw_image(self, color_): color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth) #Marker is on X: 256 Y:256 # cv.drawMarker(color, (int(color_.width/2), int(color_.height/2)), (0, 255, 0), cv.MARKER_CROSS, 25, 2) cv.drawMarker(color, (self.depthX, self.depthY), (0, 0, 255), cv.MARKER_CROSS, 25, 2) cv.drawMarker(color, (self.depthX, self.depthY+150), (0, 0, 255), cv.MARKER_CROSS, 25, 2) cv.drawMarker(color, (self.depthX+150, self.depthY), (0, 0, 255), cv.MARKER_CROSS, 25, 2) cv.drawMarker(color, (self.depthX+150, self.depthY+150), (0, 0, 255), cv.MARKER_CROSS, 25, 2) cv.drawMarker(color, (256, 230), (0, 0, 255), cv.MARKER_CROSS, 25, 2) plt.imshow(color) def draw_camera(self, color_, title): plt.figure(1) plt.clf() plt.imshow(color_) plt.title('Front Camera ') plt.pause(.00001) @QtCore.Slot() def compute(self): all = self.camerargbdsimple_proxy.getAll(self.camera_name) self.image = all.image self.depth = all.depth self.draw_image(self.image) # Depth processing # Max value: 10.0 Min value from apple: between 0.18 and 0.19 if len(self.depth.depth) <= 1048576: self.depth_array = np.frombuffer(self.depth.depth,dtype=np.float32).reshape(self.depth.height, self.depth.width) if self.treeCatched == False: self.x, self.y, self.color = self.colorDetect(self.image, 36, 0, 0, 86, 255, 255) #green elif self.state != 'turnleft': self.x, self.y, self.color = self.colorDetect(self.image, 0, 50, 120, 10, 255, 255) #red if self.state == 'turnleft': self.ar_detection(self.image) try: self.mainSwitch() except Ice.Exception as e: print(e) def mainSwitch(self): if self.state == 'idle': self.idle() elif self.state == 'movex': self.movex(True) elif self.state == 'movey': self.movey(True) elif self.state == 'advance': self.advance(True) elif self.state == 'reverse': self.reverse(True) elif self.state == 'turnleft': self.turn_left(True) elif self.state == 'turnright': self.turn_right(True) elif self.state == 'pickapple': self.pick_apple() elif self.state == 'drop': self.drop() elif self.state == 'stop': self.stop() elif self.state == 'tag': self.tag() elif self.state == 'tree': self.tree() else: self.error() # =============== Drone Movements =================== # =================================================== # PoseType parameters for movements # x = adv rx: None # y = width ry: None # z = height rz: rotate def moveDummy(self, x_=0, y_=0, z_=0, rz_=0): self.pose = RoboCompCoppeliaUtils.PoseType(x=x_,y=y_,z=z_,rz=rz_) headCamera = RoboCompCoppeliaUtils.TargetTypes.HeadCamera self.coppeliautils_proxy.addOrModifyDummy(headCamera, "Quadricopter_target", self.pose) #State: idle def idle(self): print("[ IDLE ] depth = ", self.depth_array[self.depthX][self.depthY]) if self.depth_array[self.depthX][self.depthY] < 0.3: self.moveDummy(x_= -0.002) elif self.depth_array[self.depthX][self.depthY] > 0.6: self.moveDummy(x_= 0.002) elif self.depth_array[self.depthX][self.depthY] > 0.3 and self.depth_array[self.depthX][self.depthY] < 0.6 : if self.treeCatched == False: self.state = 'tree' elif self.appleCatched == False: self.state = 'pickapple' # State: home POSE: 0 -2.0e-01 +6.50e-01 # ORI: 0 0 0 def tree(self): print("[ TREE ] ") print("x: ", self.x, " color: ", self.color) if self.x > 0 and self.color == 86: #tree detected print("[+++] Tree detected") depth = self.depth_array[self.depthX][self.depthY] if depth > 0.46: self.treeCatched = True self.state = 'idle' else: for x in range(2): self.reverse(False) for x in range(2): self.movex(False) for x in range(2): self.movey(False) def pick_apple(self): print("[ LOOKING FOR APPLE ]: ", self.depth_array[self.depthX][self.depthY]) print("x: ", self.x, " color: ", self.color) self.state = 'idle' if self.x > 0 and self.color == 10: #apple detected print("[+++] Apple detected") self.appleCatched = True self.state = 'movex' #state = move x #State: move coordinate x def movex(self, state): if self.x > 256: self.moveDummy(y_=-0.00125) if self.x < 256: self.moveDummy(y_=0.00125) if self.x >= 255 or self.x <= 256: print("[MOVE X] = ", self.x) if state == True: self.state = 'movey' #state = move y #State: move coordinate y def movey(self, state): if self.y > 225: self.moveDummy(z_=0.00125) if self.y < 200: self.moveDummy(z_=-0.00125) return if self.y >= 224 or self.y <= 225: print("[MOVE Y] = ", self.y) if state == True: self.state = 'advance' #state = advance #State: advance until the suctionPad catches the apple def advance(self, state): print("[ADVANCE] depth = ", self.depth_array[self.depthX][self.depthY]) depth = self.depth_array[self.depthX][self.depthY] if depth > 0.18: self.moveDummy(x_=0.005) else: self.moveDummy(x_=0.00125) if state == True: # if depth > 0.165 and depth < 0.176 or self.y < 50: # print("[---] DEPTH < ", depth) # self.state = 'reverse' #state = reverse # else: # self.state = 'movex' #state = move x self.closeDepth(self.depthX, self.depthY, 0, 0) self.closeDepth(self.depthX, self.depthY, 0, 150) self.closeDepth(self.depthX, self.depthY, 150, 0) self.closeDepth(self.depthX, self.depthY, 150, 150) def closeDepth(self,x,y,offset_x, offset_y): depth = self.depth_array[x+offset_x][y+offset_y] if depth > 0.165 and depth < 0.177 or self.y < 50: print("[---] DEPTH < ", depth) self.state = 'reverse' #state = reverse else: self.state = 'movex' #state = move x # State: drop def drop(self): print("[ DROP ] depth: ", self.depth_array[self.depthX][self.depthY]) leave = RoboCompCoppeliaUtils.PoseType(x=1.00000000,y=1.00000000,z=1.00000000) #catch = RoboCompCoppeliaUtils.PoseType(x=0.00000000,y=0.00000000,z=0.00000000) for x in range(1000): print("[ DETACHED ]") self.coppeliautils_proxy.addOrModifyDummy(RoboCompCoppeliaUtils.TargetTypes.Hand, "suctionPad_Dummy", leave) # for x in range(100): # print("[ CATCH ]") # self.coppeliautils_proxy.addOrModifyDummy(RoboCompCoppeliaUtils.TargetTypes.Hand, "suctionPad_Dummy", catch) self.n_tags = 0 self.treeCatched = False self.state = 'stop' # State: stop def stop(self): for x in range(20): print("[ STOP ]") self.state = 'turnright' # State: REVERSE def reverse(self, state): print("[ REVERSE ] depth: ", self.depth_array[self.depthX][self.depthY]) self.moveDummy(x_=-0.0025) if state == True: if self.depth_array[self.depthX][self.depthY] >= 0.35: self.state = 'turnleft' self.appleCatched = False #apple detection is disabled # State: turnleft def turn_left(self, state): print("[ TURN LEFT ] ", self.depth_array[self.depthX][self.depthY]) if(self.n_tags == 0): self.moveDummy(rz_=-0.007) else: print(" + TAG SPOTTED") if state == True: self.state = 'tag' # State: turnright def turn_right(self, state): print("[ TURN RIGHT ] depth", self.depth_array[self.depthX][self.depthY]) if(self.depth_array[self.depthX][self.depthY] > 0.5): self.moveDummy(rz_=0.005) else: if state == True: self.state = 'idle' print("RECTIFIED TURN RIGHT") for x in range(20): self.movex(False) self.movey(False) self.moveDummy(rz_=0.0025) # State: tag def tag(self): print("[ TAG ] ") print("Centro X del tag: ",self.center[0]) print("Centro Y del tag: ",self.center[1]) self.x = self.center[0] self.y = self.center[1] print("TAG rectified") for x in range(2): self.movex(False) self.movey(False) self.n_tags = 0 self.state = 'drop' def error(self): print("Error en el switch") # =============== Object detection =================== # ==================================================== def colorDetect(self, color_, low_h, low_s, low_v, high_h, high_s, high_v): color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth) x, y, x_, y_ = [0 for _ in range(4)] first = False color = cv.cvtColor(color, cv.COLOR_RGB2BGR) hsv = cv.cvtColor(color, cv.COLOR_BGR2HSV) lower_red_range = np.array([low_h, low_s, low_v]) upper_red_range = np.array([high_h, high_s, high_v]) mask = cv.inRange(hsv, lower_red_range, upper_red_range) # Find contours cnts = cv.findContours(mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) # Iterate through contours and filter by the number of vertices for c in cnts: area = cv.contourArea(c) if area > 700: cv.drawContours(color,[c],-1,(0,255,0), 2) M = cv.moments(c) x = int(M["m10"]/ M["m00"]) y = int(M["m01"]/ M["m00"]) if(first == False): x_ = x y_ = y first = True cv.circle(color,(x,y),2,(0,255,0), 2) # cv.putText(color, "rojo", (x-20, y-20), cv.FONT_HERSHEY_SIMPLEX,2, (255,255,255), 2) color = cv.cvtColor(color, cv.COLOR_BGR2RGB) self.draw_camera(color, 'Drone Camera') return (x_, y_, high_h) def ar_detection(self, color_): color = np.frombuffer(color_.image, np.uint8).reshape(color_.height, color_.width, color_.depth) gray = cv.cvtColor(color, cv.COLOR_RGB2GRAY) k = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] cameraMatrix = np.array(k).reshape((3,3)) camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] ) tag_size = 0.065 self.tags = self.at_detector.detect(gray, False, camera_params, tag_size) if(len(self.tags) == 1): print("[ INFO ] {} total AprilTags detected".format(len(self.tags))) self.n_tags = 1 if(self.n_tags == 1): for tag in self.tags: for idx in range(len(tag.corners)): cv.line(color, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0),thickness=2) cv.circle(color, tuple(tag.center.astype(int)), 2, (0,255,0), 2) self.center = tag.center.astype(int) # print("Centro X del tag: ",self.center[0]) # print("Centro Y del tag: ",self.center[1]) self.draw_camera(color, 'Drone Camera') def startup_check(self): QTimer.singleShot(200, QApplication.instance().quit) # =============== Slots methods for State Machine =================== # =================================================================== # # sm_initialize # @QtCore.Slot() def sm_initialize(self): #print("Entered state initialize") self.t_initialize_to_compute.emit() pass # # sm_compute # @QtCore.Slot() def sm_compute(self): #print("Entered state compute") self.compute() pass # # sm_finalize # @QtCore.Slot() def sm_finalize(self): print("Entered state finalize") pass
with open(test_images_path + '/test_info.yaml', 'r') as stream: parameters = yaml.load(stream) #### test WITH THE SAMPLE IMAGE #### print("\n\nTESTING WITH A SAMPLE IMAGE") img = cv2.imread(test_images_path+'/'+parameters['sample_test']['file'], cv2.IMREAD_GRAYSCALE) cameraMatrix = numpy.array(parameters['sample_test']['K']).reshape((3,3)) camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] ) if visualization: cv2.imshow('Original image',img) tags = at_detector.detect(img, True, camera_params, parameters['sample_test']['tag_size']) print(tags) color_img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) for tag in tags: for idx in range(len(tag.corners)): cv2.line(color_img, tuple(tag.corners[idx-1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (0, 255, 0)) cv2.putText(color_img, str(tag.tag_id), org=(tag.corners[0, 0].astype(int)+10,tag.corners[0, 1].astype(int)+10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, color=(0, 0, 255)) if visualization:
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') self.bridge = CvBridge() # # Write your code here # self.homography = self.load_extrinsics() self.H = np.array(self.homography).reshape((3, 3)) self.Hinv = np.linalg.inv(self.H) self.at_detector = Detector(searchpath=['apriltags'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.sub_compressed_img = rospy.Subscriber( "/robot_name/camera_node/image/compressed", CompressedImage, self.callback, queue_size=1 ) self.sub_camera_info = rospy.Subscriber( "/robot_name/camera_node/camera_info", CameraInfo, self.cb_camera_info, queue_size=1 ) self.pub_map_img = rospy.Publisher( "~april_tag_duckie/image/compressed", CompressedImage, queue_size=1, dt_topic_type=TopicType.VISUALIZATION, ) def callback(self, msg): header = msg.header cv_img = self.readImage(msg) cameraMatrix = np.array(self.camera_model.K) camera_params = (cameraMatrix[0, 0], cameraMatrix[1, 1], cameraMatrix[0, 2], cameraMatrix[1, 2]) gray_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY) tags = self.at_detector.detect(gray_img, True, camera_params, 0.065) Ho = np.array(tags[0].homography) print("first", Ho) Ho_new = self.projection_matrix(cameraMatrix, Ho) print("second", Ho_new) img_out = self.renderer.render(cv_img, Ho_new) msg_out = self.bridge.cv2_to_compressed_imgmsg(img_out, dst_format='jpeg') msg_out.header = header self.pub_map_img.publish(msg_out) def cb_camera_info(self, msg): # unsubscribe from camera_info self.loginfo('Camera info message received. Unsubscribing from camera_info topic.') # noinspection PyBroadException try: self.sub_camera_info.shutdown() except BaseException: self.loginfo('BaseException') pass # --- self.H_camera, self.W_camera = msg.height, msg.width # create new camera info self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(msg) def projection_matrix(self, intrinsic, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ # # Write your code here # kinv = np.linalg.inv(intrinsic) rt = np.dot(kinv, homography) r1 = rt[:, 0] r2 = rt[:, 1] t = rt[:, 2] r1_norm = r1 print(rt) r2_new = r2 - np.dot(r1_norm, r2) * r1_norm r2_norm = r2_new / np.linalg.norm(r2_new) * np.linalg.norm(r1) r3_new = np.cross(r1_norm, r2_norm) r3_norm = r3_new / np.linalg.norm(r3_new) * np.linalg.norm(r1) matrix = np.zeros((3, 4)) matrix[:, 0] = r1_norm matrix[:, 1] = r2_norm matrix[:, 2] = r3_norm matrix[:, 3] = t h**o = np.dot(intrinsic, matrix) return h**o def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def readYamlFile(self,fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" %(fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(ARNode, self).onShutdown() def load_extrinsics(self): """ Loads the homography matrix from the extrinsic calibration file. Returns: :obj:`numpy array`: the loaded homography matrix """ # load intrinsic calibration cali_file_folder = '/data/config/calibrations/camera_extrinsic/' cali_file = cali_file_folder + rospy.get_namespace().strip("/") + ".yaml" # Locate calibration yaml file or use the default otherwise if not os.path.isfile(cali_file): self.log("Can't find calibration file: %s.\n Using default calibration instead." % cali_file, 'warn') cali_file = (cali_file_folder + "default.yaml") # Shutdown if no calibration file not found if not os.path.isfile(cali_file): msg = 'Found no calibration file ... aborting' self.log(msg, 'err') rospy.signal_shutdown(msg) try: with open(cali_file, 'r') as stream: calib_data = yaml.load(stream) except yaml.YAMLError: msg = 'Error in parsing calibration file %s ... aborting' % cali_file self.log(msg, 'err') rospy.signal_shutdown(msg) return calib_data['homography']
class ATPoseNode(DTROS): """ Computes an estimate of the Duckiebot pose using the wheel encoders. Args: node_name (:obj:`str`): a unique, descriptive name for the ROS node Configuration: Publisher: ~/rectified_image (:obj:`Image`): The rectified image ~at_localization (:obj:`PoseStamped`): The computed position broadcasted in TFs Subscribers: ~/camera_node/image/compressed (:obj:`CompressedImage`): Observation from robot """ def __init__(self, node_name): # Initialize the DTROS parent class super(ATPoseNode, self).__init__(node_name=node_name, node_type=NodeType.LOCALIZATION) # get the name of the robot self.veh = rospy.get_namespace().strip("/") self.bridge = CvBridge() self.camera_info = None self.Rectify = None self.at_detector = Detector(families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # Construct publishers self.at_estimated_pose = rospy.Publisher( f'/{self.veh}/at_localization', Odometry, queue_size=1, dt_topic_type=TopicType.LOCALIZATION) # Camera subscribers: camera_topic = f'/{self.veh}/camera_node/image/compressed' self.camera_feed_sub = rospy.Subscriber(camera_topic, CompressedImage, self.detectATPose, queue_size=1, buff_size=2**24) camera_info_topic = f'/{self.veh}/camera_node/camera_info' self.camera_info_sub = rospy.Subscriber(camera_info_topic, CameraInfo, self.getCameraInfo, queue_size=1) self.image_pub = rospy.Publisher(f'/{self.veh}/rectified_image', Image, queue_size=10) self.tag_pub = rospy.Publisher(f'/{self.veh}/detected_tags', Int32MultiArray, queue_size=5) self.log("Initialized!") def getCameraInfo(self, cam_msg): if (self.camera_info == None): self.camera_info = cam_msg self.Rectify = Rectify(self.camera_info) return def _broadcast_tf(self, parent_frame_id: str, child_frame_id: str, stamp: Optional[float] = None, translations: Optional[Tuple[float, float, float]] = None, euler_angles: Optional[Tuple[float, float, float]] = None, quarternion: Optional[List[float]] = None): br = tf2_ros.StaticTransformBroadcaster() ts = TransformStamped() ts.header.frame_id = parent_frame_id ts.child_frame_id = child_frame_id if stamp is None: stamp = rospy.Time.now() ts.header.stamp = stamp if translations is None: translations = 0, 0, 0 ts.transform.translation.x = translations[0] ts.transform.translation.y = translations[1] ts.transform.translation.z = translations[2] if euler_angles is not None: q = tf_conversions.transformations.quaternion_from_euler( *euler_angles) elif quarternion is not None: q = quarternion else: q = 0, 0, 0, 1 ts.transform.rotation.x = q[0] ts.transform.rotation.y = q[1] ts.transform.rotation.z = q[2] ts.transform.rotation.w = q[3] br.sendTransform(ts) def _broadcast_detected_tag(self, image_msg, tag_id, tag): # inverse the rotation and tranformation comming out of the AT detector. # The AP detector's output is the camera frame relative to the TAG # According to: https://duckietown.slack.com/archives/C6ZHPV212/p1619876209086300?thread_ts=1619751267.084600&cid=C6ZHPV212 # While the transformation below needs TAG relative to camera # Therefore we need to reverse it first. pose_R = np.identity(4) pose_R[:3, :3] = tag.pose_R inv_pose_R = np.transpose(pose_R) pose_t = np.ones((4, 1)) pose_t[:3, :] = tag.pose_t inv_pose_t = -np.matmul(inv_pose_R, pose_t) out_q = quaternion_from_matrix(inv_pose_R) out_t = inv_pose_t[:3, :] tag_frame_id = 'april_tag_{}'.format(tag_id) tag_cam_frame_id = 'april_tag_cam_{}'.format(tag_id) camera_rgb_link_frame_id = 'at_{}_camera_rgb_link'.format(tag_id) camera_link_frame_id = 'at_{}_camera_link'.format(tag_id) base_link_frame_id = 'at_{}_base_link'.format(tag_id) # ATag to ATag cam (this is because AprilTAG pose is different from physical) self._broadcast_tf(parent_frame_id=tag_frame_id, child_frame_id=tag_cam_frame_id, euler_angles=(-90 * math.pi / 180, 0, -90 * math.pi / 180)) # ATag cam to camera_rgb_link (again this is internal cam frame) self._broadcast_tf(parent_frame_id=tag_cam_frame_id, child_frame_id=camera_rgb_link_frame_id, stamp=image_msg.header.stamp, translations=out_t, quarternion=out_q) # camera_rgb_link to camera_link (internal cam frame to physical cam frame) self._broadcast_tf(parent_frame_id=camera_rgb_link_frame_id, child_frame_id=camera_link_frame_id, stamp=image_msg.header.stamp, euler_angles=(0, -90 * math.pi / 180, 90 * math.pi / 180)) # camera_link to base_link of robot self._broadcast_tf(parent_frame_id=camera_link_frame_id, child_frame_id=base_link_frame_id, stamp=image_msg.header.stamp, translations=(-0.066, 0, -0.106), euler_angles=(0, -15 * math.pi / 180, 0)) def detectATPose(self, image_msg): """ Image callback. Args: msg_encoder (:obj:`Compressed`) encoder ROS message. """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(image_msg) except ValueError as e: self.logerr('Could not decode image: %s' % e) return new_cam, rectified_img = self.Rectify.rectify_full(cv_image) try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(rectified_img, "bgr8")) except ValueError as e: self.logerr('Could not decode image: %s' % e) return gray_img = cv2.cvtColor(rectified_img, cv2.COLOR_RGB2GRAY) camera_params = (new_cam[0, 0], new_cam[1, 1], new_cam[0, 2], new_cam[1, 2]) detected_tags = self.at_detector.detect(gray_img, estimate_tag_pose=True, camera_params=camera_params, tag_size=0.065) detected_tag_ids = list(map(lambda x: fetch_tag_id(x), detected_tags)) array_for_pub = Int32MultiArray(data=detected_tag_ids) for tag_id, tag in zip(detected_tag_ids, detected_tags): print('detected {}: ({}, {})'.format( tag_id, image_msg.header.stamp.to_time(), rospy.Time.now().to_time())) self._broadcast_detected_tag(image_msg, tag_id, tag) self.tag_pub.publish(array_for_pub)
class ARNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ARNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") rospack = rospkg.RosPack() # Initialize an instance of Renderer giving the model in input. self.renderer = Renderer( rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj') # Load calibration files self._res_w = 640 self._res_h = 480 self.calibration_call() # Initialize classes self.at_detec = Detector() self.helper = Helpers(self.current_camera_info) self.bridge = CvBridge() # Subscribe to the compressed camera image self.cam_image = rospy.Subscriber( f'/{self.veh}/camera_node/image/compressed', CompressedImage, self.callback, queue_size=10) # Publishers self.aug_image = rospy.Publisher( f'/{self.veh}/{node_name}/image/compressed', CompressedImage, queue_size=10) def callback(self, image): """ Callback method that ties everything together """ # convert the image to cv2 format image = self.bridge.compressed_imgmsg_to_cv2(image) #undistort the image image = self.helper.process_image(image) # detect AprilTags gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) tags = self.at_detec.detect(gray, estimate_tag_pose=False, camera_params=None, tag_size=None) for tag in tags: # estimate homography homography = tag.homography # calculate projection matrix proj_mat = self.projection_matrix(self.current_camera_info.K, homography) # rendering magics image = self.renderer.render(image, proj_mat) #publish image to new topic self.aug_image.publish(self.bridge.cv2_to_compressed_imgmsg(image)) def projection_matrix(self, intrinsic, homography): """ Write here the compuatation for the projection matrix, namely the matrix that maps the camera reference frame to the AprilTag reference frame. """ # format the K matrix K = np.reshape(np.array(intrinsic), (3, 3)) # calculate R_t R_t = np.matmul(np.linalg.inv(K), homography) # normalization parameters R_t n_r1 = np.linalg.norm(R_t[:, 0]) n_r2 = np.linalg.norm(R_t[:, 1]) # calculate R_3 and rebuild R_t R = np.array([ R_t[:, 0], R_t[:, 1], (n_r1 + n_r2) / 2 * np.cross(R_t[:, 0] / n_r1, R_t[:, 1] / n_r2) ]) R_t = np.vstack((R, R_t[:, 2])) # calculate the projection matrix proj_mat = np.matmul(K, np.transpose(R_t)) return proj_mat def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def calibration_call(self): # copied from the camera driver (dt-duckiebot-interface) # For intrinsic calibration self.cali_file_folder = '/data/config/calibrations/camera_intrinsic/' self.frame_id = rospy.get_namespace().strip( '/') + '/camera_optical_frame' self.cali_file = self.cali_file_folder + rospy.get_namespace().strip( "/") + ".yaml" # Locate calibration yaml file or use the default otherwise if not os.path.isfile(self.cali_file): self.logwarn( "Calibration not found: %s.\n Using default instead." % self.cali_file) self.cali_file = (self.cali_file_folder + "default.yaml") # Shutdown if no calibration file not found if not os.path.isfile(self.cali_file): rospy.signal_shutdown("Found no calibration file ... aborting") # Load the calibration file self.original_camera_info = self.load_camera_info(self.cali_file) self.original_camera_info.header.frame_id = self.frame_id self.current_camera_info = copy.deepcopy(self.original_camera_info) self.update_camera_params() self.log("Using calibration file: %s" % self.cali_file) # extrinsic calibration self.ex_cali_file_folder = '/data/config/calibrations/camera_extrinsic/' self.ex_cali_file = self.ex_cali_file_folder + rospy.get_namespace( ).strip("/") + ".yaml" self.ex_cali = self.readYamlFile(self.ex_cali_file) # define homography self.homography = self.ex_cali["homography"] def update_camera_params(self): """ Update the camera parameters based on the current resolution. The camera matrix, rectification matrix, and projection matrix depend on the resolution of the image. As the calibration has been done at a specific resolution, these matrices need to be adjusted if a different resolution is being used. TODO: Test that this really works. """ scale_width = float(self._res_w) / self.original_camera_info.width scale_height = float(self._res_h) / self.original_camera_info.height scale_matrix = np.ones(9) scale_matrix[0] *= scale_width scale_matrix[2] *= scale_width scale_matrix[4] *= scale_height scale_matrix[5] *= scale_height # Adjust the camera matrix resolution self.current_camera_info.height = self._res_h self.current_camera_info.width = self._res_w # Adjust the K matrix self.current_camera_info.K = np.array( self.original_camera_info.K) * scale_matrix # Adjust the P matrix (done by Rohit) scale_matrix = np.ones(12) scale_matrix[0] *= scale_width scale_matrix[2] *= scale_width scale_matrix[5] *= scale_height scale_matrix[6] *= scale_height self.current_camera_info.P = np.array( self.original_camera_info.P) * scale_matrix @staticmethod def load_camera_info(filename): """Loads the camera calibration files. Loads the intrinsic camera calibration. Args: filename (:obj:`str`): filename of calibration files. Returns: :obj:`CameraInfo`: a CameraInfo message object """ with open(filename, 'r') as stream: calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info def onShutdown(self): super(ARNode, self).onShutdown()
class Vision: coral_model_file = "/home/pi/catkin_ws/src/robot5/models/mobilenet_ssd_v2/mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite" coral_labels_file = "/home/pi/catkin_ws/src/robot5/models/mobilenet_ssd_v2/coco_labels.txt" coral_confidence = 0.3 caffe_model_file = "/home/pi/catkin_ws/src/robot5/models/res10_300x300_ssd_iter_140000.caffemodel" caffe_confidence = 0.5 caffe_prototxt = "/home/pi/catkin_ws/src/robot5/models/deploy.prototxt.txt" face_cascade = cv2.CascadeClassifier('/home/pi/opencv/data/haarcascades/haarcascade_frontalface_default.xml') eye_cascade = cv2.CascadeClassifier('/home/pi/opencv/data/haarcascades/haarcascade_eye.xml') APRILWIDTH = 172 FOCALLENGTH = 0.304 def __init__(self): self.coral_model = {} self.coral_labels = {} self.caffe_model = {} self.at_detector = {} self.videoStream = {} self.status = None self.captureFrame = None self.visionFrame = None self.thread = Thread(target=self.frameUpdate,args=()) self.thread.daemon = True self.flaskThread = Thread(target=self.runFlask) self.flaskThread.daemon = True self.frameLock = Lock() print("[INFO] Initialising ROS...") #self.pub = rospy.Publisher(name='vision_detect',subscriber_listener=vision_detect,queue_size=5,data_class=vision_detect) self.pub = rospy.Publisher('/vision_detect',vision_detect) rospy.init_node('robot5_vision',anonymous=False) for row in open(self.coral_labels_file): (classID, label) = row.strip().split(maxsplit=1) self.coral_labels[int(classID)] = label.strip() print("[INFO] loading Coral model...") self.coral_model = DetectionEngine(self.coral_model_file) #print("[INFO] loading Caffe model...") #self.caffe_model = cv2.dnn.readNetFromCaffe(self.caffe_prototxt, self.caffe_model_file) self.at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) print("[INFO] Running Flask...") self.app = Flask(__name__) self.add_routes() self.flaskThread.start() print("[INFO] starting video stream...") self.videoStream = VideoStream(src=0,usePiCamera=True).start() time.sleep(2.0) # warmup self.captureFrame = self.videoStream.read() self.visionFrame = self.captureFrame self.thread.start() time.sleep(0.5) # get first few srun = True print("[INFO] running frames...") while srun: srun = self.doFrame() cv2.destroyAllWindows() self.videoStream.stop() def frameUpdate(self): while True: self.captureFrame = self.videoStream.read() time.sleep(0.03) def gen(self): while True: if self.visionFrame is not None: bout = b"".join([b'--frame\r\nContent-Type: image/jpeg\r\n\r\n', self.visionFrame,b'\r\n']) yield (bout) else: return "" def add_routes(self): @self.app.route("/word/<word>") def some_route(word): self.testout() return "At some route:"+word @self.app.route('/video_feed') def video_feed(): return Response(self.gen(),mimetype='multipart/x-mixed-replace; boundary=frame') def testout(self): print("tested") pass def runFlask(self): self.app.run(debug=False, use_reloader=False,host='0.0.0.0', port=8000) def coralDetect(self,frame,orig): start1 = time.time() results = self.coral_model.detect_with_image(frame, threshold=self.coral_confidence,keep_aspect_ratio=True, relative_coord=False) fcount = 0 points = [] for r in results: box = r.bounding_box.flatten().astype("int") (startX, startY, endX, endY) = box label = self.coral_labels[r.label_id] cv2.rectangle(orig, (startX, startY), (endX, endY), (0, 255, 0), 2) y = startY - 15 if startY - 15 > 15 else startY + 15 cx = startX + (endX - startX/2) cy = startY + (endY - startY/2) #points.append({"type":"coral","num":fcount,"x":cx,"y":cy,"label":label,"score":r.score,"time":time.time() }) points.append(["coral",fcount,int(cx),int(cy),label,int(r.score*100),rospy.Time.now()]) fcount +=1 text = "{}: {:.2f}%".format(label, r.score * 100) cv2.putText(orig, text, (startX, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) end1 = time.time() #print("#1:",end1-start1) return orig,points def caffeDetect(self,frame,orig): start2 = time.time() (h, w) = frame.shape[:2] blob = cv2.dnn.blobFromImage(cv2.resize(frame, (300, 300)), 1.0,(300, 300), (104.0, 177.0, 123.0)) self.caffe_model.setInput(blob) detections = self.caffe_model.forward() fcount = 0 points = [] for i in range(0, detections.shape[2]): confidence = detections[0, 0, i, 2] if confidence < self.caffe_confidence: continue box = detections[0, 0, i, 3:7] * np.array([w, h, w, h]) (startX, startY, endX, endY) = box.astype("int") text = "{:.2f}%".format(confidence * 100) y = startY - 10 if startY - 10 > 10 else startY + 10 cx = startX + (endX - startX/2) cy = startY + (endY - startY/2) #points.append({"type":"caffe","num":fcount,"x":cx,"y":cy,"score":confidence,"time":time.time()}) points.append(["caffe",fcount,int(cx),int(cy),"",int(confidence*10),rospy.Time.now()]) fcount +=1 cv2.rectangle(orig, (startX, startY), (endX, endY),(0, 0, 255), 2) cv2.putText(orig, text, (startX, y), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2) end2 = time.time() #print("#2:",end2-start2) return orig,points def aprilDetect(self,grey,orig): start3 = time.time() Xarray = [338.563277422543, 0.0, 336.45495347495824, 0.0, 338.939280638548, 230.486982216255, 0.0, 0.0, 1.0] camMatrix = np.array(Xarray).reshape((3,3)) params = (camMatrix[0,0],camMatrix[1,1],camMatrix[0,2],camMatrix[1,2]) tags = self.at_detector.detect(grey,True,params,0.065) fcount = 0 points =[] for tag in tags: pwb = tag.corners[2][1] - tag.corners[0][1] pwt = tag.corners[3][1] - tag.corners[1][1] pwy = (pwb+pwt)/2 pwl = tag.corners[3][1] - tag.corners[0][1] pwr = tag.corners[2][1] - tag.corners[1][1] pwx = (pwl+pwr)/2 dist = self.distanceToCamera(self.APRILWIDTH,(pwx)) #print(dist) #points.append({"type":"april","num":fcount,"x":pwx,"y":pwy,"label":tag.id,"score":dist,"time":time.time()}) points.append(["april",fcount,int(pwl + (pwx/2)),int(pwb + (pwy/2)),str(tag.tag_id)+"|"+str(dist),0,rospy.Time.now()]) fcount += 1 cv2.putText(orig, str(dist), (int(pwx), int(pwy)), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2) for idx in range(len(tag.corners)): cv2.line(orig,tuple(tag.corners[idx-1,:].astype(int)),tuple(tag.corners[idx,:].astype(int)),(0,255,0)) end3 = time.time() #print("#3:",end3-start3) return orig,points def haarDetect(self,grey,orig): start4 = time.time() faces = self.face_cascade.detectMultiScale(grey,1.3,5) fcount=0 points =[] for(x,y,w,h) in faces: #points.append({"type":"haar","num":fcount,"x":x+(w/2),"y":y+(h/2),"time":time.time()}) points.append(["haar",fcount,int(x+(w/2)),int(y+(h/2)),"",0,rospy.Time.now()]) orig = cv2.rectangle(orig,(x,y),(x+w,y+h),(255,255,0),2) roi_gray = grey[y:y+h, x:x+w] roi_color = orig[y:y+h, x:x+w] fcount += 1 eyes = self.eye_cascade.detectMultiScale(roi_gray) for (ex,ey,ew,eh) in eyes: cv2.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,255,0),2) end4 = time.time() #print("#4:",end4-start4) return orig,points def doFrame(self): frame = self.captureFrame if frame is None: return False frame = imutils.resize(frame, width=500) orig = frame.copy() grey = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) frame = Image.fromarray(frame) outframe,cpoints = self.coralDetect(frame,orig) outframe,apoints = self.aprilDetect(grey,outframe) outframe,hpoints = self.haarDetect(grey,outframe) #outframe,fpoints = self.caffeDetect(orig,outframe) points = list(itertools.chain(cpoints,apoints,hpoints)) for p in points: self.pub.publish(p[0],p[1],p[2],p[3],p[4],p[5],p[6]) pass ret, self.visionFrame = cv2.imencode('.jpg', outframe) #self.visionFrame = outframe #cv2.imshow("Frame", outframe) #key = cv2.waitKey(1) & 0xFF #if key == ord("q"): # return False return True def distanceToCamera(self,actWidth,perWidth): return ((actWidth*self.FOCALLENGTH)/perWidth)*2
class ATLocalizationNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(ATLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") # State variable for the robot self.pose = Pose2D(0.27, 0.0, np.pi) # Initial state given arbitrarily # Static transforms # Map -> Baselink. To be computed self.T_map_baselink = TransformStamped() self.T_map_baselink.header.frame_id = 'map' self.T_map_baselink.child_frame_id = 'at_baselink' # Map -> Apriltag. STATIC self._T_map_apriltag = TransformStamped() self._T_map_apriltag.header.frame_id = 'map' self._T_map_apriltag.header.stamp = rospy.Time.now() self._T_map_apriltag.child_frame_id = 'apriltag' self._T_map_apriltag.transform.translation.x = 0.0 self._T_map_apriltag.transform.translation.y = 0.0 self._T_map_apriltag.transform.translation.z = 0.09 # Height of 9 cm q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, 0.0) self._T_map_apriltag.transform.rotation.x = q[0] self._T_map_apriltag.transform.rotation.y = q[1] self._T_map_apriltag.transform.rotation.z = q[2] self._T_map_apriltag.transform.rotation.w = q[3] # Apriltag -> Camera. Computed using apriltag detection self.T_apriltag_camera = TransformStamped() self.T_apriltag_camera.header.frame_id = 'apriltag' self.T_apriltag_camera.child_frame_id = 'camera' # Camera -> Baselink. STATIC self._T_camera_baselink = TransformStamped() self._T_camera_baselink.header.frame_id = 'camera' self._T_camera_baselink.header.stamp = rospy.Time.now() self._T_camera_baselink.child_frame_id = 'at_baselink' self._T_camera_baselink.transform.translation.x = -0.0582 self._T_camera_baselink.transform.translation.y = 0.0 self._T_camera_baselink.transform.translation.z = -0.110 q = tf_conversions.transformations.quaternion_from_euler( 0.0, np.deg2rad(-15), 0.0) self._T_camera_baselink.transform.rotation.x = q[0] self._T_camera_baselink.transform.rotation.y = q[1] self._T_camera_baselink.transform.rotation.z = q[2] self._T_camera_baselink.transform.rotation.w = q[3] # Transformation Matrices to ease computation R_MA = tf_conversions.transformations.euler_matrix( 0.0, 0.0, 0.0, 'rxyz') t_MA = tf_conversions.transformations.translation_matrix( np.array([0.0, 0.0, 0.09])) self.T_MA = tf_conversions.transformations.concatenate_matrices( t_MA, R_MA) R_CB = tf_conversions.transformations.euler_matrix( 0.0, np.deg2rad(-15.0), 0.0, 'rxyz') t_CB = tf_conversions.transformations.translation_matrix( np.array([-0.0582, 0.0, -0.1072])) self.T_CB = tf_conversions.transformations.concatenate_matrices( t_CB, R_CB) # Define rotation matrices to follow axis convention in rviz when using apriltag detection self.T_ApA = tf_conversions.transformations.euler_matrix( np.pi / 2.0, 0.0, -np.pi / 2.0, 'rxyz') self.T_CCp = tf_conversions.transformations.euler_matrix( -np.pi / 2.0, 0.0, -np.pi / 2.0, 'rzyx') # Load calibration files self.calib_data = self.readYamlFile( '/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml') self.log('Loaded intrinsics calibration file') self.extrinsics = self.readYamlFile( '/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml') self.log('Loaded extrinsics calibration file') # Retrieve intrinsic info cam_info = self.setCamInfo(self.calib_data) self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(cam_info) # Initiate maps for rectification self._init_rectify_maps() # Create AprilTag detector object self.at_detector = Detector(families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # Create cv_bridge self.bridge = CvBridge() # Define subscriber to recieve images self.image_sub = rospy.Subscriber( '/' + self.veh + '/camera_node/image/compressed', CompressedImage, self.callback) # Publishers and broadcasters self.pub_robot_pose_tf = rospy.Publisher('~at_baselink_transform', TransformStamped, queue_size=1) self.static_tf_br = tf2_ros.StaticTransformBroadcaster() self.tfBroadcaster = tf.TransformBroadcaster(queue_size=1) self.log(node_name + ' initialized and running') def callback(self, ros_image): ''' MAIN TASK Convert the image from the camera_node to a cv2 image. Then, detects the apriltags position and retrieves the Rotation and translation to it from the camera frame, and next computes T_map_baselink by concatenating transformations. The resultant transformation is published, as well as all intermedium are broadcasted ''' # Convert to cv2 image image = self.readImage(ros_image) # Rectify image image = self.processImage(image) gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # Extract camera parameters K = np.array(self.cam_model.K).reshape((3, 3)) cam_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2]) # Detect apriltags detections = self.at_detector.detect(gray_image, estimate_tag_pose=True, camera_params=cam_params, tag_size=0.065) # If an apriltag is detected, update transformations for tag in detections: # Retrieve rotation and translation from apriltag to camera and homogeneize R_CA = tf_conversions.transformations.identity_matrix() R_CA[:3, :3] = tag.pose_R p = tag.pose_t.T[0] t_CA = tf_conversions.transformations.translation_matrix( np.array([p[0], p[1], p[2], 1.0])) # Define homogeneous transformation matrix T_CpAp = tf_conversions.transformations.concatenate_matrices( t_CA, R_CA) # Compute apriltag->camera transform self.T_CA = tf_conversions.transformations.concatenate_matrices( self.T_CCp, T_CpAp, self.T_ApA) self.T_AC = tf_conversions.transformations.inverse_matrix( self.T_CA) # Compute map->baselink transform self.T_MB = tf_conversions.transformations.concatenate_matrices( self.T_MA, self.T_AC, self.T_CB) # Obtain TransformStamped messages self.T_apriltag_camera.header.stamp = ros_image.header.stamp t = tf_conversions.transformations.translation_from_matrix( self.T_AC) self.T_apriltag_camera.transform.translation.x = t[0] self.T_apriltag_camera.transform.translation.y = t[1] self.T_apriltag_camera.transform.translation.z = t[2] q = tf_conversions.transformations.quaternion_from_matrix( self.T_AC) self.T_apriltag_camera.transform.rotation.x = q[0] self.T_apriltag_camera.transform.rotation.y = q[1] self.T_apriltag_camera.transform.rotation.z = q[2] self.T_apriltag_camera.transform.rotation.w = q[3] self.T_map_baselink.header.stamp = ros_image.header.stamp t = tf_conversions.transformations.translation_from_matrix( self.T_MB) self.T_map_baselink.transform.translation.x = t[0] self.T_map_baselink.transform.translation.y = t[1] self.T_map_baselink.transform.translation.z = t[2] q = tf_conversions.transformations.quaternion_from_matrix( self.T_MB) self.T_map_baselink.transform.rotation.x = q[0] self.T_map_baselink.transform.rotation.y = q[1] self.T_map_baselink.transform.rotation.z = q[2] self.T_map_baselink.transform.rotation.w = q[3] # Publish transform map -> baselink self.pub_robot_pose_tf.publish(self.T_map_baselink) # Broadcast all transforms self.static_tf_br.sendTransform(self._T_map_apriltag) self.static_tf_br.sendTransform(self._T_camera_baselink) self.tfBroadcaster.sendTransformMessage(self.T_apriltag_camera) def setCamInfo(self, calib_data): ''' Introduces the camera information from the dictionary obtained reading the yaml file to a CameraInfo object ''' cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info def readImage(self, msg_image): """ Convert images to OpenCV images Args: msg_image (:obj:`CompressedImage`) the image from the camera node Returns: OpenCV image """ try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image) return cv_image except CvBridgeError as e: self.log(e) return [] def _init_rectify_maps(self): # Get new optimal camera matrix W = self.cam_model.width H = self.cam_model.height rect_camera_K, _ = cv2.getOptimalNewCameraMatrix( self.cam_model.K, self.cam_model.D, (W, H), 1.0) # Initialize rectification maps mapx = np.ndarray(shape=(H, W, 1), dtype='float32') mapy = np.ndarray(shape=(H, W, 1), dtype='float32') mapx, mapy = cv2.initUndistortRectifyMap(self.cam_model.K, self.cam_model.D, np.eye(3), rect_camera_K, (W, H), cv2.CV_32FC1, mapx, mapy) self.cam_model.K = rect_camera_K self.mapx = mapx self.mapy = mapy def processImage(self, raw_image, interpolation=cv2.INTER_LINEAR): ''' Undistort a provided image using the calibrated camera info Implementation based on: https://github.com/duckietown/dt-core/blob/952ebf205623a2a8317fcb9b922717bd4ea43c98/packages/image_processing/include/image_processing/rectification.py Args: raw_image: A CV image to be rectified interpolation: Type of interpolation. For more accuracy, use another cv2 provided constant Return: Undistorted image ''' image_rectified = np.empty_like(raw_image) processed_image = cv2.remap(raw_image, self.mapx, self.mapy, interpolation, image_rectified) return processed_image def readYamlFile(self, fname): """ Reads the 'fname' yaml file and returns a dictionary with its input. You will find the calibration files you need in: `/data/config/calibrations/` """ with open(fname, 'r') as in_file: try: yaml_dict = yaml.load(in_file) return yaml_dict except yaml.YAMLError as exc: self.log("YAML syntax error. File: %s fname. Exc: %s" % (fname, exc), type='fatal') rospy.signal_shutdown() return def onShutdown(self): super(ATLocalizationNode, self).onShutdown()