class VIO(object):
def __init__(self, config, img_queue, imu_queue):
self.config = config
self.img_queue = img_queue
self.imu_queue = imu_queue
self.image_processor = ImageProcessor(config)
self.msckf = MSCKF(config)
self.img_thread = Thread(target=self.process_img)
self.imu_thread = Thread(target=self.process_imu)
self.img_thread.start()
self.imu_thread.start()
def process_img(self):
while True:
img_msg = self.img_queue.get()
if img_msg is None:
return
cv2.imshow('cam0', img_msg.cam0_image)
cv2.waitKey(1)
feature_msg = self.image_processor.stareo_callback(img_msg)
self.msckf.feature_callback(feature_msg)
def process_imu(self):
while True:
imu_msg = self.imu_queue.get()
if imu_msg is None:
return
self.image_processor.imu_callback(imu_msg)
self.msckf.imu_callback(imu_msg)
class VIO(object):
def __init__(self, config, img_queue, imu_queue, viewer=None):
self.config = config
self.viewer = viewer
self.img_queue = img_queue
self.imu_queue = imu_queue
self.feature_queue = Queue()
self.image_processor = ImageProcessor(config)
self.msckf = MSCKF(config)
self.img_thread = Thread(target=self.process_img)
self.imu_thread = Thread(target=self.process_imu)
self.vio_thread = Thread(target=self.process_feature)
self.img_thread.start()
self.imu_thread.start()
self.vio_thread.start()
def process_img(self):
while True:
img_msg = self.img_queue.get()
if img_msg is None:
self.feature_queue.put(None)
return
# print('img_msg', img_msg.timestamp)
if self.viewer is not None:
self.viewer.update_image(img_msg.cam0_image)
feature_msg = self.image_processor.stareo_callback(img_msg)
if feature_msg is not None:
self.feature_queue.put(feature_msg)
def process_imu(self):
while True:
imu_msg = self.imu_queue.get()
if imu_msg is None:
return
# print('imu_msg', imu_msg.timestamp)
self.image_processor.imu_callback(imu_msg)
self.msckf.imu_callback(imu_msg)
def process_feature(self):
while True:
feature_msg = self.feature_queue.get()
if feature_msg is None:
return
print('feature_msg', feature_msg.timestamp)
result = self.msckf.feature_callback(feature_msg)
if result is not None and self.viewer is not None:
self.viewer.update_pose(result.cam0_pose)