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 __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 __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 __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)
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 __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 __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')
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 __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')
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 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("/")
# 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_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()
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)))
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.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.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.
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()
#
# 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']
