def process(self, raw_image, cameraMatrix, distCoeffs):
"""
Processes an image.
"""
try:
# 0. Initialize the image rectifier if it hasn't been already (that's done so that we don't recompute the remapping)
if self.ImageRectifier is None:
self.ImageRectifier = ImageRectifier(
raw_image, cameraMatrix, distCoeffs)
# 1. Rectify the raw image and get the new camera cameraMatrix
rect_image, newCameraMatrix = self.ImageRectifier.rectify(
raw_image)
# 2. Extract april tags data
if len(rect_image.shape) == 3:
rect_image_gray = cv2.cvtColor(rect_image, cv2.COLOR_BGR2GRAY)
else:
rect_image_gray = rect_image
# Beautify if wanted:
if self.opt_beautify and self.ImageRectifier:
raw_image = self.ImageRectifier.beautify(raw_image)
# 3. Extract poses from april tags data
camera_params = (
newCameraMatrix[0, 0], newCameraMatrix[1, 1], newCameraMatrix[0, 2], newCameraMatrix[1, 2])
tags = self.aprilTagProcessor.detect(
rect_image_gray, True, camera_params, self.tag_size)
# 4. Package output
outputDict = dict()
outputDict['rect_image'] = rect_image
outputDict['new_camera_matrix'] = newCameraMatrix
outputDict['apriltags'] = list()
for atag in tags:
outputDict['apriltags'].append({'tag_id': atag.tag_id,
'corners': atag.corners,
'qvec': self.mat2quat(atag.pose_R),
'tvec': atag.pose_t,
'tag_family': atag.tag_family,
'hamming': atag.hamming,
'decision_margin': atag.decision_margin,
'homography': atag.homography,
'center': atag.center,
'pose_error': atag.pose_err})
return outputDict
except Exception as e:
self.logger.warning(
'deviceSideProcessor process failed: : %s' % str(e))
pass
def camera_image_process(self, currRawImage, currCameraInfo):
"""
Contains the necessary camera image processing.
"""
# Convert from ROS image message to numpy array
cv_image = self.bridge.compressed_imgmsg_to_cv2(
currRawImage, desired_encoding='mono8')
# Scale the K matrix if the image resolution is not the same as in the calibration
currRawImage_height = cv_image.shape[0]
currRawImage_width = cv_image.shape[1]
scale_matrix = np.ones(9)
if currCameraInfo.height != currRawImage_height or currCameraInfo.width != currRawImage_width:
scale_width = float(currRawImage_width) / currCameraInfo.width
scale_height = float(currRawImage_height) / currCameraInfo.height
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[4] *= scale_height
scale_matrix[5] *= scale_height
outputDict = dict()
# Process the image and extract the apriltags
cameraMatrix = (np.array(currCameraInfo.K) * scale_matrix).reshape(
(3, 3))
if self.ImageRectifier is None:
self.ImageRectifier = ImageRectifier(cv_image, cameraMatrix,
currCameraInfo.D)
# Rectify the raw image and get the new camera cameraMatrix
rect_image, newCameraMatrix = self.ImageRectifier.rectify(cv_image)
# Package output
outputDict['rect_image'] = rect_image
outputDict['new_camera_matrix'] = newCameraMatrix
outputDict['header'] = currRawImage.header
self.logger.info("end of camera process")
return outputDict
def process(self, raw_image, cameraMatrix, distCoeffs):
# 0. Initialize the image rectifier if it hasn't been already (that's done so that we don't recompute the remapping)
if self.ImageRectifier is None:
self.ImageRectifier = ImageRectifier(raw_image, cameraMatrix, distCoeffs)
# 1. Rectify the raw image and get the new camera cameraMatrix
rect_image, newCameraMatrix = self.ImageRectifier.rectify(raw_image)
# 2. Extract april tags data
if len(rect_image.shape) == 3:
tect_image_gray = cv2.cvtColor(rect_image, cv2.COLOR_BGR2GRAY)
else:
tect_image_gray = rect_image
# Beautify if wanted:
if self.opt_beautify and self.ImageRectifier:
raw_image = self.ImageRectifier.beautify(raw_image)
aprilTags = self.aprilTagProcessor.configureDetector(newCameraMatrix).detect(tect_image_gray, return_image=False)
# 3. Extract poses from april tags data
aprilTags = self.aprilTagProcessor.resultsToPose(aprilTags)
# 4. Package output
outputDict = dict()
outputDict["rect_image"] = rect_image
outputDict["new_camera_matrix"] = newCameraMatrix
outputDict["apriltags"] = list()
for atag in aprilTags:
outputDict["apriltags"].append({'tag_id': atag.tag_id,
'goodness': atag.goodness,
'corners': atag.corners,
'qvec': atag.qvec,
'tvec': atag.tvec })
return outputDict