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)