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("/") # 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 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 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 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 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 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') # # 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()