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()