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)