parser = argparse.ArgumentParser() parser.add_argument('--no-viz', action='store_true', help='do not visualize') parser.add_argument('--dataset', type=str, help='dataset (KITTI/EuRoC)', default='KITTI') parser.add_argument('--path', type=str, help='dataset path', default='path/to/your/KITTI_odometry/sequences/00') parser.add_argument('--output', type=str, help='storing output values') args = parser.parse_args() if args.dataset.lower() == 'kitti': params = ParamsKITTI() dataset = KITTIOdometry(args.path) elif args.dataset.lower() == 'euroc': params = ParamsEuroc() dataset = EuRoCDataset(args.path) sptam = SPTAM(params) visualize = not args.no_viz if visualize: from viewer import MapViewer viewer = MapViewer(sptam, params) cam = Camera( dataset.cam.fx, dataset.cam.fy, dataset.cam.cx, dataset.cam.cy, dataset.cam.width, dataset.cam.height, params.frustum_near, params.frustum_far, dataset.cam.baseline)
if __name__ == '__main__': from queue import Queue from threading import Thread from config import ConfigEuRoC from dataset import EuRoCDataset, DataPublisher img_queue = Queue() imu_queue = Queue() config = ConfigEuRoC() image_processor = ImageProcessor(config) path = 'path/to/your/EuRoC_MAV_dataset/MH_01_easy' dataset = EuRoCDataset(path) dataset.set_starttime(offset=0.) duration = 3. ratio = 0.5 imu_publisher = DataPublisher(dataset.imu, imu_queue, duration, ratio) img_publisher = DataPublisher(dataset.stereo, img_queue, duration, ratio) now = time.time() imu_publisher.start(now) img_publisher.start(now) def process_imu(in_queue): while True: msg = in_queue.get() if msg is None:
self.viewer.update_pose(result.cam0_pose) if __name__ == '__main__': import time from dataset import EuRoCDataset, DataPublisher from viewer import Viewer dataset_path = "/media/erl/disk2/euroc/MH_01_easy" viewer = Viewer() dataset = EuRoCDataset(dataset_path) offset = 40 dataset.set_starttime(offset) # start from static state img_queue = Queue() imu_queue = Queue() gt_queue = Queue() config = ConfigEuRoC() Orc_VIO = VIO(config, img_queue, imu_queue, gt_queue, viewer=viewer) duration = float('inf') # make it smaller if image processing and OrcVIO computation is slow if not config.load_features_flag: ratio = 1.0 else:
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) if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--path', type=str, default='/media/james/MX_NTFS/dataset/EuRoc/MH_01_easy', help='Path of EuRoC') parser.add_argument('--view', action='store_true', help='Show trajectory.') args = parser.parse_args() dataset = EuRoCDataset(args.path) dataset.set_starttime(offset=40.) # start from static state img_queue = Queue() imu_queue = Queue() config = ConfigEuRoC() msckf_vio = VIO(config, img_queue, imu_queue) duration = float('inf') ratio = 0.4 # make it smaller if image processing and MSCKF computation is slow imu_publisher = DataPublisher(dataset.imu, imu_queue, duration, ratio) img_publisher = DataPublisher(dataset.stereo, img_queue, duration, ratio) now = time.time() imu_publisher.start(now)
return None, None T = g2o.Isometry3d(cv2.Rodrigues(rvec)[0], tvec) return T, inliers.ravel() if __name__ == '__main__': import time from config import VOConfig, IMUConfig from dataset import EuRoCDataset, DataPublisher path = 'path/to/your/EuRoC_MAV_dataset/V1_01_easy/' dataset = EuRoCDataset(path) vo = StereoVO(dataset.cam, VOConfig(), True) img_queue = Queue() duration = 20 img_publisher = DataPublisher( dataset.stereo, dataset.starttime, img_queue, duration) now = time.time() img_publisher.start(now) timestamps = [] while True: x = img_queue.get() if x is None:
parser.add_argument('--view', action='store_true', help='Show trajectory.') args = parser.parse_args() if args.view: viewer = Viewer() else: viewer = None img_queue = Queue() imu_queue = Queue() pos_queue = Queue() gt_queue = Queue() config = ConfigEuRoC() msckf_vio = VIO(config, img_queue, imu_queue, pos_queue, viewer=viewer) dataset = EuRoCDataset(args.path) if not config.init_pose: # start from static state dataset.set_starttime(offset=40.0) else: # start from first record dataset.set_starttime(offset=0.0) duration = float('inf') dataset.pos.readall(msckf_vio.msckf.all_position_data) # make it smaller if image processing and MSCKF computation is slow ratio = 0.4 imu_publisher = DataPublisher(dataset.imu, imu_queue, duration, ratio)