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.stereo_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)
class VIO(object): def __init__(self, config, img_queue, imu_queue, gt_queue, viewer=None): self.config = config self.viewer = viewer self.img_queue = img_queue self.imu_queue = imu_queue self.feature_queue = Queue() self.gt_queue = gt_queue self.image_processor = ImageProcessor(config) self.orcvio = ORCVIO(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.gt_thread = Thread(target=self.process_gt) self.img_thread.start() self.imu_thread.start() self.vio_thread.start() self.gt_thread.start() self.first_gt_flag = False def process_gt(self): while True: x = self.gt_queue.get() if x is None: return data = x if not self.first_gt_flag: p0 = data.p self.first_gt_flag = True R = utils.to_rotation(data.q) gt_pose = utils.Isometry3d(R, data.p - p0) if self.viewer is not None: self.viewer.update_gt(gt_pose) 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.stereo_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.orcvio.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.orcvio.feature_callback(feature_msg) if result is not None and self.viewer is not None: self.viewer.update_pose(result.cam0_pose)
class VIO(object): def __init__(self, config, img_queue, imu_queue, pos_queue, viewer=None): self.config = config self.viewer = viewer self.img_queue = img_queue self.imu_queue = imu_queue self.pos_queue = pos_queue self.feature_queue = Queue() self.mutex = Lock() 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.pos_init_thread = Thread(target=self.position_init_callback) self.pos_upda_thread = Thread(target=self.process_position_update) self.img_thread.start() self.imu_thread.start() self.vio_thread.start() self.pos_init_thread.start() self.pos_upda_thread.start() def process_img(self): while True: if not self.msckf.isready(): continue img_msg = self.img_queue.get() if img_msg is None: self.feature_queue.put(None) continue # viewer if self.viewer is not None: self.viewer.update_image(img_msg.cam0_image) feature_msg = self.image_processor.stereo_callback(img_msg) if feature_msg is not None: self.feature_queue.put(feature_msg) def process_imu(self): while True: if self.msckf.is_block: continue imu_msg = self.imu_queue.get() if imu_msg is None: continue self.image_processor.imu_callback(imu_msg) self.msckf.imu_callback(imu_msg) def process_feature(self): while True: if not self.msckf.isready(): continue if self.msckf.is_block: continue feature_msg = self.feature_queue.get() if feature_msg is None: continue # print('feature_msg', feature_msg.timestamp) self.mutex.acquire() if self.config.monocam: result = self.msckf.feature_callback_mono(feature_msg) else: result = self.msckf.feature_callback(feature_msg) self.mutex.release() # viewer if result is not None and self.viewer is not None: self.viewer.update_pose(result.cam0_pose) def position_init_callback(self): while True: if self.msckf.is_block: continue pos_msg = self.pos_queue.get() if pos_msg is None: continue # print('pos_msg', pos_msg.timestamp) self.mutex.acquire() self.msckf.position_init_callback(pos_msg) self.mutex.release() def process_position_update(self): nhz = 100 count = 0 while True: if self.msckf.is_block: continue count += 1 if count < nhz: continue count = 0 # print('pos_msg', pos_msg.timestamp) self.mutex.acquire() self.msckf.position_update_callback() self.mutex.release()