class deviceSideProcessor():
"""
Packages the image rectification and AprilTag detection for images.
"""
def __init__(self, options):
self.ImageRectifier = None
self.opt_beautify = options.get("beautify", False)
self.aprilTagProcessor = aprilTagProcessor(options.get("tag_size", 0.065))
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
class ImageProcessor(multiprocessing.Process):
"""
Packages the image rectification and AprilTag detection for images.
"""
def __init__(self, options, logger, publishers, aprilTagProcessor,
publish_queue, config, image_queue):
super(ImageProcessor, self).__init__()
self.logger = logger
self.ImageRectifier = None
self.publish_queue = publish_queue
self.image_queue = image_queue
self.bridge = CvBridge()
self.aprilTagProcessor = aprilTagProcessor
self.publishers = publishers
self.opt_beautify = options.get('beautify', False)
self.tag_size = options.get('tag_size', 0.065)
self.ACQ_DEVICE_NAME = config['ACQ_DEVICE_NAME']
self.ACQ_TEST_STREAM = config['ACQ_TEST_STREAM']
self.raw_image = None
self.camera_info = None
self.seq_stamper = None
self.total_time = 0.0
self.min_time = 10000000000000.0
self.max_time = -1.0
self.iterations = 0.0
def run(self):
while True:
(self.raw_image, self.camera_info,
self.seq_stamper) = self.image_queue.get(block=True)
t0 = time.time()
# cv_image = cv2.imread('/home/galanton/duckietown/cslam_aruco_detector/ros/src/example_image.jpg',
# cv2.IMREAD_ANYCOLOR)
cv_image = self.bridge.compressed_imgmsg_to_cv2(
self.raw_image, 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 self.camera_info.height != currRawImage_height or self.camera_info.width != currRawImage_width:
scale_width = float(currRawImage_width) / \
self.camera_info.width
scale_height = float(currRawImage_height) / \
self.camera_info.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
outputDict = self.process(
cv_image, currRawImage_width, currRawImage_height,
(np.array(self.camera_info.K) * scale_matrix).reshape(
(3, 3)), self.camera_info.D)
outputDict['header'] = self.raw_image.header
# outputDict['header'] = rospy.Header
# Add the time stamp and source of the input image to the output
for idx in range(len(outputDict['apriltags'])):
# outputDict['apriltags'][idx]['timestamp_secs'] = 123
# outputDict['apriltags'][idx]['timestamp_nsecs'] = 42
outputDict['apriltags'][idx][
'timestamp_secs'] = self.raw_image.header.stamp.secs
outputDict['apriltags'][idx][
'timestamp_nsecs'] = self.raw_image.header.stamp.nsecs
outputDict['apriltags'][idx]['source'] = self.ACQ_DEVICE_NAME
# Generate a diagnostic image
if self.ACQ_TEST_STREAM == 1:
image = np.copy(outputDict['rect_image'])
# Put the AprilTag bound boxes and numbers to the image
for tag in outputDict['apriltags']:
for idx in range(len(tag['corners'])):
cv2.line(image,
tuple(tag['corners'][idx - 1, :].astype(int)),
tuple(tag['corners'][idx, :].astype(int)),
(0, 255, 0))
# cv2.rectangle(image, (tag['corners'][0, 0].astype(int)-10,tag['corners'][0, 1].astype(int)-10), (tag['corners'][0, 0].astype(int)+15,tag['corners'][0, 1].astype(int)+15), (0, 0, 255), cv2.FILLED)
cv2.putText(image,
str(tag['tag_id']),
org=(tag['corners'][0, 0].astype(int) + 10,
tag['corners'][0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Put device and timestamp to the image
cv2.putText(image,
'device: ' + self.ACQ_DEVICE_NAME +
', timestamp: ' +
str(self.raw_image.header.stamp.secs) + '+' +
str(self.raw_image.header.stamp.nsecs),
org=(30, 30),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Add the original and diagnostic info to the outputDict
outputDict[
'test_stream_image'] = self.bridge.cv2_to_compressed_imgmsg(
image, dst_format='png')
outputDict[
'test_stream_image'].header.stamp.secs = self.raw_image.header.stamp.secs
outputDict[
'test_stream_image'].header.stamp.nsecs = self.raw_image.header.stamp.nsecs
outputDict[
'test_stream_image'].header.frame_id = self.ACQ_DEVICE_NAME
outputDict['raw_image'] = self.raw_image
outputDict['raw_camera_info'] = self.camera_info
outputDict[
'rectified_image'] = self.bridge.cv2_to_compressed_imgmsg(
outputDict['rect_image'], dst_format='png')
outputDict[
'rectified_image'].header.stamp.secs = self.raw_image.header.stamp.secs
outputDict[
'rectified_image'].header.stamp.nsecs = self.raw_image.header.stamp.nsecs
outputDict[
'rectified_image'].header.frame_id = self.ACQ_DEVICE_NAME
t1 = time.time()
self.total_time += t1 - t0
self.iterations += 1
self.min_time = min(t1 - t0, self.min_time)
self.max_time = max(t1 - t0, self.max_time)
# print("%05d %5.4f %5.4f %5.4f %5.4f %5.4f" %
# (self.iterations, self.total_time, self.min_time, self.max_time, self.total_time / self.iterations, t1 - t0))
# PUBLISH HERE
self.publish(outputDict)
def process(self, raw_image, width, height, 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
if len(raw_image.shape) == 3:
raw_image_gray = cv2.cvtColor(raw_image, cv2.COLOR_BGR2GRAY)
else:
raw_image_gray = raw_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
tags = self.aprilTagProcessor.detect(raw_image_gray, width, height,
cameraMatrix, distCoeffs)
# 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.rvec2quat(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('ImageProcessor process failed: : %s' % str(e))
pass
def rvec2quat(self, rvec):
x = rvec[0]
y = rvec[1]
z = rvec[2]
r = math.sqrt(x * x + y * y + z * z)
if r < 0.00001:
return np.array([1, 0, 0, 0])
c = math.cos(r / 2)
s = math.sin(r / 2)
quat_x = c
quat_y = s * z / r
quat_z = -s * y / r
quat_w = -s * x / r
return np.array([quat_x, quat_y, quat_z, quat_w])
def publish(self, incomingData):
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagExtended()
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag["homography"].flatten(
)
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
self.publish_queue.put({
"topic": "apriltags",
"message": newApriltagDetectionMsg
})
# self.publishers["apriltags"].publish(newApriltagDetectionMsg)
# self.logger.info("Published pose for tag %d in sequence %d" % (
# tag["tag_id"], self.seq_stamper))
# Publish the test and raw data if submitted and requested:
if self.ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "test_stream_image",
"message": imgMsg
})
# self.publishers["test_stream_image"].publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "raw_image",
"message": imgMsg
})
# self.publishers["raw_image"].publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "rectified_image",
"message": imgMsg
})
# self.publishers["rectified_image"].publish(imgMsg)
if "raw_camera_info" in incomingData:
self.publish_queue.put({
"topic":
"raw_camera_info",
"message":
incomingData["raw_camera_info"]
})
# self.publishers["raw_camera_info"].publish(
# incomingData["raw_camera_info"])
if "new_camera_matrix" in incomingData:
new_camera_info = CameraInfo()
try:
new_camera_info.header = incomingData[
"raw_camera_info"].header
new_camera_info.height = incomingData["raw_image"].shape[0]
new_camera_info.width = incomingData["raw_image"].shape[1]
new_camera_info.distortion_model = incomingData[
"raw_camera_info"].distortion_model
new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
except:
pass
new_camera_info.K = incomingData["new_camera_matrix"].flatten()
self.publish_queue.put({
"topic": "new_camera_matrix",
"message": new_camera_info
})
class ImageProcessor(multiprocessing.Process):
"""
Packages the image rectification and AprilTag detection for images.
"""
def __init__(self, options, logger, publishers, aprilTagProcessor,
publish_queue, config, image_queue):
super(ImageProcessor, self).__init__()
self.logger = logger
self.ImageRectifier = None
self.publish_queue = publish_queue
self.image_queue = image_queue
self.bridge = CvBridge()
self.aprilTagProcessor = aprilTagProcessor
self.publishers = publishers
self.opt_beautify = options.get('beautify', False)
self.tag_size = options.get('tag_size', 0.065)
self.ACQ_DEVICE_NAME = config['ACQ_DEVICE_NAME']
self.ACQ_TEST_STREAM = config['ACQ_TEST_STREAM']
self.raw_image = None
self.camera_info = None
self.seq_stamper = None
def run(self):
while True:
(self.raw_image, self.camera_info,
self.seq_stamper) = self.image_queue.get(block=True)
cv_image = self.bridge.compressed_imgmsg_to_cv2(
self.raw_image, 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 self.camera_info.height != currRawImage_height or self.camera_info.width != currRawImage_width:
scale_width = float(currRawImage_width) / \
self.camera_info.width
scale_height = float(currRawImage_height) / \
self.camera_info.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
outputDict = self.process(cv_image, (np.array(self.camera_info.K) *
scale_matrix).reshape((3, 3)),
self.camera_info.D)
outputDict['header'] = self.raw_image.header
# Add the time stamp and source of the input image to the output
for idx in range(len(outputDict['apriltags'])):
outputDict['apriltags'][idx][
'timestamp_secs'] = self.raw_image.header.stamp.secs
outputDict['apriltags'][idx][
'timestamp_nsecs'] = self.raw_image.header.stamp.nsecs
outputDict['apriltags'][idx]['source'] = self.ACQ_DEVICE_NAME
# Generate a diagnostic image
if self.ACQ_TEST_STREAM == 1:
image = np.copy(outputDict['rect_image'])
# Put the AprilTag bound boxes and numbers to the image
for tag in outputDict['apriltags']:
for idx in range(len(tag['corners'])):
cv2.line(image,
tuple(tag['corners'][idx - 1, :].astype(int)),
tuple(tag['corners'][idx, :].astype(int)),
(0, 255, 0))
# cv2.rectangle(image, (tag['corners'][0, 0].astype(int)-10,tag['corners'][0, 1].astype(int)-10), (tag['corners'][0, 0].astype(int)+15,tag['corners'][0, 1].astype(int)+15), (0, 0, 255), cv2.FILLED)
cv2.putText(image,
str(tag['tag_id']),
org=(tag['corners'][0, 0].astype(int) + 10,
tag['corners'][0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Put device and timestamp to the image
cv2.putText(image,
'device: ' + self.ACQ_DEVICE_NAME +
', timestamp: ' +
str(self.raw_image.header.stamp.secs) + '+' +
str(self.raw_image.header.stamp.nsecs),
org=(30, 30),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Add the original and diagnostic info to the outputDict
outputDict[
'test_stream_image'] = self.bridge.cv2_to_compressed_imgmsg(
image, dst_format='png')
outputDict[
'test_stream_image'].header.stamp.secs = self.raw_image.header.stamp.secs
outputDict[
'test_stream_image'].header.stamp.nsecs = self.raw_image.header.stamp.nsecs
outputDict[
'test_stream_image'].header.frame_id = self.ACQ_DEVICE_NAME
outputDict['raw_image'] = self.raw_image
outputDict['raw_camera_info'] = self.camera_info
outputDict[
'rectified_image'] = self.bridge.cv2_to_compressed_imgmsg(
outputDict['rect_image'], dst_format='png')
outputDict[
'rectified_image'].header.stamp.secs = self.raw_image.header.stamp.secs
outputDict[
'rectified_image'].header.stamp.nsecs = self.raw_image.header.stamp.nsecs
outputDict[
'rectified_image'].header.frame_id = self.ACQ_DEVICE_NAME
# PUBLISH HERE
self.publish(outputDict)
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('ImageProcessor process failed: : %s' % str(e))
pass
def mat2quat(self, M):
"""
Helper function that converts rotation matrices to quaternions.
"""
try:
Qxx, Qyx, Qzx, Qxy, Qyy, Qzy, Qxz, Qyz, Qzz = M.flat
K = np.array([[Qxx - Qyy - Qzz, 0, 0, 0],
[Qyx + Qxy, Qyy - Qxx - Qzz, 0, 0],
[Qzx + Qxz, Qzy + Qyz, Qzz - Qxx - Qyy, 0],
[Qyz - Qzy, Qzx - Qxz, Qxy - Qyx, Qxx + Qyy + Qzz]
]) / 3.0
# Use Hermitian eigenvectors, values for speed
vals, vecs = np.linalg.eigh(K)
# Select largest eigenvector, reorder to x,y,z,w quaternion
q = vecs[[0, 1, 2, 3], np.argmax(vals)]
# Prefer quaternion with positive w
# (q * -1 corresponds to same rotation as q)
if q[3] < 0:
q *= -1
return q
except Exception as e:
self.logger.warning('ImageProcessor process failed: : %s' % str(e))
pass
def publish(self, incomingData):
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagExtended()
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag["homography"].flatten(
)
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
self.publish_queue.put({
"topic": "apriltags",
"message": newApriltagDetectionMsg
})
# self.publishers["apriltags"].publish(newApriltagDetectionMsg)
self.logger.info("Published pose for tag %d in sequence %d" %
(tag["tag_id"], self.seq_stamper))
# Publish the test and raw data if submitted and requested:
if self.ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "test_stream_image",
"message": imgMsg
})
# self.publishers["test_stream_image"].publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "raw_image",
"message": imgMsg
})
# self.publishers["raw_image"].publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "rectified_image",
"message": imgMsg
})
# self.publishers["rectified_image"].publish(imgMsg)
if "raw_camera_info" in incomingData:
self.publish_queue.put({
"topic":
"raw_camera_info",
"message":
incomingData["raw_camera_info"]
})
# self.publishers["raw_camera_info"].publish(
# incomingData["raw_camera_info"])
if "new_camera_matrix" in incomingData:
new_camera_info = CameraInfo()
try:
new_camera_info.header = incomingData[
"raw_camera_info"].header
new_camera_info.height = incomingData["raw_image"].shape[0]
new_camera_info.width = incomingData["raw_image"].shape[1]
new_camera_info.distortion_model = incomingData[
"raw_camera_info"].distortion_model
new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
except:
pass
new_camera_info.K = incomingData["new_camera_matrix"].flatten()
self.publish_queue.put({
"topic": "new_camera_matrix",
"message": new_camera_info
})
class deviceSideProcessor():
"""
Packages the image rectification and AprilTag detection for images.
"""
def __init__(self, options, logger):
self.logger = logger
self.ImageRectifier = None
self.opt_beautify = options.get('beautify', False)
self.tag_size = options.get('tag_size', 0.065)
self.aprilTagProcessor = apriltags3.Detector(searchpath=[ACQ_APRILTAG_SO],
families='tag36h11',
nthreads=4,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
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 mat2quat(self, M):
"""
Helper function that converts rotation matrices to quaternions.
"""
try:
Qxx, Qyx, Qzx, Qxy, Qyy, Qzy, Qxz, Qyz, Qzz = M.flat
K = np.array([[Qxx - Qyy - Qzz, 0, 0, 0],
[Qyx + Qxy, Qyy - Qxx - Qzz, 0, 0],
[Qzx + Qxz, Qzy + Qyz, Qzz - Qxx - Qyy, 0],
[Qyz - Qzy, Qzx - Qxz, Qxy - Qyx, Qxx + Qyy + Qzz]]) / 3.0
# Use Hermitian eigenvectors, values for speed
vals, vecs = np.linalg.eigh(K)
# Select largest eigenvector, reorder to x,y,z,w quaternion
q = vecs[[0, 1, 2, 3], np.argmax(vals)]
# Prefer quaternion with positive w
# (q * -1 corresponds to same rotation as q)
if q[3] < 0:
q *= -1
return q
except:
self.logger.warning('deviceSideProcessor process failed: : %s' % str(e))
pass